論文 / 著書情報
Article / Book Information
題目(和文)

時相論理を用いたタスク表現に対するロボットシステムの軌道最適化と制御

Title(English)

Trajectory Optimization and Control of Robot Systems with Temporal
Logic Tasks

著者(和文)

徳田俊平

Author(English)

Shumpei Tokuda

出典(和文)

学位:博士(工学),学位授与機関:東京工業大学,
報告番号:甲第12692号,
授与年月日:2024年3月26日,
学位の種別:課程博士,
審査員:山北 昌毅,三平 満司,倉林 大輔,田中 正行,畑中 健志

Citation(English)

Degree:Doctor (Engineering),
Conferring organization: Tokyo Institute of Technology,
Report number:甲第12692号,
Conferred date:2024/3/26,
Degree Type:Course doctor,
Examiner:,,,,

学位種別(和文)

博士論文

Type(English)

Doctoral Thesis

Powered by T2R2 (Science Tokyo Research Repository)

L

Tokyo Institute of Technology

令和 5 年度 博士論文
時相論理を用いたタスク表現に対するロボットシステムの軌道最適化と制御

Trajectory Optimization and Controlof Robot Systems with Temporal Logic Tasks
2024 年 3 月

東京工業大学工学院 システム制御系
德田 俊平
指導教員 山北 昌毅 准教授

Abstract
In recent years, robots play increasingly important roles in society, not only in the industrial
ﬁeld but also in the service industry. With the spread of robots, opportunities for nonprofessionals are increasing to operate robots and introduce them to services. However,teaching robots to perform desired tasks safely is diﬃcult, and this is one of the obstacles tothe introduction of robots. One of the reasons for the diﬃculty in teaching tasks to robots
is the need to simultaneously consider the logical consistency of the task and its feasibility
considering the dynamic characteristics of the robot. Therefore, we need to use a control
architecture that considers simultaneously both.
The use of modal logic such as linear temporal logic (LTL) and signal temporal logic (STL)
has attracted much attention as a way of logically representing tasks in robot systems. These
modal logics represent the properties of sequences of state transitions. By describing the rules
and constraints of a task as logical expressions, we can determine in advance whether theassumed task is logically sound or not. When generating a sequence of state transitions
satisfying the temporal logic on a robot system, we need to generate robot trajectories onan abstracted system, rather than on the actual robot system.
In this case, depending on the state and the deﬁnition of the approximated system, theextent to which the abstracted system can emulate the characteristics of the actual robot
system diﬀers. In addition, the method of generating robot trajectories diﬀers depending
on how the abstracted system is constructed. Therefore, the nonlinearity of the abstracted
system and how it is constructed are important depending on the actual robot systems
or tasks. Especially, when the trajectory optimization is formulated as a mixed integer
programming problem, it is necessary to construct a dynamical linear system that emulates
the dynamical characteristics of the actual robot as an approximated system.
To achieve tasks, we need to control the actual system based on trajectories generated
by the abstracted system. Control methods for satisfying tasks expressed as temporal logic
speciﬁcations include model predictive control (MPC) and control barrier functions (CBFs).
CBFs are used as constraints in determining control inputs to keep the system trajectory
within a certain region, and the constrained optimization problem with CBFs can be formulated as a quadratic programming problem, which requires less computation time than
the nonlinear MPC. In designing a controller to achieve tasks represented by temporal logic
speciﬁcations using CBFs, the issue is how to set up the CBFs. In the case of a task with atime limit expressed in STL, the design method of CBFs proposed in the previous studies requires several processes to design the CBF from the robustness functions of STL. Therefore,
we propose a new design method for CBFs. Furthermore, there are cases in which robots

cannot achieve a given task with the task plans. In this case, if there are multiple robots,
the robots need to change their motions to achieve the task considering the other robots.
In this paper, we propose trajectory optimization methods and control methods for robot
systems for tasks expressed as temporal logic speciﬁcations. We focus on the task represented
by syntactically co-safe LTL, generate trajectories in response to their tasks, and control
robots. In this paper, we assume that the control architecture is divided into multiple levels.
The high-level controller deﬁnes an abstracted robot system for the actual robot system.
We generate trajectories and motions for this abstracted system to achieve tasks expressedby LTL or STL speciﬁcations by optimization. Then, the extent to which the abstracted
system can emulate the actual robot depends on the abstraction method of the abstracted
system. Therefore, we discuss abstraction methods for robot systems. In addition, we showthat we can compute trajectories that satisfy the given temporal logic speciﬁcations for theabstracted system. Furthermore, we propose a trajectory optimization method for tasks
involving external events represented by event-based STL speciﬁcations.
The low-level controller uses the task plans and trajectories generated by the high-level
controller to control the actual robot systems. In this paper, we propose a control methodthat considers time constraints to satisfy a given temporal logic speciﬁcations using CBFs. In
addition, we also consider methods to represent trajectories as distributions, and to update
the distributions based on the observation information obtained during the operation tochange the motions.

目次
第 1 章 序論
1.1 本論文の背景 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1.1 時相論理を用いたタスク表現 . . . . . . . . . . . . . . . . . . . . .
1.1.2 時相論理を用いた行動計画手法 . . . . . . . . . . . . . . . . . . . .
1.1.3 タスク達成を考慮した制御手法 . . . . . . . . . . . . . . . . . . . .
1.2 本論文の目的 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.3 論文構成 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1

第 2 章 事前知識
2.1 線形時相論理 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.1.1 クリプキモデル . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.1.2 線形時相論理 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 ロボットシステム上での時相論理 . . . . . . . . . . . . . . . . . . . . . .
2.3 信号時相論理 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

第 3 章 混合整数計画問題による軌道最適化
3.1 はじめに . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2 準備 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2.1 混合論理動的モデル . . . . . . . . . . . . . . . . . . . . . . . . . .
ピッキングモデルのハイブリッドシステム表現 . . . . . . . . . .
ピッキングのモデル化 . . . . . . . . . . . . . . . . . . . . . . . . .
3.2.2 混合整数凸計画 . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2.3 拡大線形化 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.3 混合整数計画問題による定式化 . . . . . . . . . . . . . . . . . . . . . . . .
3.3.1 線形時相論理命題の線形制約 . . . . . . . . . . . . . . . . . . . . .
3.3.2 付加関数との対応関係 . . . . . . . . . . . . . . . . . . . . . . . . .
3.3.3 混合整数線形 (二次) 計画問題による定式化 . . . . . . . . . . . . .
3.4 クォータニオンベースの SO(3) の MICP を用いた行動計画 . . . . . . . .
3.4.1 オイラーパラメーターの近似 . . . . . . . . . . . . . . . . . . . . .
3.4.2 姿勢を考慮した MLD システム . . . . . . . . . . . . . . . . . . . .
3.4.3 2 次元モデルの姿勢近似 . . . . . . . . . . . . . . . . . . . . . . . .
3.5 拡大線形システム, 拡大双線形システムを用いた行動計画 . . . . . . . .
3.5.1 拡大線形システムのための MLD システム . . . . . . . . . . . . .

11

i

1
1
1
2
3
4

7
7
7
8
9

11
11
11
12
12
15
16
18
18
19
19
20
20
22
22
22
23

拡大双線形システムのための MLD システム . . . . . . . . . . . .
3.5.3 拡大システム上での状態のジャンプ . . . . . . . . . . . . . . . . .
3.6 SO(3) に対する数値実験 . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.6.1 3 次元のピックアンドプレイスタスク . . . . . . . . . . . . . . . .
3.6.2 ドローンモデルにおけるプランニング . . . . . . . . . . . . . . . .
シミュレーターでの挙動との比較 . . . . . . . . . . . . . . . . . .
3.7 拡大システムに対する数値実験 . . . . . . . . . . . . . . . . . . . . . . . .
3.7.1 2 次元平面上でのピックアンドプレイスタスク . . . . . . . . . . .
開ループ系での評価 . . . . . . . . . . . . . . . . . . . . . . . . . .
閉ループ系での評価 . . . . . . . . . . . . . . . . . . . . . . . . . .
3.8 まとめ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

23

第 4 章 不確かな外的事象に関する命題への軌道最適化
4.1 はじめに . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 Event-based STL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.3 Event-based STL のロバストネス関数 . . . . . . . . . . . . . . . . . . . . .
4.4 不確かさを持つ外的事象に対する Event-based STL 命題のための行動計
画手法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5 数値実験 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5.1 1 次元の通過タスク . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5.2 2 次元の下げ膳タスク . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5.3 提案手法の十分性 . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.6 まとめ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

37

第 5 章 時間制約をもつ信号時相論理に対する制御
5.1 はじめに . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2 準備 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.1 対象システム . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.2 時間オートマトン . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.3 零化制御バリア関数 . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.4 時間軸変換 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3 仮想時間を用いた ϵ-零化制御バリア関数 (ϵ-ZCBFs) . . . . . . . . . . . .
5.3.1 ϵ-零化制御バリア関数 . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3.2 関数値に基づく時間軸変換 . . . . . . . . . . . . . . . . . . . . . .
5.3.3 ϵ-ZCBF への仮想時間の適用 . . . . . . . . . . . . . . . . . . . . . .
5.3.4 STL 命題への提案手法の適用 . . . . . . . . . . . . . . . . . . . . .

49

3.5.2

ii

24
25
26
28
29
33
33
34
34
35

37
37
38
39
40
40
44
45
48

49
49
49
49
50
51
51
53
54
57
58

数値実験 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.4.1 問題設定 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.4.2 結果・考察 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.5 まとめ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

59

第 6 章 信号時相論理に対する確率的動作プリミティブの再配合
6.1 はじめに . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.2 確率的動作プリミティブ . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.2.1 確率的動作プリミティブの配合 . . . . . . . . . . . . . . . . . . . .
6.3 無香料カルマンフィルタ . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.3.1 アルゴリズム . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
準備 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
シグマポイントの生成 . . . . . . . . . . . . . . . . . . . . . . . . .
シグマポイントの更新 . . . . . . . . . . . . . . . . . . . . . . . . .
状態の推定 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.4 観測情報に基づく確率的動作プリミティブの推定 . . . . . . . . . . . . .
6.4.1 配合された確率的動作プリミティブに対する無香料カルマンフィ
ルタ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.4.2 推定された確率的動作プリミティブを用いた判別 . . . . . . . . .
6.5 観測情報による確率的動作プリミティブの再配合 . . . . . . . . . . . . .
6.6 数値実験 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.6.1 問題設定 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.6.2 結果・考察 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.7 まとめ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

63

第7章

75

5.4

結論

59
60
62

63
63
64
64
65
65
65
66
66
67
67
68
68
69
69
70
70

参考文献

77

研究業績

83

謝辞

85

iii

図目次
1.1

Control architecture in this paper . . . . . . . . . . . . . . . . . . . . . . . .

3.1

Modeling of picking. Top: the robot behavior in the real world. Bottom:

4

the behavior of MLD model in the simulated world. xarm,1 and xobj,1 are theposition of the arm 1 and the object 1, respectively. H11 is the bounding boxof the object and proposition φϑ11 is xarm,1 ∈ H11 . When the arm enters
the bounding box H11 , φϑ11 becomes 1. We can change ϖ11 to 1 and when
ϖ11 = 1, the velocity constraint of the object is activated, and it follows
the arm. This is the modeling of picking. Since the proposition ϖ11 = 1 isa suﬃcient condition of (3.8), the arm can release objects after picking theobject.

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.2

An approximation of the quadratic term xy by MICP

. . . . . . . . . . . .

3.3

Setting of the object and box. xobj1 exists at the center of the picking polytope.
The arm needs to lay the object down to put it in the box.

3.4

. . . . . . . . .

14
16
27

Behavior in semi-optimal solutions. Upper: a situation in which the object islying down. Lower: a situation in which the object is upright. The blue linewith circles is the trajectory of the arm. In each situation, the arm completes
the pick-and-place task by laying the object down.

3.5. . . . . . . . . . . . . .

27

Overview of reaching task of the drone. Left: initial states and workspace.
The yellow rectangle represents initial states of the drone. The green, blue
and red areas are the initial, slit, and goal areas, respectively. Right: thetrajectory of a semi-optimal solution. The drone passes through the slit areawith the body in a vertical position.

3.6. . . . . . . . . . . . . . . . . . . . . .

30

Left: Comparison of the behavior of a solution and simulator. The green
rectangle represents the simulation of the drone to which inputs obtainedby the Mixed-Integer Linear Problem planning are applied. Right: Input
sequences used in the simulator. We applied the First-order hold to the input
obtained by the Mixed-Integer Linear Problem planning. . . . . . . . . . . .

3.7

Comparison of the Euler parameter states of the solution and simulator. Solid:
the solution. Dashed: simulator.

3.8. . . . . . . . . . . . . . . . . . . . . . . .

31

Comparison of position states of the solution and simulator. Solid: the solution. Dashed: simulator.

3.9

30

. . . . . . . . . . . . . . . . . . . . . . . . . . . .

31

Comparison of velocity states of the solution and simulator. Solid: the solution. Dashed: simulator.

. . . . . . . . . . . . . . . . . . . . . . . . . . . .

iv

32

3.10 The trajectory of l2 -norm of the Euler parameter states of simulation androtation angles of diﬀerence of rotation matrics: ∆θa and ∆θ̄a . . . . . . . .

32

3.11 Left: RMSE between the trajectories of x and y of the planning, the openloop system, and the closed-loop system. Right: The trajectories of planning,open-loop, and closed-loop systems. The yellow trajectory is a trajectoryof the planning obtained in a case of 8 intervals. The blue trajectory is a
trajectory of the open-loop system. The pink trajectory is a trajectory of theclosed-loop system. In this case, we see that the trajectories of both open-loop
and closed-loop systems could not complete the task. . . . . . . . . . . . . .

35

4.1

The case of the passage task for the 1-dimensional robot . . . . . . . . . . .

41

4.2

The trajectories of the passage task in case I).

. . . . . . . . . . . . . . . .

42

4.3

The trajectories of the passage task in case II).

. . . . . . . . . . . . . . . .

42

4.4

The trajectories of the passage task in case III). . . . . . . . . . . . . . . . .

43

4.5

The trajectories of the service task in 2-dimensional space for each cases.
Green: case i). Orange: case ii).

4.6

. . . . . . . . . . . . . . . . . . . . . . . .

The trajectories of the service task in case i). We can see that the robots
visited the table A, the table B, and the Sink area in that order.

4.7

46

. . . . . .

47

The trajectories of the service task in case ii). We can see that the robots
visited the table A, the Sink, the table B area in that order. . . . . . . . . .

47

5.1

An example of a timed automaton for STL

50

5.2

Overview of proposed method in Chapter 5. First, we consider a timed au-. . . . . . . . . . . . . . . . . .

tomaton of given STL task. We assume that we can obtain an accepting run(green run) of the timed automaton. We convert the accepting run to a new
STL speciﬁcation (blue underline) which has STL propositions (pink underline) corresponding to each transition between locations of the accepting run(orange enclosed parts). Then, we set ϵ-ZCBFs and the virtual time from the
new STL speciﬁcation. The proposed controller controls a system and ﬁnallyachieve given STL task.

. . . . . . . . . . . . . . . . . . . . . . . . . . . . .

52

5.3

Model of an adaptive cruise control . . . . . . . . . . . . . . . . . . . . . . .

59

5.4

Three behaviors of car in front: 1) fast, 2) slow at ﬁrst, and 3) fast at ﬁrst
and last.

5.5. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

61

The Trajectories of robustness functions of ψA and ϕB with and without
virtual time in behavior 3. We can see that ψA is a positive value at time Tand ϕB is always positive. . . . . . . . . . . . . . . . . . . . . . . . . . . . .

v

62

6.1

Workspace of the two mobile robots. . . . . . . . . . . . . . . . . . . . . .

6.2

Behaviors of ProMPs in the numerical simulation. Black circles are states

71

of Robot 2, magenta circles are current states of ProMPs, magenta trajectories are trajectories of ProMP of robot 1, perple and green trajectories aretrajectories of estimated ProMP of robot 2: when d1P ro < 4, trajectories aregreen, and brown trajectoies are trajectories used in optimization problem for
ProMP of robot 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.3

72

The trajectories of outliers for ProMPs of Motion 2.a, Motion 2.b and esatimated one.

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

73

6.4

The trajectory of the blending parameter of ProMP of robot 1: γ11. . . . .

73

6.5

The trajectory of the blending parameter of ProMP of robot 2: γ12. . . . .

74

6.6

The trajectory of the time parameter of ProMP of robot 2: τ

. . . . . . . .

74

表目次
4.1

5.1

Comparison of trajectories of the proposed method and the optimization withﬁxed occurrence time . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

45

Solved parameters ratio in the simulations . . . . . . . . . . . . . . . . . . .

61

vi

第1章
1.1

序論

本論文の背景近年, ロボットの社会における役割は大きくなっており, 工業的な現場のみならずサービス業などの現場で活躍している [1]. ロボットの波及に伴い, 非専門家がロボットを操作し現場に導入する機会が増加している. しかし, ロボットに安全性を考慮して望みのタスクを実行するように教示することは難しく, そのことがロボットの導入に対する障害の一つとなっている. ロボットにタスクを教示する際の難しさの理由の一つとして, タスクの論理的な整合性とロボットの動的特性を考慮した実現性を同時に考慮する必要があることが挙げられる.
そのため, それらを同時に考慮したロボットの制御アーキテクチャを用いることが望ましいと考えられる.

1.1.1

時相論理を用いたタスク表現

ロボットシステムのタスクを論理的に表現する方法として, Linear Temporal Logic (線形時
相論理, LTL) や Signal Temporal Logic (信号時相論理, STL) といった様相論理を用いることが注目されている [2, 3]. これらの様相論理は状態遷移列の性質を表現する論理である [4].
タスクに関するルールや制約条件を論理式として記述することで, 想定しているタスクに論
理的な破綻がないかどうかを事前に確認することが可能である. STL は LTL を拡張した時相論理である [5]. STL を状態遷移モデルに対して定義する際, STL の各命題に対して状態を引数とした関数であるロバストネス関数を導入する. このとき, 各 STL 命題の真偽はロバストネス関数の値によって評価することができる. 加えて, STL はタスクに対する制限時間といった定量的な時間特性を表現することも可能である. 加えて, ロボットの状態に関係なく発生するような外的事象に依存したタスクを表現するための時相論理である Event-based
STL が提案されている [6]. Event-base STL を用いることで, ロボットが人間が発信する信号に反応して対応するようなタスクを記述することも可能である.

1.1.2

時相論理を用いた行動計画手法

LTL や STL を用いて論理的な整合性を考慮して定義されたタスクに対してロボットの行動計画を行う手法として様々な手法が提案されている [7–11]. ロボットシステム上で時相論理を満たす状態遷移列を生成する際, 実際に制御を行うロボットシステムではなく, 抽象1

第 1 章 序論

2化したシステム上でロボットの軌道を生成する. 抽象化したシステムとは, 実際に制御を行うロボットシステムの特性を反映したシステムである. 抽象化システムの構築方法として,
Simulation, Bisimulation や マルコフ決定過程としてシステムを表現する手法が提案されている [7, 8, 12]. これらの手法の特徴として, 実際のロボットシステムと抽象化したシステムの状態間での関係を定義することで, 抽象化したシステム上での軌道から対応した実際のシ
ステムの軌道を生成することが可能である. しかし, 実際のロボットシステムは非線形なダ
イナミクスを持つため, 抽象化システムとの状態間の関係を定義し, これらの手法を用いて抽象化したシステムを構築することは難しい. そのため, 手先座標といったタスクに関連するロボットの状態に対して, 適当な近似システムを定義し抽象化システムとする手法も検討されている [13, 14]. 本論文でも, これらの手法と同様に近似システムを抽象化システムとして用いている. この際, 抽象化したシステムの状態や近似システムの定義によって, 実際のロボットシステムの特性をどの程度反映可能なのかが異なっている.
加えて, 抽象化システムの構築方法によってロボットの軌道を生成する手法も異なる. 抽
象化したシステムを離散状態システムとして表現した場合, 与えられた時相論理命題に対応するオートマトンとの並行システムを考えることで, 価値反復法など強化学習の手法によってタスクを達成する状態遷移列を生成することができる [8, 15, 16]. しかし, 離散状態システ
ムとして表現する場合, Simulation, Bisimulation や マルコフ決定過程等を用いて表現することになり, 上記で述べた問題に加えて, 並行システムの状態集合が大規模になるといった問題がある.
連続状態システムとして表現した場合, 軌道最適化問題として定式化し, 混合整数計画問
題や非線形計画問題を解くことで軌道を生成することができる [9, 10]. 混合整数計画問題を用いて軌道を生成する場合, 原子論理命題の真偽を離散 (0-1) 変数を用いて表現し, 各時相論
理命題を 0-1 変数の時系列に対する四則演算を用いて表現することができる [9]. 加えて, 抽
象化したシステムとしてハイブリッドシステムである混合論理動的 (MLD) モデルが用いられている場合にも, タスクを達成する軌道を生成することが可能である. しかし, 混合整数計
画問題として定式化する場合, 非線形制約が存在したり非線形システムに適用する際に計算コストが大きいといった問題がある. 一方で, 非線形計画問題を用いて軌道を生成する場合,
原子論理命題の真偽をロバストネス関数用いて表現し, 各時相論理命題のロバストネス関数
を softmin, softmax 関数を用いて連続な関数として表現することができる [3]. 加えて, 非線形システムに対して混合整数計画問題を用いる場合と比較して, 計算時間が短いといった利点がある. そのため, 対象とするロボットシステムやタスクによって抽象化したシステムの非線形性やどのクラスの問題として定式化するかが重要であり, 特に, 混合整数計画問題として定式化する場合は, 近似システムとして実際のロボットの動的特性を反映した動的線形システムを構築することが必要であり, 近似システムの構築方法には検討の余地が存在する.

1.1.3

タスク達成を考慮した制御手法

タスクを達成するためには, 抽象化したシステムで生成した軌道を基に実際のシステム上で制御を行い, 行動を実行する必要がある. 時相論理命題として表現されたタスクを達成する

1.2. 本論文の目的

3ための制御手法として, Model Predictive Control (モデル予測制御, MPC) [17,18] や Control
Barrier Functions (制御バリア関数, CBFs) [19–22] を用いた制御器が提案されている. 特に,CBF はシステムの軌道を特定の領域内に保つための制御入力を決定する際の制約条件として用いられ [23], CBF を用いた制約付き最適化問題は二次計画問題として定式化することができ, これは非線形 MPC と比較して計算時間が短い.
CBF を用いて時相論理によって表現されたタスクを実行するための制御器を設計するうえで, CBF をどのように設定するかが課題となる. 特に, STL で表現された制限時間があるよ
うなタスクの場合, 先行研究で提案されている CBF の設計手法では, 補助的な時間関数の設計といった手順を用いて STL のロバストネス関数から CBF を設計する必要がある [19, 24].
加えて, CBF 等の制約条件付き最適化問題を解くことによって制御入力を生成する手法では,最適化問題の解が存在しない場合に, 制御入力が得られず制御不能になるといった問題が存在する.
さらに, 行動を実行している際に想定している行動では与えられたタスクが達成できなくなる場合が存在する. そのような場合の例として, 複数のロボットが行動を実行している際に, ロボットが不具合を起こして行動の実行が不可能になる場合である. この際, タスクを達成するためには残ったロボットが行動の変更を行い, 不具合を起こしたロボットの行動を補う必要がある.

1.2

本論文の目的

本論文では，時相論理命題で表現されたタスクをロボットシステムが達成するための制御手法の性能および拡張性の改善を目的とし, 行軌道最適化手法や制御手法を提案する. また,
本論文では有限時間で命題の真偽が定まる syntactically co-safe LTL (scLTL) 命題 [25] および同様な形式で記述できる STL 命題で表現可能なタスクを対象とし, それらのタスクに対して有限時間のロボットシステムの行動計画や軌道を生成し, それを基に制御を行うことでタスクを達成する.
本論文では, Fig. 1.1 に示す複数の段階に分かれた構造を持つ制御アーキテクチャを仮定する.
高レベル制御器では，制御対象であるロボットシステムに対して, 抽象化されたロボット
システムを定義する. この抽象化したシステムに対して, LTL や STL 命題で与えられたタスクを達成するための行動や軌道を最適化によって生成する. この際, 抽象化したシステムの設計方法によって, 実際の制御対象であるロボットの特性をどの程度反映することができるかが異なっている. そのため, 本論文ではロボットシステムの抽象化手法について検討を行う. 加えて, 抽象化したシステムに対して与えられた時相論理命題を満たす軌道を計算できることを示す. さらに, Event-based STL 命題で表現された外的事象が関与するタスクに対する軌道計画手法を提案する.
低レベル制御器では, 高レベル制御器で生成した行動計画や軌道を用いて, 実際の制御対象であるロボットシステムを制御する. 本論文では, 与えられた時相論理命題を達成するた

第 1 章 序論

4Fig. 1.1: Control architecture in this paper
めに時間的な制約を考慮した CBF を用いた制御手法を提案する. 加えて, 軌道を分布として表現し, 行動中に得られた観測情報から分布を更新し行動を変更する手法についても検討を
行う.

1.3

論文構成

本論文の構成を以下に示す．

1 章 序論
論文の全体としての研究背景と目的について，先行研究との対比を行いながら紹介を行った．

2 章 事前知識
本章では, 本論文に関する事前知識として, まず時相論理の基礎的な内容について説明する. 続いて, 時相論理に基づくロボットシステムの制御に関する基礎として, ロボッ
トシステム上での時相論理を用いたタスク表現について説明する.

3 章 混合整数計画問題による軌道最適化
本章では, 線形時相論理で表現されたタスクに関する軌道最適化を混合整数計画問題として定式化し, ハイブリッドシステムまたは非線形システムに適用する. 非線形システ
ムに適用する際, 混合整数凸計画や Koopman 作用素を用いた近似線形システムを導入することで, 混合整数線形計画問題として定式化する. 最後に, 数値シミュレーション

1.3. 論文構成

5にて, いくつかのロボットシステムに提案手法を適用し, 各ロボットシステム上で与えられた時相論理が達成されることを示す.

4 章 不確かな外的事象に関する命題への軌道最適化
本章では, 発生時刻が不確かな外的事象に関する命題を達成するための軌道最適化手法を提案する. 外的事象はロボットの状態に依存せず発生する事象であり, 本章では発生時刻に関する分布が既知であると仮定する. その分布に基づき, 複数の発生時刻を想定して与えられた命題を達成する軌道を生成することが可能な軌道最適化を非線形計画問題として定式化する. 最後に, 不確かな外的事象に関する命題が与えられた移動ロ
ボットに対する数値シミュレーションにて, 提案手法の有効性を示す.

5 章 時間制約をもつ信号時相論理に対する制御
本章では, 時間制約をもつ様相作用素を含む信号時相論理を達成するための制御バリア関数を用いた制御手法を提案する. 与えられた時相論理命題に対応する時間オートマ
トンと, 3,4 章で述べた軌道最適化等の手法によって生成された軌道に対応するパスを考える. このパスに含まれる時間オートマトン上の各ロケーションの遷移に対応する命題を達成するために, CBF を拡張した制御器を提案する. この制御器を用いてロケー
ション間の遷移を逐次達成することで, 最終的に与えられた本来の命題が達成されることを示す. 最後に, 移動ロボットに対する数値シミュレーションにて, 提案手法の有効性を示す.

6 章 信号時相論理に対する確率的動作プリミティブの再配合
本章では, 複数のシステムでタスクを行う際に, 他のロボットの行動に合わせて自身の行動を修正するための軌道最適化手法を提案する. まず 3,4 章で述べた軌道最適化に
よって得られた軌道から, 確率的動作プリミティブを生成する. 生成された確率的動作
プリミティブを用いて, 他のシステムの行動が属している動作プリミティブをカルマン
フィルターを用いて推定する手法を提案する. その後, 推定された動作プリミティブを基に, 自身の動作プリミティブを更新することで, 他のシステムの行動を考慮して与えられた命題を達成する軌道を生成する手法を提案する. 最後に, 移動ロボットに対する
数値シミュレーションにて, 提案手法の有効性を示す.

7 章 結論本章では，本論文で得られた成果の総括と，我々の研究の今後の発展と課題などについて述べる．

数学記号
Mathmatical symbols
R

実数全体の集合 (R+ は正の実数全体の集合, R0+ は 0 を含む正の実数全体の集合)

Rnn 次元実数ベクトル全体の集合

Rm×n m × n 次元実数行列全体の集合
C

複素数全体の集合

Int

集合の内部

0m×n

m 行 n 列の零行列Inn 次元単位行列AT

行列 A ∈ Rm×n の転置

A−1

正方行列 A ∈ Rn×n の逆行列

Lf h

関数 h(x) : Rn → Rp の関数 f (x) : Rn → Rn に沿った微分 Lf h = ∂h
f (x)
∂x

diag(a1 , . . . , an ) スカラー ai ∈ R, (i = 1, ...n) を対角要素にもつ対角行列
N (µx , Σx ) 平均値 µx ，分散 Σx を持つガウス分布
Logical symbols::=

Backus-Naur 記法, BNF¬, ∧, ∨, ⇒ 論理記号 (否定, 論理積, 論理和, 実質含意)|=

Double-turnstile

Other symbols
x(t0 : tN ) x の時系列データ, 無限長の場合, x(t0 :)xt0 :tN x(t0 : tN ) を省略した表現

6

第2章

事前知識

本章では, 本論文に関する事前知識として, まず線形時相論理の基礎的な内容について説明する. 続いて, 時相論理に基づくロボットシステムの制御に関する基礎として, ロボットシ
ステム上での時相論理を用いたタスク表現について説明する. その後, 線形時相論理を拡張した信号時相論理について説明する.

2.1

線形時相論理2.1.1

クリプキモデル

クリプキモデルとは, 原子論理命題の集合 AP 上で定義されたクリプキ構造 (S, R) を持つ組 (S, R, V ) である [26].

• (S, R) はクリプキ構造• V : S × AP → {⊤, ⊥} は付加関数
ここで, クリプキ構造は次のような組 (S, R) である.

• S は空でない集合, S の各元 s ∈ S を状態または可能世界と呼ぶ• R は S 上の二項関係

2.1.2

線形時相論理線形時相論理 (LTL) は様相論理の１つであり, 状態遷移列の性質を表現する論理である.
LTL 論理式の集合は, 以下の BNF によって定義される.

φ ::= π | ¬φ | φ1 ∧ φ2 | ⃝ φ | φ1 Uφ2 ,(2.1)

ここで, π は原子論理命題, φ は時相論理式, ⃝ は next 作用素, U は until 作用素とする. ∨等その他の論理演算子や G (always) や F (eventually) といった様相作用素は上記の作用素
によって定義できる:

Gφ = ⊤Uφ, Fφ = ¬G¬φ7

第 2 章 事前知識

8

LTL はクリプキモデルの各パスにおいて真偽が定まる. ここで, パスとはクリプキ構造上の状態がなす無限の列 s = s0 , s1 , . . . であり, パス上の各状態に対して (si , si+1 ) ∈ R となるもののことである.
本論文における LTL の意味論は次のように再帰的に定義される.

(s, t) |= π⇐⇒ st |= π(s, t) |= ¬φ⇐⇒ ¬((s, t) |= φ)(s, t) |= φ1 ∧ φ2 ⇐⇒ (s, t) |= φ1 ∧ (s, t) |= φ2(s, t) |= ⃝φ⇐⇒ (s, t + 1) |= φ(s, t) |= φ1 U φ2 ⇐⇒∃t′ ∈ T s.t. (s, t′ ) |= φ2∧ ∀t′′ ∈ [t, t′ ] , (s, t′′ ) |= φ1 .

2.2

ロボットシステム上での時相論理

ここでは, 本論文で扱うロボットシステムについて説明する. 加えて, ロボットシステムを
クリプキモデルとして扱うことができることを示し, 本論文で扱う時相論理について述べる.
本論文で扱うロボットシステムは, 以下の式で表されるダイナミクスを持つシステムである:

x+ = f (x(t), u(t)),(2.2)

ここで, t ∈ T は時刻, x ∈ X ⊂ Rnx は状態, u ∈ U ⊂ Rnu は入力, f : Rnx × Rnu → Rnx は遷移関数, x+ は x の時間微分, または時刻 t + 1 での x である.
遷移関数 f がロボットシステム上のすべての状態に対して定義されているとする. この際,ロボットシステム上の軌道 x(t0 :) = x(t0 ), x(t1 ), . . . が得られた際, 状態 x(ti ), x(ti+1 ) に対して x(i + 1) = F (x(i + 1), u(i)) を満たす関数 F : Rnx × Rnu → Rnx , 入力関数 u(t) : R → Ruが存在する. そのため, 状態 x(ti ), x(ti+1 ) は二項関係を持つ. 従って, 本論文で扱うロボット
システムに対してクリプキ構造を考えることができる. 加えて, 付加関数 V を以下のように定義することでクリプキモデルとして扱うことができ, ロボットシステムの振る舞いを LTL
を用いて表現することができる.
定義 1. ロボットシステム上の軌道 x が与えられたとき, 付加関数 V は各時刻の状態 x(t) に対して定義される, V : Rnx × AP → {⊤, ⊥}.
本論文では以後, パスとしてロボットの軌道 x を扱うことで, 命題の意味論を (x, t) |= φで表わすこととする.
加えて, LTL 命題として表現されたタスクが無矛盾であることをチェックすることは一般に計算量が非常に大きくなる [27]. 従って, 実際の応用にあたっては, 計算が容易に抑えられ2.3. 信号時相論理

9るような LTL のサブクラスである GR(1) や scLTL などに限定して用いるなどの工夫が必要になる [25, 28]. 特に, 有限時間で命題の真偽が定まる scLTL で表現されるタスクに対して本論文で提案する手法を適用することで, 時相論理命題を満たす有限時間のシステムの軌道を生成することが可能である.

2.3

信号時相論理Signal Temporal Logic (STL) は LTL を拡張した様相論理である. STL の原子論理命題
AP, π の充足関係は以下の様にロバストネス関数 ρπ : Rnx → R によって評価される [5]:⊤ if ρ (x) ≥ 0
π
.
(2.3)
π=
⊥ if ρπ (x) < 0
本論文では, ロバストネス関数は微分可能な連続関数と仮定する.
STL 論理式の集合は LTL と同様に以下の BNF で定義される.

φ ::= π | ¬φ | φ1 ∧ φ2 | φ1 UI φ2 ,(2.4)

ここで I = [t1 , t2 ], t1 < t2 [29], UI は時間区間 I に対する until 作用素とする. GI (always)
や FI (eventually) といった様相作用素は LTL と同様に上記の作用素によって定義できる.
STL の意味論は以下の様に再帰的に定義される:(x, t) |= π⇐⇒ ρπ (x(t)) ≥ 0(x, t) |= ¬φ⇐⇒ ¬((x, t) |= φ)(x, t) |= φ1 ∧ φ2⇐⇒ (x, t) |= φ1 ∧ (x, t) |= φ2(x, t) |= φ1 U[t1 ,t2 ] φ2 ⇐⇒∃t′ ∈ [t + t1 , t + t2 ] s.t. (x, t′ ) |= φ2
このとき, φ∧ =計算できる:

Vi φi や φ∨ =W

∧ ∀t′′ ∈ [t, t′ ] , (x, t′′ ) |= φ1 .
i φi といった命題に対するロバストネス関数は以下の様に

ρφ∧ (x) = min ρφi (x),(2.5)ρφ∨ (x) = max ρφi (x).

(2.6)

i

i

また, 上記のロバストネス関数は min / max を近似することで, 下記の様になめらかな関数として表現できる [19]:!
X1min ρφi (x) ≥ − lnexp(−κρφi (x)) ,
(2.7)
i
κ
i=1
!
X
1
max ρφi (x) ≤ ln
exp(κρφi (x)) ,
(2.8)
i
κ
i=1

10
ここで κ > 0 は定数とする.

第 2 章 事前知識

第3章

3.1

混合整数計画問題による軌道最
適化

はじめに

混合整数計画問題による定式化では, 時相論理に関する離散変数とロボットシステムに関する連続変数を同時に扱うことが可能である. しかし, 混合整数非線形計画問題の場合では計算コストが大きい傾向にあり, 非線形システムや非線形制約に対して適用することが難しい.
これを踏まえ, 本章では非線形システムや非線形制約を線形システムで近似し, 混合整数
計画問題として定式化することを検討する. まず, 混合整数計画問題による定式化の準備として, 本章で扱う混合論理動的 (MLD) モデル, 混合整数凸計画, 拡大線形化について説明する. 次に, 混合整数計画問題による定式化ついて説明する. その後, 混合整数凸計画を用いることで, クォータニオンベースの SO(3) システムに対して混合整数線形計画問題の定式化が可能であることを示す. 加えて, 拡大線形化を用いることで, 非線形システムに対しても混合
整数線形計画問題の定式化が可能であることを示す. 最後に, 数値シミュレーションにて, い
くつかのロボットシステムに提案手法を適用し, 各ロボットシステム上で与えられた時相論
理命題が達成されることを示す

3.2

準備

ここでは, MLD モデル, 混合整数凸計画, 拡大線形化について説明する. 特に, MLD モデ
ルとして, ロボットのタスクに現れるハイブリッドシステムの例であるアーム型ロボットのピックアンドプレイスタスクにおける手先座標の近似システムについて説明する.

3.2.1

混合論理動的モデル

ハイブリッドシステムは連続的な状態と離散的な状態の両方を含むシステムである.
混合論理動的 (MLD) モデルはハイブリッドシステムの表現の 1 つであり, 本論文では MLD
モデルを下記の様に記述する.
 x+ = Ax + B u + B z + B ϖ + B ϑ + B p
1
2
3
4
5
(3.1)
 Cx + D1 u + D2 z + D3 ϖ + D4 ϑ + D5 p ≤ E,11

第 3 章 混合整数計画問題による軌道最適化

12

ここで z(t) ∈ Rnz は連続値の補助変数, ϖ(t) ∈ {0, 1}nϖ は MLD モデルの離散モードに関する離散値の補助変数, A, Bi , C, Di , E, (i = 1, 2, 3, 4, 5) はシステム行列とする. 加えて,
ϑ(t) ∈ {0, 1}nϑ , p(t) ∈ [0, 1]np はそれぞれ LTL に関係する離散値と連続値の変数である.
B2 = 0nx ×nz , D2 = 0nE ×nz とすることで, 複数のモードを持たない線形システムを表現することができる.
ピッキングモデルのハイブリッドシステム表現
ピックアンドプレイスタスクに対して, アーム型ロボットの手先座標とオブジェクトの中
心座標を状態とする近似システムを考える. 近似システムの状態として, 以下を定義する.
h
iT
T
T
T
x = Xa,1 Xo,1 . . . Xo,no ,
(3.2)
h
iT
Xai = xTarm,i ẋTarm,i ,
(3.3)
h
iT
Xoi = xTobj,i ẋTobj,i ,(3.4)
ここで Xa,i は手先 i の状態, Xobj,i はオブジェクト i の状態, no はオブジェクトの数とする.
また, x∗ , ẋ∗ は各手先とオブジェクトの位置と速度とする.
ピッキングのモデル化
ピッキングモデルの挙動を Fig.3.1 に示す. 近似システム上では, アームがオブジェク
トをつかんでいるとき, オブジェクトの速度は手先の速度と一致すると仮定する. ここで,ϖij ∈ {0, 1} を命題 “アーム i がオブジェクト j を掴んでいる” の論理値を示す変数とする.
このとき, ピッキング動作による状態遷移は以下の様に記述できる:

ϖij = 1 ⇒ ẋarm,i = ẋobj,j .

(3.5)

MLD システムを用いて上記の状態遷移を含むシステムは次のように記述できる:
x+ = A¬ϖ x + B ¬ϖ u + ϖ((Aϖ − A¬ϖ )x + (B ϖ − B ¬ϖ )u),(3.6)

ここで A¬ϖ , B ¬ϖ は ϖ = 0 のときのシステム行列であり, Aϖ , B ϖ は ϖ = 1 のときのシステ
ム行列とする. 例として, MLD システムは次のように表わすことができる (Fig. 3.1).
"# "#xobj,1
xobj,1 + ∆T ẋobj,1
P
=
(3.7)
ẋobj,1i ϖi1 ẋarm,i
ここで ∆T は時間ステップとする.
また, ϖij は次の命題を満たす:

ϖij = 1 ⇒ φϑij ,(3.8)

3.2. 準備13

ここで命題 φϑij は ”xarm,i ∈ Hij ”を示す命題, Hij はアーム i とオブジェクト j の状態に関係するポリトープである:

Hij = {xarm,i ∈ R2 or 3 : Cij (xarm,i − xobj,j ) ≤ eij },(3.9)

ここで Cij は定数行列, eij は定数ベクトルとする. この命題は, アームがオブジェクトに関するポリトープの内側にあるときに限り, ピッキング動作を実行することが可能であることを意味している. 式 (3.7) で表現されるダイナミクスと, 命題 (3.8) とポリトープ (3.9) に関する関係式を後述する方法によって線形制約で表現することで, ピッキングモデルを MLDシステムとして式 (3.1) の形式で表現できる.

14

第 3 章 混合整数計画問題による軌道最適化

Fig. 3.1: Modeling of picking. Top: the robot behavior in the real world. Bottom: thebehavior of MLD model in the simulated world. xarm,1 and xobj,1 are the position of thearm 1 and the object 1, respectively. H11 is the bounding box of the object and proposition
φϑ11 is xarm,1 ∈ H11 . When the arm enters the bounding box H11 , φϑ11 becomes 1. We canchange ϖ11 to 1 and when ϖ11 = 1, the velocity constraint of the object is activated, and it
follows the arm. This is the modeling of picking. Since the proposition ϖ11 = 1 is a suﬃcientcondition of (3.8), the arm can release objects after picking the object.

3.2. 準備

3.2.2

15

混合整数凸計画

双線形項 xy を混合整数凸計画 (MICP) で近似する手法について説明する [30]. まず, スカyx∗
ラ変数 x ∈ [ψ0x , ψN], y ∈ [ψ0y , ψN
] に対して, x, y の定義域を Nw 個の区間 [ψi∗ , ψi+1
] に分割
w
w
する. その後, いくつかの補助変数を導入し, 次の制約を用いて変数 wxy によって双線形項
xy を近似する (Fig.3.2):x=

Xλx, i ψix , y =λy, j ψjy ,(3.10)

γij ψix ψjy ,(3.11)

γij = λy, j ,(3.12)

j

i

wxy =
X

X

XX
i

γij = λx, i ,

j

X

j

i

ここで λx, i , λy, j , γij ∈ [0, 1] (i, j = 0, . . . , N ) は連続値の補助変数であり, 特に λx, i , λy, j は
以下のように定義される special ordered sets of type 2 (SOS2) 制約を満たす変数とする.
定義 2. (SOS2 制約) もし変数 λi (i = 0, . . . , N ) が SOS2 制約を満たすならば, λi は次の条件を満たす;• 最大で 2 つの λi は非ゼロである.
• もし 2 つの λi が非ゼロであれば, それらの添え字は隣り合っている.
• λi の合計は 1 である.
SOS2 制約は log2 Nw 個のバイナリ変数を用いて混合整数線形制約として実装することができる [31].
同様に二乗項についても下記の様に近似することができる: wx2 ≈ x2 ,Xx=λx, i ψix ,(3.13)
iw x2 =XjXXiγij = λx, i ,γij ψix ψjx ,(3.14)

γij = λx, j .

(3.15)

j

Xi

本章の数値実験では, 特に記載がない場合, これらの制約について分割区間を Nw = 4 として実装している.

第 3 章 混合整数計画問題による軌道最適化

16

Fig. 3.2: An approximation of the quadratic term xy by MICP

3.2.3

拡大線形化

次の微分方程式で表現される連続時間非線形システムを考える

ẋ(t) = f (x(t), u(t)).

(3.16)

ここで Γ(t, x) をシステム (3.16) の ﬂow map, F を無限次元の観測関数 g : X → C の空間とする. 連続時間の Koopman 作用素 Kτ : F → F は次の関係を満たす:(Kτ g)(·) = g ◦ Γ(τ, ·),(3.17)(Kτ g)(x(t), ū(t)) = g(x(t + τ ), ū(t)).

(3.18)

ここで ◦ は写像の合成を表わす:ū は時間区間 [t, t + τ ] で一定値の入力とする.
無限小生成作用素 Kτ を用いてシステムの軌道に沿った観測関数の時間微分は以下の様に
書ける;
Kg =

d
g,
dt

(3.19)

3.2. 準備

17

すなはち,

(Kg)(x(t), u(t)) =∂gf (x(t), u(t)).
∂x(3.20)

そのため, Koopman 作用素を用いることで非線形システムを線形システムとして表現することができる. 本来の Koopman 作用素は無限次元であるが, 実際に制御などで用いる場合は,Koopman 作用素を有限次元の観測関数を用いた拡大状態空間で近似する. この際, 近似するシステムに元々含まれていた状態に対して, 新たに追加する状態を拡大状態と呼ぶことにする. 加えて, 本来の Koopman 理論においては入力のない自励系について扱っているが, 入力のあるシステムへの理論の拡張についても研究されている [32] [33].
Koopman 作用素の近似を用いることで, 非線形システムを以下のように線形システムとして表現できる.

ξ˙ = Aξ ξ + Bξ u,(3.21)

x = Cξ ξ,(3.22)

ここで ξ ∈ Rnξ は拡大状態空間上で定義される高次 nξ ≥ nx の状態変数である:
g1 (x)
 g2 (x) ξ= ..  .
.
gnξ (x)(3.23)

特に, もし ξ = [xT , gT ]T ならば, Cξ = [Inx ,nx , 0nx ,nξ −nx ] となる. 行列 Aξ および Bξ は定数行列であり, この拡大線形システムは非線形システムを大域的に近似したシステムになっている.
しかし, ロボットなどのシステムの多くは, 線形な入力伝達項をもつ線形システムとして表現することが難しいことが知られている. そのため, Koopman 作用素を用いて表現可能なシステムのクラスを拡大するために, 状態と入力の双線形項を持つようなモデルが提案されている [34]:ξ˙ = Aξ ξ + Bξ u +nuXHξ,i ξui ,(3.24)

ix = Cξ ξ,(3.25)

ここで, Hξ,i (i = 1, . . . , nu ) は定数行列である.
観測関数の選定方法や及び定数行列 Aξ , Bξ , Cξ , and Hξ,i の計算方法については多くの手法が提案されており, 例としては Extended Dynamic Mode Decomposition (EDMD) [35] やdeep learning を用いた手法 [36] が挙げられる. 本章で提案する手法は, 上記の述べた計算手法に依存せず適用可能である. 加えて, Koopman 作用素および本章で提案する手法は離散時間システムに対しても適用することができ, 本章内でも離散時間システムについて考えるものとする.

第 3 章 混合整数計画問題による軌道最適化

18

3.3

混合整数計画問題による定式化

LTL 命題で表現されたタスク φ を達成する MLD システムの軌道を生成する問題を混合
整数線形計画問題として定式化する. まず, LTL 命題 φ を線形制約として表現する. その後,前節で定義した MLD システムを用いることで混合整数線形計画問題または二次計画問題として定式化できることを示す.

3.3.1

線形時相論理命題の線形制約

LTL 命題 φ に対して, サブ命題 φi (i = 1, 2, , . . . ) を定義する.
Example 1. LTL 命題:
φ = G¬(π1 ∧ π2 ) ∧ F ((π3 ∨ π4 ) ∧ ⃝π5 ) ,に関するサブ命題は,

φ1 = Gφ3 , φ2 = Fφ4 ,φ3 = ¬φ5 , φ4 = φ6 ∧ φ7 ,φ5 = φ8 ∧ φ9 , φ6 = φ10 ∨ φ11 , φ7 = ⃝φ12 ,φ8 = π1 , φ9 = π2 , φ10 = π3 , φ11 = π4 , φ12 = π5 .
各サブ命題に対して, 原子論理の真偽を示す変数 ϑj (t) ∈ {0, 1}, それ以外の命題の真偽を示す変数 pj (t) ∈ [0, 1] を定義する. ここで, p0 は φ の真偽を示す変数とする. このとき, 有
限な時間区間 [0, T ] におけるサブ命題の論理的関係は, 以下の LTL 命題 φ の真偽を示す変数 p(t) = [0, 1] に関する制約条件を用いて逐次的に表現することができる [2].

φ = ¬φi : p(t) = 1 − pi (t)
φ=

φ=

k
_
i=1
k
^

φi : p(t) ≤ pi (t) (i = 1, . . . , k), p(t) ≥φi : p(t) ≥ pi (t) (i = 1, . . . , k), p(t) ≤

k
X
i=1
k
X

i=1

pi (t) − (k − 1)pi (t)i=1

φ = ⃝φi : p(t) = pi (t + 1)
φ = Gφi : p(t) ≤ pi (τ ) (i = 1, . . . , T ), p(t) ≥φ = Fφi : p(t) ≥ pi (τ ) (i = 1, . . . , T ), p(t) ≤

T
X
τ =t
T
X

pi (τ ) − (T − t)pi (τ )

τ =tuntil 演算子を用いた命題 φ = φi Uφj に関しても新たな補助変数を導入することで制約条件として表現することができる [2]. 原子論理命題以外の命題の真偽を示す変数の値は, 原理論

3.3. 混合整数計画問題による定式化

19

理命題の真偽を示す離散変数に依存して決定されるため, 連続変数である命題の真偽を示す
変数 pj も, 制約条件を満たすならば 0-1 変数のように振る舞う. このため, 実装において, 真
偽を示す変数は原子論理命題に関する変数 ϑj のみを離散変数とすれば十分である.

3.3.2

付加関数との対応関係

原子論理命題の真偽を返す付加関数は, ロボットシステム上での制約条件と真偽を示す変数との制約条件で表現することができる [2] [37]. 本章では, 各原子論理命題はロボットシステ
ムの状態に関する線形制約の真偽, または, 離散入力に対応すると仮定し, 以下に示す big-M
線形拘束を用いて真偽を示す変数と制約条件との対応関係を実装している. big-M 線形拘束
では, 十分に大きな定数 M > 0 を用いることで離散変数の振る舞いを拘束することができる. さらに, 目的に合わせて以下の 2 種類の制約条件を用いている.

1. ϑ = 1 ⇐⇒ Cϑ x ≤ 1 を満たす場合 [2]:Cϑ x ≤ 1 + M (1 − ϑ), Cϑ x ≥ 1 + M (ϵ − ϑ)
2. ϑ = 1 ⇒ Cϑ x ≤ 1 を満たす場合 [37]:Cϑ x ≤ 1 + M (1 − ϑ)
ここで, CϑT ∈ Rnx は定数ベクトル, ϵ > 0 は十分小さな定数である. 2 番目の big-M 線形拘
束は, 線形制約が満たされている場合に ϑ = 1 となることが可能であり, 前節で述べたピッキ
ングモデルのピッキング動作を離散入力を用いて表現する場合などに用いられる.

3.3.3

混合整数線形 (二次) 計画問題による定式化

LTL 命題 φ と, ロボットシステムとして式 (3.1) で表わされる MLD モデルが与えられてTいるとする. このとき, 最適化変数 Z = [ZxT , ZuT , ZzT , Zϖ
, ZϑT , ZpT ]T を定義する. Z∗ は各変数
x, u, z, ϖ, ϑ, p の時刻 t = 0, . . . , T での値をまとめたものとする.
このとき, 前小節から LTL 命題に関する制約条件は以下のように記述できる:
Dφ,ϑ Zϑ + Dφ,p Zp ≤ Eφ,(3.26)

ここで Dφ,ϑ , Dφ,p は定数行列, Eφ は定数ベクトルとする. 制約条件 (3.26) には, 与えられた LTL 命題に関する制約 −p0 ≤ −1 が含まれている. また, 式 (3.1) から MLD モデルに関する制約条件は以下の様に記述できる:

Cx Zx + Dx,u Zu + Dx,z Zz + Dx,ϖ Zϖ + Dx,ϑ Zϑ + Dx,p Zp ≤ Ex ,(3.27)

第 3 章 混合整数計画問題による軌道最適化

20

式 (3.26)(3.27) から, LTL 制約を満たす軌道を生成するための混合整数線形 (二次) 計画問
題は以下のように定式化できる:

min fZ Z(+Z T QZ Z),
Z

s.t. DZ Z ≤ EZ ,(3.28)

ここで, DZ は定数行列, fZ , EZ は定数ベクトル (QZ は定数行列) である.
fZ , (Qz ) は所望のシステムの振る舞いに応じて設定することができる. 例として, システ
ムの状態や入力の l1 (l2 )-ノルムの最小化が挙げられる [38]. また, 命題の真偽を示す変数に対して重みづけを行うことで, システムの振る舞いを表現することも可能である. 例として, 特定の領域内に存在することを示す命題の真偽を示す変数に重みづけすることによって, シス
テムのその領域での滞在時間を調節するといったことが可能である.

3.4

クォータニオンベースの SO(3) の MICP を用いた行動計画

本節では, MICP による双線形項, 二乗項の近似を用いたクォータニオンベースの SO(3)システムのダイナミクスを近似する手法を提案する. まず, オイラーパラメーター SO(3) のダイナミクスを近似する手法について説明する. その後, 近似したダイナミクスをピックア
ンドプレイスモデルと組み合わせる方法について説明する.

3.4.1

オイラーパラメーターの近似

オイラーパラメーターは表現上の特異姿勢を持たない回転を表現する手法の一つであり,次式のように定義される [39]:ε = a sinθaθa, η = cos ,22(3.29)

ここで a ∈ [−1, 1]3 は回転軸を示す 3 次元ベクトル, θa は回転軸 a 周りの回転量である. また, オイラーパラメーターはユニットクォータニオンと等価である. オイラーパラメーターのダイナミクスは次式で計算できる,#" #"1 ηI3×3 − ε×
d ε
ω
=
dt η
2
−εT


ηω1 + ε3 ω2 − ε2 ω3
1 −ε3 ω1 + ηω2 + ε1 ω3 
= 
(3.30)
,2  ε2 ω1 − ε1 ω2 + ηω3 −ε1 ω1 − ε2 ω2 − ε3 ω3

3.4. クォータニオンベースの SO(3) の MICP を用いた行動計画

21

ここで ε = [ε1 , ε2 , ε3 ]T ∈ [−1, 1]3 , ω = [ω1 , ω2 , ω3 ]T ∈ R3 は角速度, ε× は ε の外積の行列表
現である. オイラーパラメーターを用いて回転行列は以下のように計算できる:R(η, ε) = (η 2 − ||ε||2 )I3×3 + 2εεT + 2ηε×
r11 r12 r13= r21 r22 r23  ,r31 r32 r33(3.31)

ここで r11 = ε21 − ε22 − ε23 + η 2 , r12 = 2(ε1 ε2 − ε3 η), r13 = 2(ε1 ε3 + ε2 η), r21 = 2(ε1 ε2 +ε3 η), r22 = −ε21 + ε22 − ε23 + η 2 , r23 = 2(ε2 ε3 − ε1 η), r31 = 2(ε1 ε3 − ε2 η), r32 = 2(ε2 ε3 + ε1 η),
r33 = −ε21 − ε22 + ε23 + η 2 . オイラーパラメーターのダイナミクスおよび回転行列に含まれる非線形項は η, ε, ω の二乗項または双線形項のみである. そのため, 二乗項および双線形項を
MICP によって近似することで, ダイナミクスと回転行列を線形制約として表現することができる. この際, ダイナミクスに 12 個, 回転行列に 10 個の二乗項および双線形項が含まれる
ため, それぞれに対して近似変数が必要になる.
このとき, オイラー法を用いて SO(3) のダイナミクスは以下のように表現できる:




w
+
w
−
w
ηω
ε
ω
ε
ω
1
3
2
2
3
" #+ " #εε∆T  −wε3 ω1 + wηω2 + wε1 ω3 +=

2  wε2 ω1 − wε1 ω2 + wηω3 ηη
−wε1 ω1 − wε2 ω2 − wε3 ω3" #ε+ Bw w,=η
rw11 rw12 rw13


Rw = rw21 rw22 rw23 
rw31 rw32 rw33
= JRw w,

(3.32)(3.33)

ここで w = [wηω1 , wε3 ω2 , . . . ] ∈ R22 は近似変数をまとめたベクトル, Bw , Jw は定数行列,rw11 = wε21 − wε22 − wε23 + wη2 , rw12 = 2(wε1 ε2 − wε3 η ), rw13 = 2(wε1 ε3 + wε2 η ), rw21 = 2(wε1 ε2 +wε3 η ), rw22 = −wε21 + wε22 − wε23 + wη2 , rw23 = 2(wε2 ε3 − wε1 η ), rw31 = 2(wε1 ε3 − wε2 η ), rw32 =2(wε2 ε3 + wε1 η ),, rw33 = −wε21 − wε22 + wε23 + wη2 .
この際, 拡張オイラー法やルンゲクッタ法といった他の数値積分手法を用いることもできる [40]. 一方で, 近似精度と計算コストはトレードオフの関係にあることを考慮する必要がある.

第 3 章 混合整数計画問題による軌道最適化

22

3.4.2

姿勢を考慮した MLD システム

オイラーパラメーターをピッキングモデルに関する MLD モデル (3.4) に組み込むことを考える. このとき, ロボットシステムの状態を以下のように定義する:
hiT
Xai = xTarmi ẋTarmi εTarmi ηarmi ,(3.34)hiT
Xoi = xTobji ẋTobji εTobji ηobji ,(3.35)
ここでは, オイラーパラメーターに関する入力は角速度とする. ピッキング動作に伴うオイ
ラーパラメーターのダイナミクスは以下のように仮定する:
"
#+ "
#
εarmi
εarmi
=
+ Bw warmi ,(3.36)
ηarmi
ηarmi
"
#+ "
#
εobjj
εobjj
=
+ ϖij Bw wobjj .
(3.37)
ηobjj
ηobjj
この MLD システムを用いることで, アームとオブジェクトの姿勢を考慮したピッキングモ
デルを構築することができる. このロボットシステムを用いてピッキングモデルに対する混合整数線形計画問題の定式化の際の離散変数の数は, 計画問題のステップ数 N に対して
(nxy log2 N + nϑ + nϖ )N となる. このとき, nxy は双線形項または二乗項に含まれる状態の種類の数である.

3.4.3

2 次元モデルの姿勢近似

2 次元平面上での姿勢 ηz も同様に, 拡大変数 sin(ηz ), cos(ηz ) を用いて近似できる. このとき, 姿勢に関するダイナミクスは次式で表現できる:

+ 



ηz
ηz
ω3






(3.38)
 sin(ηz )  =  sin(ηz )  + ∆T  wω3 cos(ηz )  .
cos(ηz )cos(ηz )−wω3 sin(ηz )オイラーパラメーターと同様に MLD モデルに組み込むことも可能であり, 2 次元平面上でののアームとオブジェクトの姿勢の制約を考慮したピックアンドプレイスタスクの行動計画を計算することが可能である.

3.5

拡大線形システム, 拡大双線形システムを用いた行動計画

本節では, 拡大線形, 双線形システムに対する行動計画手法を提案する. まず, 拡大線形, 双線形システムに対して MLD システムを定義し, 混合整数計画問題として定式化できることを示す. 次に, 非弾性衝突のような状態のジャンプがあるシステムに対しても, 拡大線形, 双線形システムを用いて MLD システムとして表現できることを示す.

3.5. 拡大線形システム, 拡大双線形システムを用いた行動計画

3.5.1

23

拡大線形システムのための MLD システム

本章では, システムのダイナミクスに複数のモードがある場合, それぞれのモードにおいて同一の拡大状態空間を用いてシステムを表現することが可能であると仮定する. このとき,
拡大線形システムに対する MLD システムは以下のように定義できる: ξ + = Aξ + B u + B z + B ϖ + B ϑ + B p
1
2
3
4
5
(3.39) Cξ + D1 u + D2 z + D3 ϖ + D4 ϑ + D5 p ≤ E.
特に, モードの数が 2 つの場合, 各行列は次のようになる; A = Aξ,0 , B1 = Bξ0 , B2 = [Aξ1 −Aξ0 , Bξ1 − Bξ0 ], z = ϖ · [ξ T , uT ]T . ここで Aξ0 , Aξ1 , Bξ0 , Bξ1 は各モードのダイナミクスに関する定数行列である. この MLD システムは前述した MLD システム (3.1) と同等である. そのため, この MLD システムを用いることで, 拡大線形システムに対しても混合整数計画問題
による定式化が可能である.

3.5.2

拡大双線形システムのための MLD システム

拡大線形システムと同様に, システムのダイナミクスに複数のモードがある場合, それぞれのモードにおいて同一の拡大状態空間を用いてシステムを表現することが可能であると仮定する. 加えて, ダイナミクスの中に現れる双線形項を混合整数凸計画を用いて近似し, 以下のように新しい変数に置き換える.


ξu1


 ξu2 wξu ≈ (3.40)
 ..  .
.
ξunu
このとき, 拡大双線形システムに対する MLD システムは以下のように定義できる: ξ + = Aξ + B u + B z + B ϖ + B ϑ + B p + B w
1
2
3
4
5
6 ξu
 Cξ + D1 u + D2 z + D3 ϖ + D4 ϑ + D5 p + D6 wξu ≤ E,

(3.41)

ここで, B6 , D6 は定数行列である. 特に, モードの数が 2 つの場合, 各行列は次のようになる;A = Aξ,0 , B1 = Bξ0 , B2 = [Aξ1 −Aξ0 , Bξ1 −Bξ0 , Hξ1 ,1 −Hξ0 ,1 , Hξ1 ,2 −Hξ0 ,2 , . . . , Hξ1 ,nu −Hξ0 ,nu ],T T
B6 = [Hξ0 ,1 , Hξ0 ,2 , . . . , Hξ0 ,nu ], z = ϖ · [ξ T , uT , wξu] (i = 1, 2, . . . , nu ). ここで Aξ0 , Aξ1 , Bξ0 ,Bξ1 は各モードのダイナミクスに関する定数行列である. この MLD システムは wξu を z に含めることで, 前述した MLD システム (3.1) と同等となる. そのため, この MLD システムを用いることで, 拡大双線形システムに対しても混合整数計画問題による定式化が可能である.

24

3.5.3

第 3 章 混合整数計画問題による軌道最適化

拡大システム上での状態のジャンプ

機械システムには非弾性衝突のような状態のジャンプをもつものがある. このシステムを拡大システムとして表現する場合, そのジャンプに関する特性も拡大システムに反映する必要がある. ここでは, ジャンプの振る舞いが既知であると仮定し, 以下に示すような MLD シ
ステムの離散状態 ϖ が 0 から 1 へ変化した際に発生するジャンプについて考える:

ϖ = (0 ↗ 1) ⇒ q̇+ = P (q)q̇− .

(3.42)

ここで ↗ は ϖ の変化を表わし, q ∈ Rnq は一般化座標系, q̇− は ϖ が 1 になる直前の速度,q̇+ は ϖ が 1 になったことで状態のジャンプが発生した後の速度, P (q) はジャンプに関する射影行列とする. 以降では, この速度の射影として表現可能なジャンプをもつシステムについて考える.
このとき, 状態のジャンプは拡大状態空間上での状態変数同士の双線形項に対する制約として記述できる.
定理 1. 拡大システム (3.39) または (3.41) に対する状態のジャンプ (3.42) を考える. もし
P
m nq
1 m2
(mi ∈ [0, 1, . . . , M ], i mi ≤ M ),拡大システムの拡大状態が速度の多項式: q̇m1 q̇2 · · · q̇nqとそれらと一般化座標のみを引数とする関数の積のみで構成されているとき, 状態のジャン
プは拡大システムの状態の射影として表現できる,

ϖ = (0 ↗ 1) ⇒ ξ+ = Pξ (ξ− )ξ− .

(3.43)

ここで, ξ− は ϖ が 1 になる直前の状態, ξ+ は ϖ が 1 になり状態のジャンプが発生した後の状態, Pξ (ξ− ) は射影行列である.

Proof. 一般化座標はジャンプによって変化しないため, 一般化座標を引数とする関数 (m1 =m2 = · · · = mnq = 0 の場合) も変化しない. ⊗ を Kronecker 積とし, 速度ベクトル同士のm
m 回の Kronecker 積をベクトル q̇m ∈ R(nq ) , Kronecker 積のベクトルの i 個目の要素をm(nq )mm
mのジャンプは射影行列 Pm を用いて, q̇mq̇m+ = P (q)q̇−i ∈ R とする. ベクトル q̇ ∈ Rと表現できると仮定する. このとき, 速度ベクトル同士の m 回の Kronecker 積のベクトルの3.6. SO(3) に対する数値実験

25

ジャンプは,q̇m+1=(q̇+ ⊗ q̇m++)q̇1+ q̇m+ q̇2+ q̇m

+

=
..


.




q̇nq + q̇m+
Q1 P q̇− Pm q̇m− Q2 P q̇− Pm q̇m

−

=
..


.


Qnq P q̇− Pm q̇m−
m+1 m+1P1 q̇−
 m+1 m+1

 P2 q̇− 


=
..

.


m+1
Pm+1
nq q̇−=Pm+1 q̇m+1,−
ここで Pm+1(q)(i = 1, 2, . . . , nq ), Pm+1 (q) は射影行列, Qi は i 番目の要素が 1, それ以外がi
0 の行ベクトルとする. そのため, q̇m+1 のジャンプは q̇m+1 の射影となっている. q̇ の場合と速度ベクトル同士の m 回の Kronecker 積のベクトルの場合に射影であることが示された
ため, 数学的帰納法により, 速度ベクトル同士の m 回の Kronecker 積のベクトルのジャンプ
挙動は, あらゆる自然数 m に対して, q̇m の射影であることが示された.
さらに, 速度の多項式と一般化座標を引数とする関数との積のジャンプは次式で記述できる:
m(q̇mi g(q))+ =q̇i+ g(q)=Qi Pm (q)q̇m− g(q).
このとき, ベクトル q̇m+1g(q) の要素のジャンプは q̇m+1g(q) の射影である. そのため, もし拡ii
大状態が速度の多項式とそれらと一般化座標のみを引数とする関数の積のみで構成されているとき, 状態のジャンプは拡大システムの状態の射影として表現できる.
定理 1 に従って, 拡大状態を決定することで式 (3.43) で表現されるジャンプを線形または双線形な拘束条件として近似できる.

3.6

SO(3) に対する数値実験

本節では, MICP を用いた提案手法を 2 つのタスクに適用し, 有効性を確認する. まず, 3次元空間上でのオブジェクトの姿勢を考慮したピックアンドプレイスタスクに提案手法を適用する. 次に, 劣駆動システムであるドローンモデルの到達タスクに対して, 提案手法を適用

第 3 章 混合整数計画問題による軌道最適化

26

する. 本節の全ての計算は Intel Core i7 1.8GHz で行われ, 全ての混合整数線形計画問題はMATLAB によって実装され, ソルバーとして Gurobi [41] を用いた. また, 混合整数計画問題の最適性ギャップの上限を 1.0 × 10−4 と設定した.

3.6.1

3 次元のピックアンドプレイスタスク

1 つのアーム, 1 つのオブジェクト, および 1 つのゴールであるボックス領域で構成される
3 次元空間でのピックアンドプレイスタスクについて考える. オブジェクトに対して入力はアームから与えられ, コスト関数は次のように定義する.
XJ=||u(t)||1 ,(3.44)k∈T

ここで u ∈ R3 は並進方向の力, ||u||1 は l1 -ノルムとする. SO(3) への入力は角速度 ω ∈[−π/2, π/2]3 とし, アームの並進運動に関するダイナミクスは 2 重積分器のシステムと仮定する. また, 問題を単純化するため, アームの姿勢は考慮せず, オブジェクトの姿勢のみを考慮することとする. システムのダイナミクスはオイラー法を用いて実装した.
本実験では, 直方体状のオブジェクトを考え, オブジェクトの各頂点の座標は次のように計算する:

Vobji = Rwobj Lobji + xobj (i = 1, . . . , 8),(3.45)

ここで Rwobj はオブジェクトの姿勢を示す回転行列, Lobji は xobj に対する頂点 i の相対位置とする. また, 命題 “オブジェクトがボックス内に収まっている” を考え, この命題はオブジェクトの全ての頂点がボックス領域内に収まっているとき満たされるとする. 本実験ではオブジェクトのある 1 面に対してピッキング動作に関するポリトープを設定した (Fig.3.3 中,“picking polytope” ).
ピックアンドプレイスタスクの制約条件に関して以下の LTL 命題を定義する:• アームはボックス領域以外でオブジェクトを離さない:( ϖ11 ∧ ⃝¬ϖ11 ) ⇒ φθ1 ,(3.46)

ここで ϖ11 はアームがオブジェクトを掴むことを示す論理入力, φθ1 はオブジェクト 1
がボックス内に収まっていることを示す命題である.
このタスクに対して, オブジェクトの姿勢と位置が異なる 20 のケースにおいて命題を達成する軌道を提案手法を用いて生成した. 近似回転行列 Rw の近似精度を向上するために, 全てのケースで, オイラーパラメーターの初期状態は [0, 0, 0, 1] とし, オブジェクトの初期姿勢に合わせて Lobji を変更した. 時間ステップは ∆T = 0.3, N = 8 と設定した. このときの混合整数線形計画問題は 207 個の離散変数を含み, 87 個の変数は LTL 命題に関係し, 残りの 120 個
は SO(3) の双線形項に関係している.

3.6. SO(3) に対する数値実験27

結果として, 実行可能解は平均 7.63 秒, 標準偏差 ±9.00 秒以内に計算された. 最適解に関しては, 計算したケースの内, 1 つのケースのみ最適解が 6000 秒以内に収束せず, 他のケースにおいては平均 1.93 × 10+3 秒, 標準偏差 ±1.79 × 10+3 秒以内に準最適な軌道に収束した.
Fig.3.4 は, 2 つのケースと得られた準最適な軌道を示している.

Fig. 3.3: Setting of the object and box. xobj1 exists at the center of the picking polytope.
The arm needs to lay the object down to put it in the box.

Fig. 3.4: Behavior in semi-optimal solutions. Upper: a situation in which the object is lying
down. Lower: a situation in which the object is upright. The blue line with circles is the
trajectory of the arm. In each situation, the arm completes the pick-and-place task by laying
the object down.

第 3 章 混合整数計画問題による軌道最適化

28

3.6.2

ドローンモデルにおけるプランニング

下記のドローンモデルを考える.
" #
"
#
d εd
1 ηd I3×3 − εd ×
=
ω,
dt ηd
2
−εd T
 
   x00dd2  
1    yd  = R(η, ε)  0  + 0
dt2
m
zd
fz
g

  
r13 fz
0
1 
  
=
r23 fz  + 0 ,m
r33 fzg(3.47)(3.48)T10
ここで fz は機体座標系の z 軸方向の力, Xd = [xd , yd , zd , ẋd , ẏd , żd , εTd , ηd ] ∈ R はドローンの状態, [ω T , fz ] ∈ R4 をドローンモデルへの入力とする.
このモデルの計画問題は, SO(3) に加えて wr13 fz ≈ r13 fz , wr23 fz ≈ r23 fz , wr33 fz ≈ r33 fz の
項を MICP を用いて近似することで, ダイナミクス (3.48) を線形制約として表現できる. 加
えて, ドローンの幾何的な特性として機体は厚みのない長方形と見なし, [xd , yd , zd ] をドロー
ンの機体の中心の座標とする. 近似したダイナミクス (3.47)(3.48) に対して, 積分手法は拡張オイラー法 [40] を用いた. コスト関数は以下のように設定した:J=X|fz (k)|.

(3.49)k∈T

ドローンがスリットを通ってゴール領域に到達するタスクを考える. Fig.3.5 に, ドロー
ンがタスクを行う環境を示す. ドローンは, スタート領域からスリットを通ってゴール領域に到達する必要があり, スタート, スリット, ゴールの 3 つの領域に対して, LTL 命題
φexist,0 , φexist,s , φexist,g を考える. 各命題はドローンが各エリアに存在する場合に満たされるとする. また, ドローンは機体の各頂点が領域から外れないように移動することとする. このとき, 到達タスクの LTL 命題を次のように定義する.

• ドローンはゴール領域に到達する:
F φexist,g(3.50)

φexist,0 ⇒ ⃝¬φexist,g(3.51)• ドローンはスリットを通る:• ドローンの機体が領域外に出ない:G (φexist,0 ∨ φexist,s ∨ φexist,g )(3.52)

3.6. SO(3) に対する数値実験

29

各パラメーターは ∆T = 0.3, T = 6, m = 1, g = −1, fz ∈ [0, 2], および ω ∈ [−π/2, π/2]3とする. このときの混合整数線形計画問題には 259 個の離散変数を含み, 105 個の変数は LTL
命題に関係し, 残りの 154 個の変数は SO(3) とダイナミクスの双線形項に関係している. ドローンの初期姿勢と位置を変更し, 10 個のケースに対して計画問題を計算した.
このとき, 実行可能解は平均 28.2s, 標準偏差 ±22.7 秒で計算された. 最適解は平均 6000s
以内に収束しなかったが, MIP 最適性ギャップの平均 4.93 × 10−1 %, 標準偏差 ±2.08 × 10−1 %の準最適な解が得られた. そのため, 提案手法を他の最適化手法と組み合わせ, 提案手法を実現可能性チェックまたは初期解として使用することにより, より短時間で (準) 最適解が得られることが推測できる. Fig.3.5 は, 準最適な解の軌道を示している
シミュレーターでの挙動との比較
混合整数線形計画問題を解くことによって得られたシステムへの入力を, 連続時間システ
ム (3.47)(3.48) のシミュレーターに開ループ方式で適用する. ドローンモデルの連続時間シ
ステムのシミュレーションには, Simulink を使用して実装した. Fig.3.6 は, シミュレーショ
ンに適用した入力を示している. 混合整数線形計画問題を解くことによって取得した入力に対して 1 次ホールドを適用し, シミュレーターへの入力を生成した. Fig.3.6 は, 上記の入力に対応するドローンの動作を示している. シミュレーターによって得られたドローンの軌道は, 近似誤差のためにタスクを完了できていない. その原因の一つとして, 計画時に用いた近似モデルの近似精度が挙げられる. Fig.3.7-3.9 に, 混合整数線形計画問題の解として得られた軌道とシミュレーションによって得られた軌道の比較を示す.
また, Fig.3.10 に, 混合整数線形計画問題の解であるオイラーパラメーターの l2 -ノルムと,T
混合整数線形計画問題の解とシミュレーターでの回転行列の差分 ∆R = Rmilp
Rsim をオイ
ラーパラメーターで表現した際の回転量 ∆θa , ∆θ̄a の軌道を示す. ∆θa は混合整数線形計画
問題の解として得られた双線形項を近似する変数から計算された回転行列との差分に対応し,∆θ̄a は混合整数線形計画問題の解であるオイラーパラメーターを SO(3) に射影した値を用いて式 (3.31) より計算した回転行列との差分に対応している. ∆θa , の軌道から, 回転行列については長期的な近似が難しく, これは多くの双線形の近似項を含むためであると考えられる. 一方で, ∆θ̄a の軌道から, 混合整数線形計画問題を行う際に SO(3) への射影の構造を組み込むことができれば, より近似精度の高い計画を行うことができると考えられる. また, 目
標状態をより精度よく追従するには, フィードバック制御器等の機構を導入する必要が考え
られる.

30

第 3 章 混合整数計画問題による軌道最適化

Fig. 3.5: Overview of reaching task of the drone. Left: initial states and workspace. The
yellow rectangle represents initial states of the drone. The green, blue and red areas are theinitial, slit, and goal areas, respectively. Right: the trajectory of a semi-optimal solution.
The drone passes through the slit area with the body in a vertical position.

Fig. 3.6: Left: Comparison of the behavior of a solution and simulator. The green rectangle
represents the simulation of the drone to which inputs obtained by the Mixed-Integer Linear
Problem planning are applied. Right: Input sequences used in the simulator. We applied
the First-order hold to the input obtained by the Mixed-Integer Linear Problem planning.

3.6. SO(3) に対する数値実験

31

Fig. 3.7: Comparison of the Euler parameter states of the solution and simulator. Solid: thesolution. Dashed: simulator.

Fig. 3.8: Comparison of position states of the solution and simulator. Solid: the solution.
Dashed: simulator.

32

第 3 章 混合整数計画問題による軌道最適化

Fig. 3.9: Comparison of velocity states of the solution and simulator. Solid: the solution.
Dashed: simulator.

Fig. 3.10: The trajectory of l2 -norm of the Euler parameter states of simulation and rotation
angles of diﬀerence of rotation matrics: ∆θa and ∆θ̄a

3.7. 拡大システムに対する数値実験

3.7

33

拡大システムに対する数値実験

本節では, 拡大システムに対して提案手法を適用し, 有効性を確認する. 平面移動ロボット
システムに対して, 拡大線形システムを構築し, 与えられた LTL 命題を満たす軌道を混合整数線形計画問題を解くことによって生成する. 本節での計算は, 3.6GHz Intel Core i9 で実行した. 加えて, 全ての混合整数線形計画問題は MATLAB によって実装され, ソルバーとして
Gurobi [41] を用いた. また, 全ての混合整数計画問題の最適性ギャップの上限を 1.0 × 10−4と設定した.

3.7.1

2 次元平面上でのピックアンドプレイスタスク

2 次元平面上でのピックアンドプレイスタスクを考える. ロボットシステムとして, 以下のシステムのダイナミクスを考える:

  
u
cos
θ
x
1
d2   
(3.53)y  =  u1 sin θ  .
2
dt
u2
θ
ここで u1 ∈ [−1, 1] は並進方向の入力加速度, u2 ∈ [− 21 π, 12 π] は回転方向の入力加速度, x, y はロボットの位置, θ はロボットの進行方向とする. ロボットの初期状態は [x, y, θ]T = [0, 0, 0]Tとする. このシステムは初期状態のまわりでテイラー展開した線形システムを考えた場合, 状
態 y が制御不可能な状態になる. そのため, ここでは拡大システムの状態を次のようにとる:ξ = [x, y, θ, ẋ, ẏ, θ̇, sin θ, cos θ, θ̇ sin θ, θ̇ cos θ]T ∈ R10 . このとき, 離散時間拡大双線形システム
は次式で表わされる:




ẋ
O
3,1
 ẏ 
u1 cos θ θ̇ 
O5,1
 u1 sin θ 




+
 ∆t


ξ =ξ+
 O3,1  ∆t +  u2  ∆t +  O 3,1 


O4,1


 θ̇ cos θ  u2 sin θ −θ̇ sin θ
u2 cos θ
O2,1= Aξ ξ + B ξ u +2XHξ,i ξui .

(3.54)i=1

ここで ∆t = 0.1 はサンプル時間とする.
ピックアンドプレイスタスクの LTL 命題は, 前述した数値実験と同様に定義する. ここでは, “いつかオブジェクトが目的領域に存在する”という命題を考える．物体の状態は, 並進
位置と速度を考え, ピッキング動作は, ダイナミクスのモードの変化として表現する. ここでは, ピッキング動作の際に状態のジャンプは発生しないと考える. 加えて, オブジェクトをピッキングした場合, 物体の速度は移動ロボットと同期するとする. また, 行動計画を行うス
テップ数を 30 とする.

34

第 3 章 混合整数計画問題による軌道最適化

開ループ系での評価
まず, 提案手法によって得られた入力を開ループ系に適用し, タスクを達成できるか評価
した. 15 パターンの物体の位置を考え, それぞれの位置について行動計画を行う. 15 個の位
置のパターンを領域 [−0.1, 0.1] × [−0.1, 0.1] 上に格子状に設定する. 混合整数凸計画の双線形項に現れる変数の近似に用いる分割数は, 4 と 8 に設定した．
行動計画問題を混合整数計画問題として解いた結果, 4 分割の場合は, 平均 1.31 秒, 標準偏差 ±0.88 秒で実行可能解が得られた. 8 分割の場合は, 平均 11.30 秒, 標準偏差 ±9.39 秒で実現可能解が得られた. 4 分割, 8 分割ともに, 実現可能な解は得られたが, ほとんどの位置のパターンで 200 秒以内に最適解は得られなかった.
Fig.3.11 左に, 計画によって得られた軌道と開ループ系の x と y の軌道の平均二乗誤差(RMSE) を示す. このとき, 計画によって得られた軌道は最適な軌道ではないため, オブジェ
クトの位置が同じでも 4 分割と 8 分割の場合で軌道が大きく異なる場合がある. Fig.3.11 から, 8 分割では計画によって得られた軌道と開ループ系での軌道の差が小さくなっていることがわかる. この結果は, 分割数を多くすることで混合整数凸計画の精度が向上するためであると考えられる. Fig.3.11 右に計画によって得られた軌道と開ループ系の軌道を示す. 計
画によって得られた軌道は, 8 分割の場合に得られた軌道である. Fig.3.11 で示された軌道と同様に, ほぼ全てのオブジェクトの位置で, 計画によって得られた軌道がタスクを完了させているにもかかわらず開ループ系の軌道はタスクを完了することができなかった.
そこで, 計画によって得られた軌道を参照軌道とみなして, フィードバック制御を行った.
閉ループ系での評価
閉ループ系に対して, 4 分割と 8 分割で行動計画した軌道を参照軌道として用いて制御した際の軌道について評価する. 参照軌道に追従するためのフィードバック制御としてモデル予測制御 (MPC) を用いた.
MPC を拡大双線形系を用いた非線形問題として実装する. この際, MPC の目的関数は, x,y, θ の参照軌道との誤差の重み付け l2 -norm である. そして, 予測ステップ数を 10, MPC におけるサンプリング時間を 0.02, コントローラのサンプリング時間を 0.02 とする. 計画によって得られた軌道 x, y, θ の軌道を参照軌道とする. この非線形問題を CasADi [42] で実装し,ソルバーとして Ipopt を用いた.
Fig.3.11 左に, 計画によって得られた軌道と閉ループ系の x と y の軌道間の RMSE を示す.
開ループ系と比較して軌道間の誤差が減少しており, 特に 8 分割の軌道に対しては追従性が高いことが確認できる. このことから, 提案手法は拡大システムを用いることで, 本来のシステムが実行可能な軌道を生成できていると考えられる. しかし, 場合によっては Fig.3.11 右のように, 軌道追従用の制御器のみではシステムがタスクを達成できない場合がある. しかし, より保守的な計画手法や LTL の情報を利用した制御器を用いることで, より効果的な結果が得られる可能性が考えられる.

3.8. まとめ35

保守的な計画手法として, 計画時にオブジェクトや目的領域を実際より小さく設定することが挙げられる. また, 入力の上限といったシステムのダイナミクスの保守性も考慮する必要がある. また, 先行研究 [43] で提案されているような LTL の情報を活用した制御手法と組み合わせることで, よりタスクを完遂しやすくなることも期待できる.

Fig. 3.11: Left: RMSE between the trajectories of x and y of the planning, the open-loopsystem, and the closed-loop system. Right: The trajectories of planning, open-loop, andclosed-loop systems. The yellow trajectory is a trajectory of the planning obtained in acase of 8 intervals. The blue trajectory is a trajectory of the open-loop system. The pinktrajectory is a trajectory of the closed-loop system. In this case, we see that the trajectories
of both open-loop and closed-loop systems could not complete the task.

3.8

まとめ

本章では, 非線形システムに対して混合整数計画問題による定式化を行うための, MICP や
拡大線形化を用いた近似システムを検討した. MICP を用いることで SO(3) に代表される
ダイナミクスに双線形項や二乗項を含むシステムを線形システムとして表現でき, 混合整数
線形計画問題による定式化が適用できることを示した. また, 拡大線形システムを用いることで, より多くの非線形システムを線形システムとして表現できることや, MICP と組み合わせることで拡大双線形システムに対しても混合整数線形計画問題による定式化が適用できることを示した. その後, MICP, 拡大双線形システムを用いた近似システムに対して混合整数
線形制約問題を解くことで, LTL 命題で表現されたタスクを達成する軌道が生成されることを数値シミュレーションによって検証した.
本章で提案した定式化を用いることで, 計算コストを抑えながら抽象化システムとして非線形システムを採用することができる. これにより, より実際のロボットシステムの特性を反映したシステムを抽象化したシステムとして用いることで, 高レベル制御器にて実用的な36

第 3 章 混合整数計画問題による軌道最適化

軌道を生成することが可能となる. 一方で, 各数値実験での開ループ系での評価でも明らかなように, 近似システムと実際のシステムでの応答には差異がある. 抽象化システムに関して MICP の近似精度を高めることや, 拡大線形システムの状態の選定方法によって, 近似精度の高いシステムを構築することが可能である. 一方で, MICP の近似精度の向上は計算コストの増加を伴うといった問題があり, タスクごとにどのような近似システムを用いるべきかの判断基準は今後の課題である.

第4章

4.1

不確かな外的事象に関する命題への軌道最適化

はじめに前章では, ロボットの状態またはロボットの振る舞いを模した離散入力に依存する時相論
理命題に関するタスクを対象とした. 一方で, 実際のタスクには人間からの指示を受けて行動するといった場面が存在する. この際, 人間からの指示や信号はロボットシステムの状態に依存せず発生する外的事象として考えられる.
これをふまえ, 本章では発生時刻が不確かな外的事象に関する命題を達成するための軌道最適化手法を提案する. まず, 外的事象に関する命題を記述可能な時相論理である Event-based
STL について説明する. その後, 外的事象の発生時刻に関する分布が既知であると仮定し, その分布に基づき, 複数の発生時刻を想定して与えられた命題を達成する軌道を生成することが可能な軌道最適化を非線形計画問題として定式化する. 最後に, 不確かな外的事象に関する命題が与えられた移動ロボットに対する数値シミュレーションにて, 提案手法の有効性を
示す.

4.2

Event-based STL

Event-based STL は STL を拡張した時相論理であり, 解的事象に関する命題を表現することが可能である. Event-based STL の命題 Ψ は下記の様に定義される [44]:φ ::=π | ¬π | φ1 ∧ φ2
α ::=ν | ¬α | α1 ∧ α2
Ψ ::=GI φ | FI φ | φ1 UI φ2 |G(α ⇒ Ψ) | G(φ ⇒ Ψ) | Ψ1 ∧ Ψ2
ここで α は環境命題 ν ∈ AP に関する論理命題である. ν の充足関係は外的事象の発生やアラームやスイッチングといった論理入力などによって決定される. そのため, Event-based
STL で表現された命題をシステムの状態に対して評価する場合, システムの状態として離散

37

38

第 4 章 不確かな外的事象に関する命題への軌道最適化

変数を含む必要がある. Event-based STL の意味論は, STL と同様に下記の様に行われる:(x, t) |= ¬α(x, t) |= α1 ∧ α2(x, t) |= G(α ⇒ Ψ)(x, t) |= Ψ1 ∧ Ψ2

⇔ ¬((x, t) |= α)⇔ (x, t) |= α1 ∧ (x, t) |= α2⇔ ∀t, (x, t) |= ¬α ∨ (x, t) |= Ψ
⇔ (x, t) |= Ψ1 ∧ (x, t) |= Ψ2 .

本章では, 非線形計画問題としての定式化を行うため, 離散変数を用いない外的事象に関する命題のロバストネス関数が必要である. そのため, 本章では Event-based STL 命題に対す
るなめらかなロバストネス関数を定義し, 付加関数として扱う.

4.3

Event-based STL のロバストネス関数

もし環境命題 ν が複数の外的事象やシステムの離散状態に依存する場合, ロバストネス関数の定義域が分離された集合になる. 本章では計算の複雑さを減らすために, 次の仮定を考える:
仮定 1. 外的事象に関する環境命題 ν の真偽はただ一つの外的事象に依存し, 事象の発生は連続変数の値によって記述できる.
このとき, 連続な補助変数を用いたなめからなロバストネス関数を以下のように定義する:ρνi (x, xν , t) = ρνi (xνi (t)) ,(4.1)

ここで, xν = [xν1 , . . . , xνnν ]T ∈ Rnν , xνi ∈ Xνi = [xνi ,min , xνi ,max ] ∈ R, (i = 1, . . . , nν ) は連続
な補助変数, ρνi : Xνi → R はなめらかな関数, xνi ,min と xνi ,max は定数とする. 加えて, 本論
文では以下を仮定する.
仮定 2. 関数 ρν (xν ) は Xν で有界な関数であり, |ρν (xν )| < ϵν , xν ∈ Xν を満たす定数 ϵν ∈ Rが存在する.

Example 2. 関数 ρν (xν ) = xν , Xν = [−0.1, 0.1] を考える. このとき, xν ≥ 0 ならば,ρν (xν ) ≥ 0 かつ ν が発生してる. 一方で, xν < 0 ならば, ρν (xν ) < 0 かつ ν は発生していな
い. 加えて, ϵν = 0.1 は |ρν (xν )| ≤ ϵν を満たす.
このとき, Event-based STL の命題に対するロバストネス関数を以下のように定義できる.

ρ¬α (x, xν , t) := − ρα (x, xν , t),ρα1 ∧α2 (x, xν , t) := min(ρα1 (x, xν , t), ρα2 (x, xν , t)),

(4.2)
(4.3)

ρG(α⇒Ψ) (x, xν , t) :=min {max(ρ¬α (x, xν , t′ ), ρΨ (x, xν , t′ ))} .

t′ ∈[t,T ](4.4)

4.4. 不確かさを持つ外的事象に対する EVENT-BASED STL 命題のための行動計画手法39

4.4

不確かさを持つ外的事象に対する Event-based STL 命
題のための行動計画手法

Event-based STL の命題 Ψ に対する軌道最適化問題を次の非線形最適化問題として定式
化する:
X
min − ρΨ (z) +λk Jk (xk , uk )ZkρΨ (z) ≥ ϵΨ .

s.t. Eq.(2.2),(4.5)

Z は最適化変数で構成されたベクトル (システムの状態, 入力, 補助変数 x0 , . . . , xN , u0 , . . . , uN ,xν,0 , . . . , xν,N ), N は計画を行うステップ数, Jk は軌道の性能に関するコスト関数とし, λk は
それらのコスト関数に関する重みとする. ϵΨ は正の定数とする. 与えられた命題 Ψ のロバ
ストネス関数が正の値となる制約は, 大域的最適解ならばロバストネス関数の値が正の値になると考えられるため, 先行研究ではこの制約が考慮されていない [10]. しかし, 本章では,得られた解が局所的最適解の場合でもロバストネス関数の値が正の値となるように, この制
約条件を陽に最適化問題で考慮することとする.
このとき, 最適化問題 (4.5) の解は αi が常に発生しなくても実行可能解になる可能性がある. そのため, αi が軌道上で少なくとも 1 度は発生することを仮定し, 次の拘束条件を加える:
maxj∈[0,...,N ]ραi ,j (z) ≥ ϵαi ,(4.6)

ここで ϵαi は小さな正の定数とする.
本章では, 外的事象の発生時刻が正確には判明していないと仮定し, 次の仮定を置く:
仮定 3. 外的事象 ν の発生時刻に関する分布が pν (t) で与えられている.
外的事象の発生時刻に関する分布に基づいて, 不確かさを持つ外的事象に対する軌道最適化問題を次のように定式化する:
XX
min − ρΨ (z) −λpνi (j)L(ρνi (xνi ,j ))
(4.7)
Z

i

+

X

j

λk Jk (xk , uk )ks.t. Eq.(2.2), (4.6),ρΨ (z) ≥ ϵΨ .

(4.8)

ここで λpνi (t) ≥ 0 は発生に関する分布 pνi (t) に依存した重みとする. L : R → R はステー
ジコストであり, 単調増加関数とする (例: L(x) = 2/(1 + exp(−x)) − 1).
仮定 2 より, ステージコスト L を含む第 2 項は, νi が発生した場合に小さな値となる. 重み λpνi は, pνi (t) が大きな値をとる時刻では大きな値に設定することで, その時刻での νi の発生に対して大きな重みをつける. その結果として, 最適化問題 (4.8) の解は, 分布 pνi (t) に従って命題 Ψ を満たす外的事象 νi に対する複数のシナリオを考慮した軌道が生成される.

第 4 章 不確かな外的事象に関する命題への軌道最適化

40

得られた解が複数のシナリオを考慮できる理由は, 命題 Ψ 内に現れる外的事象に関する命
題は G[a,b] (α ⇒ Ψ) の形のみであり, それぞれの外的事象の発生に対してこの命題が満たされる必要があるためである. そのため, 提案手法によって外的事象の発生に関する複数のシ
ナリオに対して命題 Ψ を満たすロバストな軌道を生成することができる.

4.5

数値実験

この章では, 2 つの数値実験に提案手法を適用する. まず, 1 次元の通過タスクに提案手法を適用する. 次に, より複雑なタスクである 2 次元の下げ膳タスクに提案手法を適用する. 最後に, 提案手法の十分性と完全性に関して考察を述べる.
本章での全ての計算は, 3.6GHz Intel Core i9 で実行した. 全ての最適化問題は MATLAB
上で CasADi [42] によって実装し, ソルバーとして Ipopt [45] を用いた.

4.5.1

1 次元の通過タスク

1 次元システムの通過タスクとして次の命題を考える: “いつかロボットはゴールエリアに到達し, かつ常にロボットが通過エリアに存在できるのは許可信号 αpermit,t が発生した後の特定の時間のみである” (Fig.4.1).
この命題を以下の Event-based STL 命題として定義する:
Ψpas = F[0,T ] (φexist )∧ G(αpermit,t ⇒ (G[0,t) ¬φcross ∧ G[t+T ′ ,T ] ¬φcross )),ここで φexist , φcross は命題 “ロボットがゴール領域にいる,” “ロボットが通過領域にいる”
を示している. 加えて, αpermit,t は時刻 t でのロボットの通過エリアでの存在を許可する外的事象とする. ロボットのダイナミクスとして 1 次元の 2 重積分器を考え, システムの状態として位置 xp ∈ [−2.5, 2.5] ⊂ R と速度 xv ∈ [−4, 4] ⊂ R, 制御入力として加速度 u ∈ [−1, 1] ⊂ Rとする.
各命題に対して次のロバストネス関数を定義する:

ρφexist (x) = min(xp − 2, −xp + 2.5, xv + 0.1, −xv + 0.1),
ρφcross (x) = min(xp − 1, −xp + 2),ραpermit,t (xν ) =xαpermit (t),また, ロボットの初期状態として [xp , xv ] = [0, 0], 命題に含まれる時間制約を T = 4, T ′ =2, 計画を行うステップ数を N = 40 とする. ステージコストを L(xαpermit ) = 2/(1 +exp(−κL xαpermit )) − 1, κL = 30 とし, 目的関数は次式とする:−ρΨpas (z) −

X
j

λp α

permit(j)L(xαpermit ,j ) +Xjλu u2j ,(4.9)

4.5. 数値実験

41

ここで λu = 10−6 とする.
次の 3 つのケースに対して, 軌道最適化を行った:

I) αpermit が計画ステップ全体で一様に発生する,II) αpermit が計画ステップの前半で一様に発生する,
III) αpermit が計画ステップ後半で一様に発生する.
ケース I) では, λpαpermit(j) = 10−1 (j = 0, . . . , N ) とし, 軌道最適化によって得られた軌道を Fig.4.2 に示す. 黄色の破線は xαpermit の軌道を示しており, 軌道上に発生している

αpermit は Ψpas を満たしていることがわかる. ケース II) では, λpα0, . . . , ⌊N/2⌋), λpαpermitpermit(j) = 10−1 (j =(j) = 0 (j = ⌊N/2⌋ + 1, . . . , N ) とし, 軌道最適化によって得られた軌道を Fig.4.3 に示す. xαpermit の軌道は, ケース I) の軌道を時間方向前方にシフトしたような軌道になっている. ケース III) では, λpαpermit(j) = 10−1 (j = 0, . . . , ⌊N/2⌋), λpαpermit(j) =0 (j = ⌊N/2⌋ + 1, . . . , N ) とし, 軌道最適化によって得られた軌道を Fig.4.4 に示す. ケース
II) 同様に, xαpermit の軌道はケース I) の軌道を時間方向後方にシフトし, 終端時間 T の影響によって切り取られたような軌道になっている.
これらの結果から, それぞれのケースに対して, 得られた軌道が複数の αpermit,t の発生シ
ナリオに対して命題 Ψpas を満たしていることがわかる. そのため, αpermit,t の発生時刻が不確かな場合でも, 生成された軌道は多くのシナリオにおいて与えられた命題を満たすことがわかる. この数値実験においては, それぞれのケースに対して, 軌道上に現れず命題 Ψ を満たすような αpermit の発生時刻は存在しなかった.
この数値実験では, 最適化問題を解く際に解の最適性を向上するために初期解を変更をしながら複数回計算を行った. この場合でも, 解は 3 回以内の最適化問題の計算で収束し, 合計の計算時間は 5 秒以内に収まっている.

Fig. 4.1: The case of the passage task for the 1-dimensional robot

42

第 4 章 不確かな外的事象に関する命題への軌道最適化

Fig. 4.2: The trajectories of the passage task in case I).

Fig. 4.3: The trajectories of the passage task in case II).

4.5. 数値実験

43

Fig. 4.4: The trajectories of the passage task in case III).

第 4 章 不確かな外的事象に関する命題への軌道最適化

44

4.5.2

2 次元の下げ膳タスク

2 次元の下げ膳タスクとして次の命題を考える: “いつか, ロボットはシンク領域に到達
し, かつロボットは各テーブルの客に呼ばれたらテーブルへ到達する.” この命題を以下のEvent-based STL 命題として定義する:
Ψsrv =F[0,T ] (φSink )∧ G(αcallA ,t ⇒ (F[t,t+T ′ ] φtableA ))(4.10)∧ G(αcallB ,t ⇒ (F[t,t+T ′ ] φtableB )),ここで, φSink , φtable i (i = 1, 2) は命題 “ロボットがシンク領域に存在する,” “ロボットがテーブル i 領域に存在する” を表している. また, αcalli,t (i = A, B) は時刻 t で “ロボットがテーブル i に呼ばれている” ことを示す外的事象とする. ロボットのダイナミクスは 2 次元の 2 重積分器とし, システムの状態はロボットの位置 xp = [xpx , xpy ]T ∈ [0, 3] × [0, 3] ⊂ R2 ,速度 xv = [xvx , xvy ]T ∈ [−0.5, 0.5] × [−0.5, 0.5] ⊂ R2 とし, 制御入力は加速度 u = [ux , uy ]T ∈[−1, 1] × [−1, 1] ⊂ R2 とする.
それぞれの命題に対して, 次のロバストネス関数を定義する:

ρφSink (x) = min(xpx − 2, −xpx + 2.5, xpy − 2, −xpy + 2.5, −||xv ||2 + 0.12 ),ρφtable (x) = min(xpx − 1, −xpx + 1.5, xpy , −xpy + 0.5, −||xv ||2 + 0.32 ),A

ρφtable (x) = min(xpx − 0.5, −xpx + 1, xpy − 1.5, −xpy + 2, −||xv ||2 + 0.32 ),B

ραcall ,t (x) = xαcall (t),AAραcall ,t (x) = xαcall (t).
BB

また, ロボットの初期状態を [xT , vT ] = [0, 0, 0, 0], 命題に含まれる時間制約を T = 30, T ′ =4.5, 計画を行うステップ数を N = 100 とする. ステージコストを L(xα) = 2/(1 +exp(−κL xαcall )) − 1 (i = A, B), κL = 30 とし, 目的関数は次式とする:

calli

i

−ρΨsrv −

XX
i

j

λp α

calli

(j)L(xαcall ,j ) +iXλu ||uj ||2 ,j

ここで λu = 10−6 とする.
次の 2 つのケースに対して, 軌道最適化を行った: αcallA は即座に発生し,i) αcallB は行動計画期間前半に一様に発生する,ii) αcallB は行動計画期間後半に一様に発生する.
重みを λpαcallA(j) = 10−1 × 5 exp(−j) (j = 0, . . . , N ) と設定する.

(4.11)

4.5. 数値実験

45

ケース i) では, λpαcallB(j) = 10−1 (j = 0, . . . , ⌊N/2⌋), λpαcallB(j) = 0 (j = ⌊N/2⌋ +1, . . . , N ) とし, 軌道最適化によって得られた軌道を Fig.4.5, 4.6 に示す. ロボットが命題を達成し, テーブル A, テーブル B, シンク領域の順番で到達していることがわかる.
ケース ii) では, λpα(j) = 10−1 (j = 0, . . . , ⌊N/2⌋), λpα(j) = 0 (j = ⌊N/2⌋ +
callB

callB

1, . . . , N ) とし, 軌道最適化によって得られた軌道を Fig. 4.5, 4.7 に示す. ケース ii) の場合
でもロボットは命題を達成しており, 行動としてはテーブル A, シンク, テーブル B 領域の順番で到達していることがわかる. 加えて, 得られた軌道から, αcallA ,t と αcallB ,t の発生に関する複数のシナリオにおいて命題 Ψsrv を満たす軌道が得られたことがわかる.
提案手法の有効性を確認するために, 外的事象 αcallA ,t と αcallB ,t の発生時刻として, ある
固定した一つの時刻を仮定し軌道最適化を行った際に得られた軌道との比較を行った. 仮定
した発生時刻は, αcallA ,t は t = 0, αcallB ,t は各ケースで発生しうる時間区間の中央の時刻とした.
比較のために, αcallA ,t と αcallB ,t の発生シナリオをそれぞれの分布に従って生成し, 各シナリ
オにおいて軌道が命題 Ψsrv を満たすか評価した. それぞれの分布は pα(t) ∼ exp(−1/0.3t),
call
ケース i) では pαA(t) ∼ 1, (t ∈ [0, T /2]), pαcall (t) ∼ 0, (t ∈ (T /2, T ]), ケース ii) では
call
B

B

pαcall (t) ∼ 0, (t ∈ [0, T /2)), pαcall (t) ∼ 1, (t ∈ [T /2, T ]) とした. 10000 通りのシナリオをB

B

用意し評価を行った結果, 命題を満たすことができたシナリオの割合を Table.4.1 に示す. 結果から, ケース ii) において提案手法によって命題を満たすことのできる発生シナリオが増加していることが確認できる.

Case
ﬁxed occurrence time
proposed methodi)76.21%
76.21%

ii)
65.94%
86.33%

Table 4.1: Comparison of trajectories of the proposed method and the optimization withﬁxed occurrence time
しかし, この数値実験では軌道上に現れず命題 Ψ を満たす αcalli ,t (i = A, B) の発生時刻が存在する. このことについては, 次の小節にて議論を行う.
この数値実験では, 最適化問題を解く際に解の最適性を向上するために初期解を変更をしながら複数回計算を行った. この場合でも, 解は 3 回以内の最適化問題の計算で収束し, 合計の計算時間は 4 分以内に収まっている.

4.5.3

提案手法の十分性

この節では, 提案手法で得られる解が外的事象の発生時刻に対して十分性を持つことを示す. ここで, 十分性とは解に現れる外的事象の発生時刻のなかで, 与えられた命題を満たさない時刻が存在しないことである. 提案手法では, 解として得られた軌道内の各 νi の発生に対

46

第 4 章 不確かな外的事象に関する命題への軌道最適化

して, 解として得られた軌道は与えられた命題を満たしている. しかし, 本論文で用いている
max 関数のなめらかな近似が under-approximation ではないため, 実際の最適化計算によって得られた解は十分性を満たさない可能性がある. この問題に関しては under-approximationであるなめらかな近似を用いることで解決することができる [46]. 一方で, 提案手法で得られる解は完全性を持たない. なぜならば, 提案手法の解として得られる軌道上に現れず命題 Ψを満たすような νi の発生時刻が存在するためである. 提案手法が完全性を満たさない理由のひとつは, 現実には提案手法の大域的最適解を得ることが難しいことであると考えられる.

Fig. 4.5: The trajectories of the service task in 2-dimensional space for each cases. Green:case i). Orange: case ii).

4.5. 数値実験

47

Fig. 4.6: The trajectories of the service task in case i). We can see that the robots visitedthe table A, the table B, and the Sink area in that order.

Fig. 4.7: The trajectories of the service task in case ii). We can see that the robots visitedthe table A, the Sink, the table B area in that order.

第 4 章 不確かな外的事象に関する命題への軌道最適化

48

4.6

まとめ

本章では, 不確かな外部事象に関する Event-based STL 命題のための新しい非線形最適化
による行動計画手法を提案した. まず, Event-based STL 命題のなめらかなロバストネス関数を定義した. 次に, 不確実な外的事象を含む Event-basd STL 命題のための軌道最適化手法を提案した. 最後に, 数値シミュレーションにより本手法の有効性を実証した.
本章では, 外的事象の発生に関する分布が事前に得られていることを仮定している. しかし, 実際にはタスクの実行中に分布が変化することが考えられる. そのため, 分布を観測情報を用いて更新することで, よりロバストな軌道を生成し, 現実的なケースに対応することが可能であると考えられる.

第5章

5.1

時間制約をもつ信号時相論理に対
する制御

はじめに

時相論理で表現されたタスクを達成するためには, 抽象化したシステムで生成した軌道を基に実際のシステム上で制御を行い, 動作を実行する必要がある. 加えて, 実際のロボットシ
ステム上で制御を行う際, タスクを達成するための時間的な制約や状態に関する制約条件を違反することなく動作を行う必要がある.
これをふまえ, 本章では時間制約をもつ様相作用素を含む信号時相論理を達成するための制御バリア関数を用いた制御手法を提案する. まず, 本章で考える時間制約を持つ STL と,それに対応する時間オートマトンについて説明する. 次に, 本章で提案する制御手法を説明する準備として, 零化制御バリア関数に関するいくつかの定義と時間軸変換について述べる.
その後, 本章で提案する制御器の構造について説明し, 有限時間で目的の不変集合へ遷移させるための制御バリア関数を拡張した制御器を提案する. 加えて, 関数の値によって進行速度が変化する仮想時間を定義し, 時間オートマトン上の各状態遷移に対応する命題を達成するための制御器を提案する. 最後に, 移動ロボットに対する数値シミュレーションにて, 提案
手法の有効性を示す.

5.2

準備

5.2.1

対象システム

本章では, 以下の入力アフィンシステムを対象とする.

ẋ = f (x) + g(x)u,(5.1)

ここで f (x) : Rnx → Rnx , g(x) : Rnx → Rnx ×nu は局所リプシッツ連続とする.

5.2.2

時間オートマトン

STL 論理式 (2.4) で記述される各命題に対して, 対応した時間オートマトンを考えることができる [29]. 時間オートマトンは有限オートマトンの拡張であり, 事象と時間経過の両方による状態遷移をもつモデルである (Fig.5.1).
49

第 5 章 時間制約をもつ信号時相論理に対する制御

50

定義 3. (時間オートマトン [47]) 時間オートマトン A は組 A = (Q, Q0 , C, Inv, E, F, AP) によって定義される. ここで Q はロケーションの集合, Q0 ⊆ Q は初期ロケーションの集合, C
はクロックの集合, Inv : Q → Ξ(C) はクロックに関する関数, Ξ(C) はクロックに関する制約の集合, E ⊆ Q × Q × 2AP × Ξ(C) × 2C はエッジの集合, F ⊆ Q は受理ロケーションの集合とする.
時間オートマトンの状態 (q, c) はロケーション q ∈ Q とクロックの値 c の組として定義される.
また, 本章では STL の原子論理命題の真偽はロボットシステムの状態を引数とするロバストネス関数によって決定されると仮定する. このとき, ロボットシステムの軌道が与えられたとき, 軌道上の各時刻の状態に対して対応する時間オートマトンの状態が存在し, 軌道に対応するオートマトンの上のパスを計算することができる.

Fig. 5.1: An example of a timed automaton for STL

5.2.3

零化制御バリア関数

零化制御バリア関数ついて定義する準備として, 以下の閉集合を定義する.

Cδ = {x ∈ Rnx : b(x) ≥ δ},(5.2)∂Cδ = {x ∈ Rnx : b(x) = δ},(5.3)

Int(Cδ ) = {x ∈ Rnx : b(x) > δ},(5.4)

ここで b : Rnx → R は微分可能な連続関数, δ ≥ 0 は制約条件 b(x) ≥ 0 に対するマージンとする. ただし δ = 0 のとき, Cδ の代わりに C と記述する.
次に, 拡張クラス K 関数は以下のように定義される.

5.3. 仮想時間を用いた ϵ-零化制御バリア関数 (ϵ-ZCBFS)51

定義 4. (拡張クラス K 関数 [48]) 連続関数 ζ : (−l1 , l2 ) → (−∞, ∞), l1 , l2 > 0 が厳密に単調増加し ζ(0) = 0 を満たすとき, ζ は拡張クラス K 関数に属する.
ここで, 入力アフィンシステム (5.1) に対して, 零化制御バリア関数 (ZCBF) を以下のように定義する.
定義 5. (零化制御バリア関数 [48]) 微分可能な連続関数 b : Rnx → R に対して式 (5.2)-(5.4)
で定義される集合 C ⊂ Rnx を考える. このとき下記の不等式を満たす拡張クラス K 関数 ζが存在するとき, b は集合 D, C ⊆ D ⊂ Rnx に対する ZCBF である:

sup[Lf b(x) + Lg b(x)u + ζ(b(x))] ≥ 0, ∀x ∈ D,

(5.5)

u∈U

ZCBF b が与えられたとき, x ∈ D に対して集合 Uzcbf (x) を下記の様に定義する:
Uzcbf (x) = {u ∈ U | Lf b(x) + Lg b(x)u + ζ(b(x)) ≥ 0}.

(5.6)

もし b が D に対する ZCBF ならば, 任意のリプシッツ連続な制御器 u : D → U , u(x) ∈Uzcbf (x) によって C は不変集合となる.

5.2.4

時間軸変換

時間軸変換は非線形システムの制御や解析に用いられている [49]. 仮想時間を用いること
で, システムを扱いやすい構造に変換することや望ましい特性を付与することができる. 仮
想時間軸 τ ∈ T を次のように定義する:

dt= s(x, t, τ ), τ (t0 ) = τ0 ,
dτ

(5.7)

ここで s(x, t, τ ) は時間軸関数と呼び, 次の不等式を満たすと仮定する:

0 < s(x, t, τ ) < ∞.

(5.8)

新しい時間軸を用いてシステム (5.1) は次のように表現できる:dx= s(x, t, τ ) (f (x(t)) + g(x(t))u(t)) .
dτ

5.3

(5.9)

仮想時間を用いた ϵ-零化制御バリア関数 (ϵ-ZCBFs)

Fig. 5.2 に本章の提案手法の概要を示す. 提案手法は参考文献 [50] の制御手法と同様の構成をしている. 参考文献 [50] との大きな違いは, 制御器の設計方法である. 時間制約をもつ
STL 命題を達成するための CBF の設計手法は様々な手法が提案されている [50, 51]. 参考
文献 [51] では, CBF を時間を引数とする関数として定義することで, 命題の持つ時間制約に対応している. 参考文献 [50] では, CBF を時間を引数とする関数として定義した上で, 有限52

第 5 章 時間制約をもつ信号時相論理に対する制御

Fig. 5.2: Overview of proposed method in Chapter 5. First, we consider a timed automatonof given STL task. We assume that we can obtain an accepting run (green run) of the timed
automaton. We convert the accepting run to a new STL speciﬁcation (blue underline) which
has STL propositions (pink underline) corresponding to each transition between locations of
the accepting run (orange enclosed parts). Then, we set ϵ-ZCBFs and the virtual time fromthe new STL speciﬁcation. The proposed controller controls a system and ﬁnally achieve
given STL task.
時間整定が可能な CBF を用いて制御を行ってる. 一方で, 本章では, 後述するように時不変な関数である ϵ-ZCBF を定義し, 制御を行うことで命題の持つ時間制約に対応してる. また,
CBF を設計する際のロバストネス関数も異なっており, 参考文献 [50, 51] では遷移に関する
2 つの命題に関する softmin 用いた関数に基づいて設計しているの対して, 本章の手法では各命題に対して CBF を設計することで, 後述するように仮想時間を導入し, 解の消失のリスクを低減できる. そのため, 本章の手法では参考文献とは異なり, 新たな時間関数を定義する必要がなく, 加えて, 仮想時間を導入した制御器を設計できる.
提案手法では, まず与えられた STL に対する時間オートマトンを考える. 本章では, 時間
オートマトンに対して受理されるパスが, 既存の手法によって得られると仮定する [13, 52].
得られた受理されるパスに対して, パス上の時間オートマトンのロケーション間の遷移条件をそれぞれ新しく STL を用いて記述する. 実際のシステム上で新しく生成した STL 命題を達成するように制御を行い, 各ロケーション間の遷移を達成することで本来のタスクを達成することができる.
本節では, 提案手法の各要素について説明する. まず ϵ-ZCBF を定義し, ϵ-ZCBF を用いた制御器が, 有限時間で状態を目的の集合に遷移させることを示す. 次に, 時間制約を緩和する
ための時間軸変換について説明する. 提案する時間軸変換によって, 関数の値と時間に依存5.3. 仮想時間を用いた ϵ-零化制御バリア関数 (ϵ-ZCBFS)53

して速度が変化する仮想時間を定義する. その後, ϵ-ZCBF と仮想時間を組み合わせ, 仮想時
間上で ϵ-ZCBF を用いた制御器が, 指定された時間制約を守りながら状態を目的の不変集合に遷移させることを示す. 最後に, 提案方法を STL で記述されたタスクに適用する方法を示す. この際, 仮想時間上で ϵ-ZCBF を用いた制御器を実装することにより、達成するべきロケーション間の遷移に関する制約を緩和することで, 常に安全性に関する命題を満たすこと
を示す.

5.3.1

ϵ-零化制御バリア関数

状態集合に到達するタスクを考えた際, そのタスクに時間制約があるならば, 状態を有限時間内に目的の不変集合に収束させる必要がある. 時間制約がある到達タスクを達成するための制御手法として ϵ-ZCBF を提案する. ϵ-ZCBF を次のように定義する.
定義 6. (ϵ-零化制御バリア関数) 動的システム (5.1) と式 (5.2)-(5.4) で定義される微分可能な連続関数 b : X → R に関する集合 C ⊂ Rnx を考える. もし定数 ϵ, ϱϵ が存在し, 以下の不等式を満たすならば b は集合 D, C ⊆ D ⊂ Rnx に対する ϵ-ZCBF である:

sup[Lf b(x(t)) + Lg b(x(t))u + ϱϵ (b(x(t)) − ϵ)] ≥ 0,

(5.10)u∈U

加えて, 定数 ϵ, ϱϵ は次の条件を満たす:

bmax > ϵ > 0, ϱϵ > 0,

(5.11)

ここで bmax = maxx∈X b(x).

ϵ-ZCBF b が与えられたとき, 状態 x に対して不等式 (5.10) を満たす入力の集合を Uϵ (x) ={u ∈ U | Lf b(x) + Lg b(x)u + ϱϵ (b(x) − ϵ) ≥ 0} とする. 次の補題にて Uϵ の性質を述べる.
補題 1. 動的システム (5.1) と式 (5.2)-(5.4) で定義される微分可能な連続関数 b : X → R に関する集合 C ⊂ Rnx を考える. もし b が集合 D ⊇ C に対する ϵ-ZCBF ならば, 任意のリプ
シッツ連続な制御器 u : D → U , u(x) ∈ Uϵ (x) は有限時間 0 < T ∗ < ∞ で状態を集合 C に遷移させる: x(T ∗ ) ∈ C.
このとき, 到達時間 T ∗ は, もしシステムの初期状態が x(0) ∈ D\C ならば, T ∗ = ϱ1ϵ ln( −b(x(0))+ϵ)ϵ∗∗である. x(0) ∈ C ならば T = 0 である. さらに, システムの状態は, 全ての時刻 t ≥ T でC に存在する.

Proof. 定義 6 から, ∀x ∈ D\Int(C), b(x) ≤ 0 であり b(x) − ϵ ≤ 0 となる. そのため, Uϵ の定d義から ∀x ∈ D\Int(C), ∀t ∈ R+ , dt(b(x(t)) − ϵ) ≥ −ϱϵ (b(x(t)) − ϵ) > 0 が成り立つ. このとき, ∀t に対して,
Z td(b(x(τ )) − ϵ)dτb(x(t)) − ϵ = b(x(0)) − ϵ +0 dτ≥ e−ϱϵ t (b(x(0)) − ϵ),54

第 5 章 時間制約をもつ信号時相論理に対する制御) で, b(x(T ∗ )) ≥ e−ϱϵ T (b(x(0)) − ϵ) + ϵ = 0を満たす. さらに, 時刻 T ∗ = ϱ1ϵ ln( −b(x(0))+ϵ
ϵ
かつ, ∀x ∈ ∂C, ḃ(x) > 0 が成り立つ. 加えて, 南雲の定理から, 集合 C は不変集合となる [53] [54].
∗実際のタスクでは, 到達時間 TC は制約条件として事前に与えられている. そのため, 時間
制約を満たすように定数 ϵ, ϱϵ を以下のように決定する.
補題 2. フィードバック制御器 u(x) ∈ Uϵ (x) と到達時間 TC ∈ R, 定義 6 で定義された入力の集合 Uϵ (x) を考える. もし b が ϵ-ZCBF で定数 ϵ, ϱϵ が次の不等式 (5.12) を満たすとき, 制御器 u(x) は状態を式 (5.2)-(5.4) で定義された b に関する集合 Cδ に有限時間 T ∗ ,0 < T ∗ ≤ TC < ∞ で遷移させる: x(T ∗ ) ∈ Cδ . さらに, システムの状態は全ての時刻 t ≥ T ∗で Cδ に存在する.

e−ϱϵ T (b(x0 ) − ϵ) + ϵ ≥ δ, ϵ ≥ δ.

(5.12)

Proof. ϵ, ϱϵ が不等式 (5.12) を満たすとき, 補題 1 の証明から, b(x(T )) ≥ e−ϱϵ T (b(x0 )−ϵ)+ϵ ≥δ が時刻 TC で成立. そのため, x(T ∗ ) ∈ ∂Cδ を満たす T ∗ ≤ TC が存在し, システムの状態はT ∗ で Cδ に遷移される. 加えて, ∀x ∈ ∂Cδ , ḃ(x) > 0 から, 補題 1 と同様に集合 Cδ は不変集合
となる.

5.3.2

関数値に基づく時間軸変換

最適化問題を解く際, 複数の制約条件が存在するとき, 解が消失する場合がある. もし制御入力を計算するための最適化問題の解が得られない場合, システムを制御することが不可能になる. そのため, 時間軸変換及び仮想時間を導入し, 入力に関する制約を時間制約の観点から緩和することで解の消失のリスクを低減する. ここでは, 制御器に導入する仮想時間の定
義とその特性について述べる. 関数 ρ に対して仮想時間を次のように定義する.
定義 7. (関数値に基づく仮想時間) 微分可能な連続関数 ρ : X → R と到達時間 TC を考える.
∀t ∈ [t0 , TC ), ∞ > ρ(x(t)) > 0 を仮定する. このとき, ρ(x) に関する仮想時間 τ を以下のように定義する:dt= s(x, t, τ ) := s1 (x) · s2 (t, τ ),dττ (t0 ) = t0 ,
 


1
ρ(x) , 0 + 1,
s1 (x) = max − lnβTC − ts2 (t, τ ) =, t0 ≤ t < T C
TC − τ + ∆ τ(5.13)(5.14)(5.15)(5.16)

ここで, β は制約を緩和する領域に関わるパラメーター, ∆τ > 0 は仮想時間の進行速度に関わるパラメーターとする.

5.3. 仮想時間を用いた ϵ-零化制御バリア関数 (ϵ-ZCBFS)

55

1
時刻 t に対する仮想時間 τ の進行速度は dτ= s(x,t,τで定義される. もし s(x, t, τ ) > 1 な
dt)
らば, τ は t より遅く進み, 1 > s(x, t, τ ) > 0 ならば, τ は t より速く進む. もし s(x, t, τ ) = 1
ならば, τ は t と同じ速度で進む.
仮想時間 τ の時間微分は 2 つの要素で構成されている: 制約を緩和する項 s1 (x) と時間を早送
りする項 s2 (t, τ ) である. もし x ̸∈ {x|ρ(x) < β} ならば, s1 (x) = 1 かつ s(x, t, τ ) = s2 (t, τ ).
もし x ∈ {x|ρ(x) < β} ならば, s1 (x) > 1 であり, 加えて s2 (t, τ ) > 1/s1 (x) ならば,
s(x, t, τ ) > 1 であり, τ は t より遅く進む. 加えて, τ が t に対して遅れるほど, s2 (t, τ ) は小さくなる. そのとき, s(x, t, τ ) < 1 であれば, τ は t より速く進む.
仮想時間 τ について, 次の補題が成立する.

補題 3. 時間 t および定義 7 で定義された仮想時間 τ , パラメーター ∆τ > 0 と到達時間
TC > 0 を考える. このとき, 任意の実数 στ , TC + ∆τ > στ > 0 に対して, ある時刻 T̃C ,TC > T̃C が存在し, 時刻 T̃C で τ (T̃C ) ≥ TC + ∆τ − στ が成立する.

Proof. まず, 時間区間 t ∈ [t0 , TC ) で τ < TC + ∆τ を満たすことを示す. 変数分離法により τ1の時間微分 dτ= s1 (x(t))sを τ (t0 ) = τ0 < TC + ∆τ のもとで τ について解き, t の関数とし
dt
2 (t,τ )て次式を得る.

Z t
−1
′
dt
(5.17)
τ (t) = TC + ∆τ − C1 exp′′
t0 s(x(t ))(TC − t )= TC + ∆τ − C1 Θ(t),ここで C1 = TC + ∆τ − τ0 は積分定数, Θ(t) = exp(5.18)
Rt−1
dt′
t0 s(x(t′ ))(TC −t′ )とする. このとき,τ (t⋆ ) ≥ TC + ∆τ かつ t⋆ < TC を満たす t⋆ が存在すると仮定する. そのとき, 以下の関係が成り立つ;−C1 Θ(t⋆ ) ≥ 0.

(5.19)

しかし, −C1 < 0 かつ Θ(t) > 0 が任意の t ∈ [t0 , TC ) で成り立つため, −C1 Θ(t) < 0 が成立
する. したがって背理法により, τ (t) < TC + ∆τ が t ∈ [t0 , TC ) で成立.
次に, 時間区間 t ∈ [t0 , T̃C ], T̃C < TC における τ の軌道を考える. ここで, τ̄ = τ − TC − ∆τを定義する. dτ
は時間区間 t ∈ [t0 , TC ) でリプシッツ連続であるため, t ∈ [t0 , T̃C ] で τ の解がdt存在する. そのため, t ∈ [t0 , T̃ ] で τ̄ の解が存在し τ̄ < 0 が成り立つ. 加えて, [t0 , T̃C ] でのシス
1
1
1
テムの軌道に対する s1 (x(t))の最小値 c = mint s1 (x(t))
を考える. 定義 7 から, dτ̄= s1 (x(t))sdt
2 (t,τ̄ )は下記の様に記述できる:

dτ̄
1
=−
τ̄
dt
s1 (x(t))(TC − t)


 
1
1
c
τ̄ + −− c τ̄
=−TC − tTC − t s1 (x(t))


 
c
1
1
=−
τ̄ + θ(t, τ̄ ), θ(t, τ̄ ) = −− c τ̄ .
TC − tTC − t s1 (x(t))(5.20)

第 5 章 時間制約をもつ信号時相論理に対する制御

56
さらに, 新たな仮想時間 τ̂ を考える,

dτ̂
c
=−
τ̂ + θ(t, τ̄ ).
dt
TC − t(5.21)

このとき, eτ = τ̄ − τ̂ の時間微分は,

deτ
dτ̄
dτ̂
=
−
dt
dt
dt


c
c
=−
τ̄ + θ(t, τ̄ ) −τ̂ + θ(t, τ̄ )TC − tTC − tc=−eτ .
TC − t(5.22)

よって, eτ の軌道は,eτ = Ce (TC − t)c ,(5.23)

0 )−τ̂ (t0 )は積分定数. さらに, τ̂ の軌道は,ここで, Ce = τ̄ (t(TC −t0 )cZ tτ̂ (t) = (TC − t)

c


1
′
′
θ(t , τ̄ )dt + C2 ,′ c
t0 (TC − t )(5.24)

1≥ c が成り
ここで, C2 = (Tτ̂C(t−t00) )c は積分定数. t ∈ [t0 , T̃C ] で τ̄ < 0, TC1−t > 0 かつ 1 ≥ s1 (x(t))
立つため, 下式を得る




1
1
−
(1 − c) τ̄ ≥ θ(t, τ̄ ) ≥ −(c − c) τ̄TC − tTC − t1(1 − c) τ̄ ≥ θ(t, τ̄ ) ≥ 0.
(5.25)−TC − t式 (5.24) と不等式 (5.25) から, 次の不等式を得る:

Z t
1
c
′
τ̂ (t) = (TC − t)θ(t , τ̄ ) + C2
′ c
t0 (TC − t )

Z t
1
c
′
≥ (TC − t)
· 0dt + C2
′ c
t0 (TC − t )= C2 (TC − t)c .

(5.26)

このとき, t ∈ [t0 , T̃C ] で τ̂ (t) ≥ C2 (TC − t)c が成り立つ. 式 (5.23) から, t ∈ [t0 , T̃C ] で 0 >τ̄ (t) ≥ (Ce + C2 )(TC − t)c かつ Ce + C2 = (Tτ̄C(t−t00) )c が成立. よって, 全ての στ に対して, ある T̃C が存在して, (Ce + C2 )(T − T̃C )c ≥ −στ かつ TC + ∆τ > στ > 0, TC > T̃C を満たす.
従って, 全ての στ , TC + ∆τ > στ > 0 に対して, ある時刻 T̃C < TC が存在し, t = T̃C のとき,
τ (t) ≥ TC + ∆τ − στ を満たす.

定義 7 と補題 3 から, t ∈ [t0 , TC ) で s(x, t, τ ) は条件 (5.8) を満たす. この補題の逆を考えることで, 次の補題を得る.

5.3. 仮想時間を用いた ϵ-零化制御バリア関数 (ϵ-ZCBFS)57

補題 4. 時間 t, および 定義 7 で定義された仮想時間 τ と到達時間 TC > 0 を考える. このとき, ある時刻 t < TC で τ = TC が成立する.

Proof. 補題 3 より, t = T̃C < TC のとき, τ (t) ≥ TC + ∆τ − στ > TC を満たす στ と T̃C が存在する. そのため, στ = ∆τ のとき, t = T̃C < TC で τ ≥ TC を満たす. 加えて, 定義 5 より,
τ の時間微分の値は常に正の値である. したがって, τ (t) = TC のとき, t < T̃C < TC を満た
す.

5.3.3

ϵ-ZCBF への仮想時間の適用

ここでは, 前述した ϵ-ZCBF に仮想時間を導入する. 提案する仮想時間上での ϵ-ZCBF 制
約を定義し, いくつかの注意点を述べる.
定理 2. (ϵ-ZCBF と時間軸変換) ϵ-ZCBF b : X → R と定数 ϱϵ と ϵ, δ, 定義 7 で定義された
ρ(x) および到達時間 TC に対して定義された仮想時間 τ を考える. 仮想時間 τ 上での ϵ-ZCBF
制約を以下のように定義する:

sup[s(x, t, τ ) (Lf b(x) + Lg b(x)u) + ϱϵ (b(x) − ϵ)] ≥ 0.

(5.27)u∈U

加えて, ϵ-ZCBF と時間軸変換が与えられたとき, 不等式 (5.27) を満たす入力の集合を Uϵ−τ (x, t)= {u ∈ U | s(x, t, τ )(Lf b(x) + Lg b(x)u) + ϱϵ (b(x) − ϵ) ≥ 0} とする. フィードバック制御器
u(x, t) ∈ Uϵ−τ を考える. このとき, 制御器 u(x, t) はシステムの状態を元の時間軸 t 上で有限時間 0 < TC∗ < TC で集合 Cδ に遷移させる: x(t) ∈ Cδ (t = TC∗ ). 加えて, システムの状態は全ての時刻 TC∗ ≤ t < TC で Cδ に留まる.

Proof. 補題 2 から, 制御器 u(x, t) はシステムの状態を時間軸 τ 上で有限時間 TC 以内に集合
Cδ に遷移させる: x(τ ) ∈ Cδ (τ = TC ). そのため, 補題 4 から, x(τ ) ∈ Cδ (τ = TC ) のとき,
x(t) ∈ Cδ を満たす時刻 t = TC∗ < TC , τ (t) = TC が存在する. 加えて, 補題 2 から, システムの状態は任意の時刻 TC ≤ τ < TC + ∆τ および TC∗ ≤ t < TC において Cδ に留まる.
注意 1. 提案する制御器は, ρ(x) の値に応じて ϵ-ZCBF 制約を緩和する; ρ(x) の値が β より小さい場合, τ の速度が遅くなり, 制約（5.27）が緩和される. ただし, t が時間 T に近づくと,
s2 (t, τ ) が大きくなり, τ の速度が速くなる. これにより, τ が t に追いつくため, 有限時間 TC
以内にシステムを集合 Cδ に遷移させる.
注意 2. 仮想時間は時間区間 [t0 , TC ) で定義されているため, 定理 2 の制御器も [t0 , TC ) で定義される. したがって, 時間 T の以降の制御には, 制御器を変更または修正する必要がある. シ
ステムを Cδ に留まるように ϵ-ZCBF を用いた制御器を用いて制御する場合, 時間 TC∗ で仮想時間の速度を s(x, t, τ ) = 1 に固定することで実現できる.

第 5 章 時間制約をもつ信号時相論理に対する制御

58

5.3.4

STL 命題への提案手法の適用

提案手法を STL で記述されたタスクに適用する. まず, 与えられた STL 命題に対応する
時間オートマトンに受理されるパスの振る舞いを次の STL 命題によって表現する.

^

F[ti ,ti+1 ] Gψi ∧ G[ti ,ti+1 ] ϕi ,(5.28)i

ここで, ti < ti+1 , ϕi と ψi は様相作用素を含まない STL 命題とする. この命題は [50] で用いられている命題と同様の形式をしている. 時間区間 [ti , ti+1 ] に対するそれぞれの命題は時間
オートマトンのロケーション間の遷移に対応している. [50] で用いられている命題との違い
は, 本章で用いる命題は時間オートマトンから生成されているため, 各命題の様相作用素に関する時間区間が自動的に決定する点である. また, ϕi は安全性に関わる命題, ψi は達成するべきロケーション間の遷移に関わる命題と考えることができる. ここで, 次の問題を考える:
問題 1. 各時間区間 [ti , ti+1 ] で, システム (5.1) が命題 (5.28) を満たすように制御する.
加えて, 本章では ϕi に関する制約は ψi 制約よりも優先して常に満たされるべきであると仮定する.
問題 1 から, 下記の CBF 制約を考える:• ψi に対する ϵ-ZCBF 制約,• ϕi に対する CBF 制約.
最適化問題が実行不可能になるリスクを低減するために, これらの CBF 制約を仮想時間上で実装する. CBF 制約を満たす制御入力を計算するための最適化問題は下記の様に定式化さ
れる:

min
u

||u||2(5.29)s.t.
si (x, t, τi )(Lf ρψi (x) + Lg ρψi (x)u) + ϱϵi (ρψi (x) − ϵi )≥ 0,(5.30)si (x, t, τi )(Lf ρϕi (x) + Lg ρϕi (x)u) + ϱϕi ρϕi (x) ≥ 0,

(5.31)

ここで, ϱϕi > 0 は定数:

τi (ti ) = ti , si (x, t, τi ) = si1 (x) · si2 (t, τi ),


 
1
si1 (x) = max − lnρϕ (x) , 0 + 1,
βi i
ti+1 − tsi2 (t, τ ) =.
ti+1 − τi + ∆τi(5.32)(5.33)(5.34)

5.4. 数値実験

59

仮想時間によって ρϕi の値が β より小さくなると s(x, t, τ ) が大きくなり, ϵ-ZCBF 制約は時間の観点から緩和される. これは, もし ϕi に関する制約が違反されそうになると, ψi に関する制約が緩和されることを意味している. ρϕi に関する CBF 制約から, ∀t ∈ [ti , ti+1 ], ρϕi (x) > 0が保証され, これにより ∀t ∈ [ti , ti+1 ], s(x) < ∞ が成り立つ.
ψi が満たされ, 時刻 t が ti+1 になったとき, 次の時間区間の CBF 制約を用いた制御器に切り替える. これを逐次的に繰り返し, 時間オートマトンのロケーション間の遷移が達成される. 受理状態への遷移が達成された際, 元のタスクが達成されることで, 問題 1 が解決される.

5.4

数値実験

ここでは, 提案手法をアダプティブクルーズコントロールに対する 1 つの時間間隔を持つタスクに適用する. 本章では, 全ての計算は, 3.6GHz Intel Corei9 で実行した. 加えて, 全ての最適化問題は MATLAB によって実装され, シミュレーションは Simulink [55] で実行した.

Fig. 5.3: Model of an adaptive cruise control

5.4.1

問題設定

1 次元のアダプティブクルーズコントロールを考える (Fig. 5.3). 対象とするシステムとして, 次のダイナミクスを考える:
ẋ = u, ẋo = vf − u,(5.35)

ここで, u は入力速度, x は制御する車の位置, xo は前方の車との距離, vf は前方の車の速度とする. 前方の車の速度は観測可能と仮定する.

第 5 章 時間制約をもつ信号時相論理に対する制御

60

タスクとして “いつか, 制御する車は時間 T までに目標位置 xG に到達し, 常に前方の車までの距離を 0.5 以上に保つ” という命題を考える. この命題を以下の STL 命題として定義
する:

F[0,T ] GψA ∧ G[0,T ] ϕB ,(5.36)

ρψA = x − xG , ρϕB = xo − 0.5.

(5.37)

このタスクでは, 入力速度の上限は前方の車までの距離の条件によって制限され, 入力速度の下限は指定された時間までに目標位置に到達するための条件によって制限される. そのため, 時間に関する制約により入力速度の下限が上限を超える可能性があり, その場合, 実行可能な入力が消失する. 提案手法により, この制御入力が消失するリスクを低減する.
仮想時間による入力の消失のリスクの低減と提案手法で STL タスクが達成されることを確認するため, 次の状況に対してシミュレーションを行った.

• CBF パラメーター ϱϵ , ϱϕ を仮想時間ありと仮想時間なしの場合に対して変更する• 前の車の動作を変更する
20 組の CBF パラメーターを用意し, 前方の車の 3 つの動作パターンに対してシミュレー
ションを行った. また, 最適化問題が実行不可能になった場合, シミュレーションを停止した.
この際に使用した CBF パラメーターは, 動作パターン 1 でタスクを達成し, 他の動作パター
ンの初期状態で最適化問題の CBF 制約を満たすようにランダムに生成した. その他のパラメーターは, x（0）= 0, x（o 0）= 2, T = 10, xG = 15, δ = 0.1, ∆τ = 0.1 および β = 2 とした.
また ϵ は, ϱϵ , T および δ に対して, 補題 2 の不等式 e−ϱϵ T（b（x0 ）− ϵ）+ ϵ = δ, ϵ ≥ δ を満たすように決定した.

5.4.2

結果・考察

Fig.5.4 は, 前方の車の 3 つの動作パターンを示してる. 3 つの動作パターンは,「前方の車は常に速く進む」, 「前方の車は最初は速く, 次にゆっくりと進み, 最後に速く進む」,「前方の車は最初はゆっくりと進み, 次に速く進む」の 3 パターンとした. 表 5.1 は, シミュレー
ションしたパラメーターの組の数に対するタスクの命題を達成したパラメーターの組の数を示している. パターン 1 では, 命題を達成したパラメーターの組の数は、シミュレーション
したパラメーターの組の数と同じ 20 である. パターン 2 では, 仮想時間なしでは命題を達成したパラメーターの組が存在しなかったが, 仮想時間を用いた場合, 15 組のパラメーターの組が命題を達成した. また, パターン 3 では, 仮想時間を用いることで, 命題を達成するパラメーターの組の数が増加した. これより, 提案した制御器を用いてシステムがタスクである
STL 命題を達成したことが確認できる. さらに, 命題を達成するパラメーターの組の数が仮
想時間を導入することで増加したことから, 仮想時間が制御入力の消失のリスクを低減していることがわかる. 加えて, ϵ-ZCBF はパラメーターとロバストネス関数が補題 2 を満たすよ

5.4. 数値実験

61

うに, ロバストネス関数を使用していくつかのパラメーターを設計することで容易に定義できる.
さらに, Fig.5.5 は, パターン 3 の時の 1 つのパラメーターの組に対するロバストネス関数
ρψA と ρϕB の軌道を示している. ρϕB は常に正であり, ρψA は T = 10 の前に正になるため,
ϵ-ZCBF を使用するコントローラーが命題を達成していることがわかる. また, 一時的に ρϕBの値が β より小さい期間が存在し, その際には仮想時間の進行が遅くなり ρψA の軌道は仮想時間がない場合よりも遅く進行していることがわかる. しかし, 仮想時間の時間を早送りする項の影響による部分的な高速化によって, 仮想時間が元の時間に追いつき, 命題を達成している.

Behavior 1
Behavior 2
Behavior 3

w/o virtual time
20/20
0/20
15/20

w/ virtual time
20/20
15/20
20/20

Table 5.1: Solved parameters ratio in the simulations

Fig. 5.4: Three behaviors of car in front: 1) fast, 2) slow at ﬁrst, and 3) fast at ﬁrst and last.

第 5 章 時間制約をもつ信号時相論理に対する制御

62

Fig. 5.5: The Trajectories of robustness functions of ψA and ϕB with and without virtualtime in behavior 3. We can see that ψA is a positive value at time T and ϕB is always
positive.

5.5

まとめ

本章では, 時間制約をもつ様相作用素を含む STL 命題を達成するための制御手法を提案した. まず, 指定された時間制約を守りながら状態を目的の集合に到達させるために ϵ-ZCBFを定義した. 次に, 制御入力が消失するリスクを軽減するため, 制約条件を緩和する仮想時間を導入した. その後, 最後に, 移動ロボットに対する数値シミュレーションにて, 提案手法の有効性を示した.
本章の数値実験では, パス上に存在するロケーション間の遷移が１つのタスクを対象としている. そのため, 複数のロケーション間の遷移がある STL 命題で表現されたタスクでの有効性検証は今後の課題と考えられる.

第6章

6.1

信号時相論理に対する確率的動作
プリミティブの再配合

はじめにタスクを達成するために複数のシステムが行動する際, 通信などによってお互いの状態を共有することができない場合がある. 特に, ロボットが人間と同時に行動する場合には, ロ
ボットが人間の行動を推定し, 場合によっては行動を変更する必要がある.
これを踏まえ, 本章ではタスクを達成するための複数のシステムの行動が与えられた際に,他のシステムの行動に合わせて自身の行動を修正するための手法を提案する. 時相論理で表現されたタスクを達成するための複数のシステムに対するタスクの配分手法に関しては様々な手法 [56, 57] が提案されており, 本章では, タスクの配分方法については既存の手法を用いて高レベル制御器内で実現されていると仮定する.
以降では, まず提案手法で用いる確率的動作プリミティブについて説明する. 次に, 生成された確率的動作プリミティブを用いて, 他のシステムの状態が属している確率的動作プリミ
ティブをカルマンフィルタを用いて推定する手法を提案する. その後, 推定された確率的動作プリミティブを基に, 自身の確率的動作プリミティブを更新することで, 他のシステムの行動を考慮して与えられた命題を達成する軌道を生成する方法を提案する. 最後に, 移動ロ
ボットに対する数値シミュレーションにて, 提案手法の有効性を示す.

6.2

確率的動作プリミティブ

確率的動作プリミティブ (ProMP) はロボットシステムの軌道上での確率分布を表現する手法である [58, 59]. ProMP を用いて, ロボットシステムの軌道 x(t) は以下のように表現できる [59]:x(t) = Φ(t)w + nx ,(6.1)

ここで, Φ ∈ Rnx ×nw ·nx は基底関数で構成された行列, w = [w1T , . . . , wnTx ] ∈ Rnx ·nw , wi ∈ Rnw
は各状態に対する重みベクトル, nx ∼ N (0nx , Σx ) は状態に関するノイズとする. Φ は以下63

第 6 章 信号時相論理に対する確率的動作プリミティブの再配合

64の式の様に定義される:

Φ1 (t) · · · 01×nw
 .
.. 
..
.
.
Φ(t) = 
. 
 .
,
.
01×nw . . Φnx (t)

ここで, Φi (t) は時刻を引数とする基底関数の集合で構成されたベクトルである. 本章では,基底関数として動径基底関数を用いる. 加えて, 重みベクトル wi について以下を仮定する:

p(wi |µwi , Σwi ) = N (µwi , Σwi )ここで, µwi , Σwi は ProMP を表現するためのパラメーターである. また, w の分布は以下のように表現できる:

 

 
Σ w 1 , w 1 . . . Σw 1 , w n x
w1
µw 1

 
 

..
...
(6.2)
N   ...   ...  ,  ...

.

w nx

µw n x

Σw nx , w 1 . . . Σwn x , w n x

ProMP はシステムの軌道データから最尤推定を用いて学習することができる. 本章の数値
実験では, 参考文献 [59] で提案されている手法を用いて ProMP を学習している.

6.2.1

確率的動作プリミティブの配合

複数の ProMP が与えられたとき, それらを配合することによって新しい軌道分布を生成することができる [58]. ここでは, M 個の ProMP を配合することを考える. また, 先行研
究 [58, 60] と異なり, ProMP の重みベクトルの分布を合成することで新しい ProMP を定義
Qする. 新しい軌道分布の特性として x(t) = Φ(t)w∗ + nx , p(w∗ ) = i pγi (wi ), γi ∈ [0, 1] を仮定する. このとき, 新しい軌道分布の重みベクトルの分布 p(w∗ ) は次式となる.

w∗ ∼ N (µw∗ , Σw∗ ) ,!−1
X 1
Σw ∗ =
Σw i, µw ∗ = Σw ∗
γ
i
i

X 1
i

γi

−1
Σw i

(6.3)

!
µw i

= T w µw ,

(6.4)

T
T
ここで, Tw∗ = [Σw∗ ( γ11 Σw1 )−1 , . . . , Σw∗ ( γ1M ΣwM )−1 ], µw = [µT
w1 , . . . , µ wM ] .
特定の γi を 1 に近い値, その他の γi を 0 に近い値にすることで, 配合された ProMP は
特定の ProMP に近い分布になる. 加えて, γi を時変にすることで, 配合された ProMP を時間に沿って変化させることも可能である.

6.3

無香料カルマンフィルタ

無香料カルマンフィルタ (Uncented Kalman Filter, UKF) は非線形システムの状態の分布をサンプル点を用いて推定することで, 高い精度で分布を近似する手法である [61] [62].

6.3. 無香料カルマンフィルタ

6.3.1

65

アルゴリズムUKF のアルゴリズムについて述べる [62]. 対象のシステムとして以下の式で表現される
離散時間非線形システムを考える;
x(t + 1) = F (x(t), nx (t))y(t) = H(x(t), ny (t)),
ここで, F, H は遷移関数と出力関数, n∗ はそれぞれ x, y に関するノイズである.
準備
前述したシステムに対して, 補助変数 xU KF ∈ RnUKF を考える;

x(t − 1)


xU KF (t − 1) = nx (t − 1) ,ny (t − 1)
加えて, 状態とノイズに関する共分散行列を用いて, 補助変数 XU KF の共分散行列を定義
する;


Σx 0
0


ΣU KF =  0 Σnx 0 00 Σ nyただし, ノイズの平均値はゼロと仮定している.
シグマポイントの生成
共分散行列を用いて, 2nUKF + 1 個のシグマポイントを以下のように生成する;


i=0

xU KF (t − 1),

XiU KF (t − 1) =xU KF (t − 1) + κ1 Si , i = 1, . . . , nUKF
xU KF (t − 1) − κ1 Si , i = nUKF + 1, . . . , 2nUKF√
ここで, Si は行列 S = ΣU KF の i 列目の列ベクトルである. また, κ1 はスケーリングパラ
メーターである [63]. このシグマポイントを状態とノイズに関する部分に以下のように分割する;
Xix (t − 1)XiU KF (t − 1) = Xinx (t − 1) .
Xiny (t − 1)

第 6 章 信号時相論理に対する確率的動作プリミティブの再配合

66
シグマポイントの更新

シグマポイントそれぞれについて, 遷移関数を用いて状態の更新を行う;

Xix,t/t−1 = F (Xix (t − 1), Xinx (t − 1)).
遷移した状態を用いて事前分布を計算する:

x̂−
t =
Σ−
xt =

2n
UKF
X
i=0
2n
UKF
X

λim Xix,t/t−1 ,− Tiλic (Xix,t/t−1 − x−t )(Xx,t/t−1 − xt ) ,i=0

ここで, 各重み λim , λic は以下のように定義される;
κ2
λ0m =
,
nUKF + κ2
κ2+ (1 − κ3 ),λ0c =nUKF + κ2
κ2,
λim = λic =2(nUKF + κ2 )i=0
i=0i = 1, . . . , 2nUKF

ここで, κ2 , κ3 は調整パラメーターである.
状態の推定
シグマポイントの更新と同様に, シグマポイントに対応する出力, 平均と共分散を計算する;
i
Yt/t−1= H(Xix,t/t−1 , Xiny (t − 1)),ŷt− =
Σ−yt =
Σxt ,yt =

2n
UKF
X
i=0
2n
UKF
X
i=0
2n
UKF
X

i
λim Yt/t−1,iiλic (Yt/t−1− ŷt− )(Yt/t−1− ŷt− )T ,− Ti
λic (Xix,t/t−1 − x−t )(Yt/t−1 − ŷt ) .

i=0

このとき, カルマンゲインは次式で計算される;−1
Kt = Σxt ,yt (Σ−yt ) .

また, 推定値と共分散行列は次式で計算される;i−x̂(t) = x̂−t + Kt (Yt/t−1 − ŷt ),T−
Σxt = Σ −x t − Kt Σ y t K t .

6.4. 観測情報に基づく確率的動作プリミティブの推定

6.4

67

観測情報に基づく確率的動作プリミティブの推定

複数のシステムで行動する際, 他のシステムの行動を考慮して行動決定を行う必要がある.
そのため, システムが想定している行動の内, どの行動を行っているのかを観測情報から推定する. ここでは, システムの行動に関する ProMP が与えられていると仮定し, 観測情報を用いてカルマンフィルタによって現在の行動が属している ProMP を逐次的に推定する手法を提案する.

6.4.1

配合された確率的動作プリミティブに対する無香料カルマンフィルタ

カルマンフィルタを構築する準備として, 推定する ProMP の配合に関するパラメーター
γ = [γ1 , . . . , γM ], γi ∈ [0, 1], (i = 1, . . . , M ) と ProMP で表現された軌道分布上での時刻τ ≥ 0 の推定値について, 以下のシステムを仮定する.

γi (t + 1) = γi (t) + nγ ,nγ ∼ N (O, Σnγ ),(6.5)

τ (t + 1) = τ (t) + dτ + nτ ,nτ ∼ N (O, Σnτ ),(6.6)

ny ∼ N (O, Σny ).

(6.7)y(t) = C(τ )µ∗w (γ(t)) + ny ,ここで, dτ > 0 は定数, n∗ はノイズ, µ∗w は γi から式 (6.4) で計算される配合された ProMPの重みベクトルの期待値, C(τ ) は τ から計算される基底関数行列を含む行列とする. 加えPて, パラメーター γi には, i γi = 1 の拘束を仮定する. このシステムは, 推定するパラメー
ター γi , τ に対して非線形なシステムであるため, UKF を用いて観測情報 y から, パラメー
ター γ̂i , τ̂ を推定する.
P
γ の制約条件として, M
i=1 γi = 1 を仮定しているため, 制約条件を考慮して推定を行う必要がある. ここでは, γi (i = 1, . . . , M ) から γM を省いた γi (i = 1, . . . , M − 1) に対して, 式
P −1(6.7) のシステムを構築し, M
i=1 γi ≤ 1 − ϵγ を制約条件に加えたシステム上で推定を行う.
ここで, ϵγ > 0 は十分小さな定数である. 加えて, シグマポイントを生成する際に制約条件を満たす領域に対して射影を行う. 具体的には以下の手順によって, シグマポイントを射影する.

1. γi , τ が制約を満たす場合, 手順を終了
2. γi が制約を違反している場合:(a) γi < ϵγ ならば, γi′ = ϵγ .
(b) γi > 1 − (M − 1)ϵγ ならば, γi′ = 1 − (M − 1)ϵγ .
PM −1
T
(c)
i=1 γi > 1 − ϵγ ならば, vγ γ + cγ = 0 へ射影;vγ vγTγ =γ−(γ − cγ vγ ),||vγ ||22′

第 6 章 信号時相論理に対する確率的動作プリミティブの再配合

68

ここで, γ = [γ1 , . . . , γM −1 ]T ,vγ = √

1
1[1, . . . , 1]T , cγ = − √(1 − ϵγ ).
M −1
M −1

3. τ が制約を違反している場合, τ = 0.

6.4.2

推定された確率的動作プリミティブを用いた判別

UKF によって推定した ProMP の配合に関するパラメーターを用いて, システムの行動が属している ProMP を判別する. ここで, Σ̄w , µ̄w を判別した ProMP とする. 判別方法としては, 推定された最大の γ̂i に対応する ProMP がそのシステムの行動が属する ProMP とする方法や推定した配合された ProMP 自体をシステムの行動が属する ProMP とする方法がある.
一方で, システムの行動が配合に用いたどの ProMP にも属していない場合がある. そのため, ProMP に関するパラメーターを推定した後, 判別した ProMP に対して外れ値であるかどうかを判別することで, システムの状態がどの ProMP にも属していない場合を判別す
る. 外れ値 dP ro は次式で計算した;−1
dP ro = (y(t) − C(t)µ̄w (t))T C(t)Σ̄w C T (t)(y(t) − C(t)µ̄w (t))
(6.8)

6.5

観測情報による確率的動作プリミティブの再配合推定した他のシステムの確率的動作プリミティブを用いて, STL 命題を達成するための軌道を生成するため行動計画手法を定式化する. 複数のシステムを用いてタスクを達成する場
合, システム全体でタスクを達成するために, 他のシステムの行動を考慮して行動計画を行う必要がある. そのため, 推定した ProMP からシステムの軌道 Xc (1 : T ) を生成し, 他のシ
ステムの行動を獲得する. 生成されたシステムの軌道 Xc (1 : T ) を用いて, STL 命題を満たす軌道を生成するために ProMP の配合に関するパラメーターを最適化によって更新する.
ProMP の配合に関するパラメーターの最適化問題は以下のように定式化できる:min −ρφ (x, Xc ) + L(γ, a)(6.9)x(t) = Φ(τx (t))µ∗w (γ),(6.10)µ∗w (γ) = Tw (γ)µw (γ),(6.11)

ρφ (x, Xc ) ≥ ϵφ ,(6.12)γ, as.t.

τx (t) =L
X

an t n ,(6.13)n=0d
τx (t) ≥ 0.
dt

(6.14)

6.6. 数値実験

69

ここで, a = [a1 , . . . , aL ]T ∈ RL
+ は時間関数 τx : T → R+ の係数, ρφ は STL 命題 φ に対応
するロバストネス関数, L(γ, a) はその他の目的関数である. 時間関数 τx を導入することによって, システムの軌道の自由度を高めることが可能となっている.
この最適化問題の解であるパラメーター γ, a から, 式 (6.10),(6.13) を用いて, STL 命題を満たすシステムの軌道を計算することができる.

6.6

数値実験

ここでは, 提案手法を 2 台の移動ロボットのタスクに適用する. 本章での全ての計算は,3.6GHz Intel Core i9 で実行した. 全ての最適化問題は MATLAB 上で CasADi [42] によって実装し, ソルバーとして Ipopt [45] を用いた.

6.6.1

問題設定

2 台の移動ロボットのタスクとして, Fig.6.1 に示す環境において次の命題を考える: ”2 台のロボットの内, 領域 1, 2 を少なくとも 1 台が通過し, その後領域 3 に移動する” . この命題を以下の STL 命題として定義する:
φ = φ1 ∧ φ2
φ1 = F[0,T ] (φ1,1 ⇒ Fφ3,1 ) ∨ F[0,T ] (φ2,1 ⇒ Fφ3,1 )φ2 = F[0,T ] (φ1,2 ⇒ Fφ3,2 ) ∨ F[0,T ] (φ2,2 ⇒ Fφ3,2 )ここで, φi,j は命題 ”領域 i にロボット j が存在する” を表わしている. ロボットのダイナミクスとして独立 2 輪型モデルを考え, システム全体の状態を x = [(x1 )T , (x2 )T ]T , 各ロボットの状態を xj = [xjpx , xjpy , xjθ ]T ∈ R3 とし, 入力を u = [uv , uw ]T , uv ∈ [0, 1], uw ∈ [−1, 1] とする. それぞれの命題に対して, 次のロバストネス関数を定義する:

ρφ1,j (x) = min(xjpx − 1.1, −xjpx + 1.3, xjpy − 1, −xjpy + 1.2),
ρφ2,j (x) = min(xjpx − 1, −xjpx + 1.2, xjpy , −xjpy + 0.2),
ρφ3,j (x) = min(xjpx − 2, −xjpx + 2.2, xjpy − 1, −xjpy + 1.2).
また, ロボットの初期値を x1 = [0, 0, 0]T , x2 = [0, 1, 0]T , 命題に関する時間制約を T = 8 とした. ProMP の学習のために, 個々のロボットについて初期値にノイズを加えた状態から以下の行動を達成する軌道を式 (4.5) と同様な最適化問題を解くことによって生成する. 本実
験では, 各ロボットの行動はタスクの命題を基に手動でタスクの配分を行った. 加えて, タス
クの中に現れるロボットの状態は各ロボットの平面座標 xjpx , xjpy のみであるため, 各ロボッ
ト毎に xjpx , xjpy の軌道を用いて ProMP を学習する. この際, 各行動に対して 20 個の軌道を生成し, ProMP の学習を行った.

• ロボット 1

第 6 章 信号時相論理に対する確率的動作プリミティブの再配合

70

行動 1.a: F[0,T ] (φ1,1 ⇒ Fφ3,1 ) を達成する行動
行動 1.b: F[0,T ] (φ1,1 ⇒ Fφ3,1 ) ∧ F[0,T ] (φ2,1 ⇒ Fφ3,1 ) を達成する行動• ロボット 2
行動 2.a: F[0,T ] (φ2,2 ⇒ Fφ3,2 ) を達成する行動
行動 2.b: F[0,T ] (φ1,2 ⇒ Fφ3,2 ) を達成する行動
ここでは, ロボット 1 がロボット 2 の状態から行動を更新することを考える. ロボット 1はロボット 2 の位置を観測することができ, 各ロボットには事前に行動 1.a, 行動 2.a が割り当てられているとする. このとき, 各ロボットが割り当てられた行動を実行することが出来れば, 命題 φ を満たすことができる. このとき, ロボット 1 がロボット 2 の行動を推定し, 行動を更新することでタスクを達成するための軌道が得られることを確認する.

6.6.2

結果・考察

本実験では, 行動 2.b の ProMP の平均軌道をロボット 2 の軌道としてロボット 1 が観測すると仮定し, ロボット 2 に対応する配合された ProMP に関する UKF を更新した際の挙動を確認した. ロボット 2 の動作が行動 2.a の ProMP に対して外れ値として判定された際にロボット 1 の行動を更新する. このとき, 最適化問題を計算する際のロボットの 2 の将来の軌道は, 更新する時刻でのロボット 2 の状態から変化しないものとする. また, 外れ値の判定は dP ro > 4 で外れ値と判定し, 行動 2.a, 行動 2.b に対応する ProMP と推定した ProMPに対する外れ値を daP ro , dbP ro , dγ̂P ro とする.
シミュレーションの結果を Fig.6.2, シミュレーション中の外れ値の値を Fig.6.3, 推定されたロボット 1, 2 の ProMP の配合に関するパラメーター γ11 , γ12 と τ の軌道を Fig.6.4, 6.5,
6.6 に示す. Fig.6.2 で, 黒円はロボット 2 の状態, マゼンタの円はロボット 1 の ProMP 上での現在の状態, マゼンタの軌道はロボット 1 の ProMP の軌道, 緑と紫の軌道はロボット 2の推定された ProMP の軌道 (dP ro > 4 の時刻では緑), 茶色の軌道は最適化問題を計算する
際のロボットの 2 の軌道である. 本実験では, シミュレーションの早い段階で外れ値として検知され, ロボット 1 の行動の更新が行われた. Fig.6.2 および Fig.6.4 から, 最適化問題を解くことによってロボット 1 の行動が行動 1.b に切り替わり命題 φ を満たす行動をとっていることがわかる. また, 本実験における最適化問題の計算では 15 − 20 秒程度を要した. 加えて, Fig.6.3, 6.5, 6.6 から UKF によってロボット 2 の行動が行動 2.b に属していることが推定されていることが確認できる.

6.7

まとめ

本章では, 観測情報に基づきシステムの行動を変更するための手法を提案した. まず, 複数の ProMP が配合された ProMP に対して, UKF を用いて配合に関するパラメーターを推定

6.7. まとめ71する手法を提案した. その後, UKF から得られたパラメーターからシステムの行動を推定し,推定したシステムの行動から STL 命題を達成する軌道を生成するための軌道最適化問題を定式化した. 最後に, 移動ロボットに対して提案手法を適用し, システムの行動の推定や軌道の変更が可能なことを示した.
本章の数値実験では, 2 つシステムが動作するタスクに対して提案手法を適用した. 一方で, 提案手法を用いて行動を更新したのは１つのシステムのみである. 複数のシステムが動作するタスクにおいて, 提案手法によって互いの行動を推定し, 行動を更新する状況で提案手法を検討することは今後の課題と考えられる. また, 提案手法で生成した目標軌道を用いてシステムの制御を行い, 有効性を検証することや計算時間の短縮も今後の課題である.

Fig. 6.1: Workspace of the two mobile robots

72

第 6 章 信号時相論理に対する確率的動作プリミティブの再配合

Fig. 6.2: Behaviors of ProMPs in the numerical simulation. Black circles are states of Robot
2, magenta circles are current states of ProMPs, magenta trajectories are trajectories of
ProMP of robot 1, perple and green trajectories are trajectories of estimated ProMP ofrobot 2: when d1P ro < 4, trajectories are green, and brown trajectoies are trajectories usedin optimization problem for ProMP of robot 1.

6.7. まとめ73

Fig. 6.3: The trajectories of outliers for ProMPs of Motion 2.a, Motion 2.b and esatimatedone.

Fig. 6.4: The trajectory of the blending parameter of ProMP of robot 1: γ11

74

第 6 章 信号時相論理に対する確率的動作プリミティブの再配合

Fig. 6.5: The trajectory of the blending parameter of ProMP of robot 2: γ12Fig. 6.6: The trajectory of the time parameter of ProMP of robot 2: τ

第7章

結論

本論文では, 時相論理で表現されたタスクをロボットシステムが達成するための軌道最適化手法と, 制御手法の提案を行った. また, 時相論理命題で表現されたタスクを達成する軌道
最適化問題に対する混合整数線形計画問題や非線形計画問題による定式化において, 適用可
能なロボットシステムや命題のクラスを拡張した. 加えて, 生成した軌道や行動計画を用いてタスクを達成するための制御手法として, 時間制約をもつ制約条件を達成するための制御手法や行動を変更する手法の提案および性能の改善を行った. 本論文で得られた成果を以下にまとめる.
2 章では, 本論文で利用する時相論理に関するいくつかの定義の導入を行い, ロボットシス
テムの振る舞いを時相論理を用いて表現できることを説明した.
3 章では, 非線形システムに対する LTL 命題を満たす軌道を生成するための行動計画問題
を, 混合整数線形計画問題として定式化する手法を構築した. まず, ハイブリッドシステムである線形システム, MLD モデルに対する行動計画が混合整数計画問題として定式化することが可能なことについて説明した. その後, 混合整数凸計画を用いることでダイナミクスに二乗項や双線形項を持つシステムを線形システムとして表現でき, 先に述べた混合整数計画
問題に組み込むことができることを示した. 加えて, 拡大線形化を用いて拡大線形システム
または拡大双線形システムを設計することで, 非線形システムを混合整数計画問題に組み込むことができることを示した. 数値シミュレーションで提案手法の有効性検証を行い, LTL
命題を満たす軌道を生成することが可能であることを確認した.
4 章では, 発生時刻が不確かな外的事象に関する命題を達成するための軌道最適化手法を提案した. 正確な発生時刻が不明な外的事象について発生時刻に関する分布を仮定し, その分布に基づき, Event-based STL 命題を達成する軌道を生成することが可能な軌道最適化を
非線形計画問題として定式化した. 軌道最適化を行う際に, 外的事象の発生に関する変数を最適化変数に含めることで, 外的事象の発生時刻に対する軌道のロバスト性を評価することを可能にした. 最後に, 不確かな外的事象に関する命題が与えられた移動ロボットに対する
数値シミュレーションにて, 提案手法を用いることで外的事象の発生に関する複数のシナリ
オに対して与えられた命題が達成されることを確認した.
5 章では, 時間制約をもつ信号時相論理を達成するための制御バリア関数を用いた制御手法を提案した. STL 命題に対応する時間オートマトンと時間オートマトン上で STL 命題を満たすパスを考え, 各ロケーション間の遷移を達成することを目的とし, 制御バリア関数を拡張した制御器を提案した. 加えて, 関数の値によって進行速度の変化する仮想時間を定義し,定義した仮想時間上で提案した制御器を用いて制御した場合, 元の時間軸上でも時間制約を

75

76

第 7 章 結論満たすことを示した. その後, 本章で考える制御構造を説明し, パス上でのロケーションの遷移を逐次達成することで与えられた本来の命題が達成されることを示した. 最後に, 移動ロ
ボットに対する数値シミュレーションにて, 制約を満たしながら時間内に命題を達成することが可能であり, 仮想時間を導入することで解の消失のリスクが低減されることを確認した.
6 章では, 複数のシステムでタスクを行う際に, 他のシステムの行動に合わせて自身の行動を修正するための軌道最適化手法を提案した. まず, 観測情報からシステムの状態が属している確率的動作プリミティブをカルマンフィルターを用いて推定する手法について説明した.
その後, 推定された他のシステムの確率的動作プリミティブを基に, 確率的動作プリミティ
ブを更新することで, 他のシステムの動作を考慮して与えられた命題を達成する軌道を生成する軌道最適化問題を定式化した. 最後に, 2 台の移動ロボットに対する数値シミュレーショ
ンにて, ロボットが他のロボットに関する観測情報を基に自身の行動を変更することが可能なことを確認した.
これらの結果は, ロボットのタスク実行という課題のアプローチの一つである時相論理を用いた行動計画手法に対して, 新たな観点から高性能化と拡張性の向上の可能性を示したものであり，この分野の研究ならびに応用を大きく進展させると期待している．
最後に本論文の研究成果に対する今後の課題について述べる.

• 動作中の行動計画の再計算
実際の作業環境では, タスクを実行する際に動作の失敗や環境の変化が発生し動作を変更する必要がある場合がある. 本論文では, 4 章にて外的事象の発生に対してロバスト性を持つ軌道を生成することや, 6 章にて ProMP の推定や再配合によって, 環境の変化や動作変更に対応することを検討した. しかし, 動作のやり直しが必要な状況など,
本論文で提案した手法で対応することが難しい状況も存在する. そういった状況に対して対応するための手法として, 動作中に再び行動計画を行うことなどが挙げられる.
しかし, 本論文で提案した手法を含め, 高レベル制御器として考えれている手法は計算コストが高く, 動作途中に実行することは現実的では無いといった問題がある. そのため, 計算コストを抑えるために近似システムの構築方法のさらなる検討や計算の高速化が課題であると考えられる.
• 実機実験における有効性検証
本論文では, 数値シミュレーションを用いて提案手法毎に有効性を確認した. 今後の課題として, 実機実験に対して本論文で考える制御アーキテクチャを適用し, 提案手法の有効性検証も行う必要があると考えている.

参考文献[1] 経済産業省，“ロボットを取り巻く環境変化と今後の施策の方向性〜ロボットによる社会変革推進計画〜”，https://www.meti.go.jp/shingikai/mono info service/robot
shakaihenkaku/pdf/20190724 report 01.pdf ，2019．
[2] S. Karaman, R. G. Sanfelice, and E. Frazzoli, “Optimal control of mixed logical dynamical systems with linear temporal logic speciﬁcations,” in 2008 47th IEEE Conferenceon Decision and Control (CDC), 2008, pp. 2117–2122.
[3] S. Sadraddini, and C. Belta, “Robust temporal logic model predictive control,” in 2015
53rd Annual Allerton Conference on Communication, Control, and Computing (Allerton), 2015, pp. 772–779.
[4] C. Baier, and J.-P. Katoen, Principles of model checking. MIT press, 2008.
[5] O. Maler, and D. Nickovic, “Monitoring temporal properties of continuous signals,”in Formal Techniques, Modelling and Analysis of Timed and Fault-Tolerant Systems,
Y. Lakhnech, and S. Yovine, Eds. Berlin, Heidelberg: Springer Berlin Heidelberg,
2004, pp. 152–166.
[6] D. Gundana, and H. Kress-Gazit, “Event-based signal temporal logic synthesis for single
and multi-robot tasks,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3687–
3694, 2021.
[7] C. Belta, B. Yordanov, and E. Gol, Formal Methods for Discrete-Time Dynamical Systems. Springer, 2017, vol. 89.
[8] X. Ding, S. L. Smith, C. Belta, and D. Rus, “Optimal control of markov decision
processes with linear temporal logic constraints,” IEEE Transactions on AutomaticControl, vol. 59, no. 5, pp. 1244–1257, 2014.
[9] V. Nenchev, C. Belta, and J. Raisch, “Optimal motion planning with temporal logic
and switching constraints,” in 2015 European Control Conference (ECC), 2015, pp.
1141–1146.
[10] C. Dawson, and C. Fan, “Robust counterexample-guided optimization for planningfrom diﬀerentiable temporal logic,” in 2022 IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), 2022, pp. 7205–7212.

77

78

参考文献

[11] J. Envall, R. Poranne, and S. Coros, “Diﬀerentiable task assignment and motion planning,” in 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2023, pp. 2049–2056.
[12] M. Cai, S. Xiao, Z. Li, and Z. Kan, “Optimal probabilistic motion planning with potential infeasible ltl constraints,” IEEE Transactions on Automatic Control, vol. 68, no. 1,
pp. 301–316, 2021.
[13] R. Takano, H. Oyama, and M. Yamakita, “Continuous optimization-based task andmotion planning with signal temporal logic speciﬁcations for sequential manipulation,”in 2021 IEEE International Conference on Robotics and Automation (ICRA), 2021, pp.
8409–8415.
[14] K. Takaya, R. Takano, and H. Oyama, “Metaheuristics approach for mathematicalprograms with switching constraints and application to robotic task planning,” in 2022
IEEE International Conference on Systems, Man, and Cybernetics (SMC), 2022, pp.
77–84.
[15] S. Haesaert, S. Soudjani, and A. Abate, “Temporal logic control of general markov
decision processes by approximate policy reﬁnement,” IFAC-PapersOnLine, vol. 51,
no. 16, pp. 73–78, 2018, 6th IFAC Conference on Analysis and Design of Hybrid
Systems ADHS 2018. [Online]. Available: https://www.sciencedirect.com/science/
article/pii/S2405896318311261[16] S. Haesaert, P. Nilsson, C. Vasile, R. Thakker, A. Agha-mohammadi, A. Ames, andR. Murray, “Temporal logic control of pomdps via label-based stochastic simulation
relations,” IFAC-PapersOnLine, vol. 51, no. 16, pp. 271–276, 2018, 6th IFAC
Conference on Analysis and Design of Hybrid Systems ADHS 2018. [Online]. Available:
https://www.sciencedirect.com/science/article/pii/S2405896318311625[17] V. Raman, A. Donze, M. Maasoumy, R. M. Murray, A. Sangiovanni-Vincentelli, and
S. A. Seshia, “Model predictive control with signal temporal logic speciﬁcations,” in53rd IEEE Conference on Decision and Control, 2014, pp. 81–87.
[18] S. Sadraddini, and C. Belta, “Robust temporal logic model predictive control,” in 2015
53rd Annual Allerton Conference on Communication, Control, and Computing (Allerton), 2015, pp. 772–779.
[19] L. Lindemann, and D. V. Dimarogonas, “Control barrier functions for signal temporal
logic tasks,” IEEE Control Systems Letters, vol. 3, no. 1, pp. 96–101, 2019.

参考文献

79

[20] P. Nilsson, and A. D. Ames, “Barrier functions: Bridging the gap between planningfrom speciﬁcations and safety-critical control,” in 2018 IEEE Conference on Decision
and Control (CDC), 2018, pp. 765–772.
[21] M. Srinivasan, S. Coogan, and M. Egerstedt, “Control of multi-agent systems withﬁnite time control barrier certiﬁcates and temporal logic,” in 2018 IEEE Conference on
Decision and Control (CDC), 2018, pp. 1991–1996.
[22] M. Srinivasan, and S. Coogan, “Control of mobile robots using barrier functions under
temporal logic speciﬁcations,” IEEE Transactions on Robotics, vol. 37, no. 2, pp. 363–
374, 2021.
[23] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based
quadratic programs for safety critical systems,” IEEE Transactions on Automatic Control, vol. 62, no. 8, pp. 3861–3876, 2017.
[24] A. Li, L. Wang, P. Pierpaoli, and M. Egerstedt, “Formally correct composition of coordinated behaviors using control barrier certiﬁcates,” in 2018 IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 2018, pp. 3723–3729.
[25] C.-I. Vasile, J. Tumova, S. Karaman, C. Belta, and D. Rus, “Minimum-violation scltl
motion planning for mobility-on-demand,” in 2017 IEEE International Conference on
Robotics and Automation (ICRA), 2017, pp. 1481–1488.
[26] 小野寛晰，情報科学における論理 日本評論社，1994．
[27] M. Bauland, M. Mundhenk, T. Schneider, H. Schnoor, I. Schnoor, and H. Vollmer,“The tractability of model checking for ltl: The good, the bad, and the ugly
fragments,” ACM Trans. Comput. Logic, vol. 12, no. 2, 2011. [Online]. Available:
https://doi.org/10.1145/1877714.1877719[28] S. Dathathri, and R. M. Murray, “Decomposing gr(1) games with singleton livenessguarantees for eﬃcient synthesis,” in 2017 IEEE 56th Annual Conference on Decision
and Control (CDC), 2017, pp. 911–917.
[29] R. Alur, T. Feder, and T. A. Henzinger, “The beneﬁts of relaxing punctuality,”
J. ACM, vol. 43, no. 1, p. 116–146, 1996. [Online]. Available:
https://doi.org/10.1145/227595.227602[30] H. Dai, G. Izatt, and R. Tedrake, “Global inverse kinematics via mixed-integer convex
optimization,” The International Journal of Robotics Research, vol. 38, no. 12-13, pp.
1420–1441, 2019.

80

参考文献

[31] J. P. Vielma, and G. L. Nemhauser, “Modeling disjunctive constraints with a logarithmic
number of binary variables and constraints,” Mathematical Programming, vol. 128, no. 1,
pp. 49–72, 2011.
[32] S. L. Brunton, B. W. Brunton, J. L. Proctor, and J. N. Kutz, “Koopman
invariant subspaces and ﬁnite linear representations of nonlinear dynamical systems
for control,” PLOS ONE, vol. 11, no. 2, pp. 1–19, 2016. [Online]. Available:
https://doi.org/10.1371/journal.pone.0150171[33] A. Surana, “Koopman operator based observer synthesis for control-aﬃne nonlinear
systems,” in 2016 IEEE 55th Conference on Decision and Control (CDC), 2016, pp.
6492–6499.
[34] D. Bruder, X. Fu, and R. Vasudevan, “Advantages of bilinear koopman realizations
for the modeling and control of systems with unknown dynamics,” IEEE Robotics andAutomation Letters, vol. 6, no. 3, pp. 4369–4376, 2021.
[35] M. Korda, and I. Mezić, “Linear predictors for nonlinear dynamical systems:
Koopman operator meets model predictive control,” Automatica, vol. 93, pp.
149–160, 2018. [Online]. Available: https://www.sciencedirect.com/science/article/pii/
S000510981830133X[36] E. Yeung, S. Kundu, and N. Hodas, “Learning deep neural network representationsfor koopman operators of nonlinear dynamical systems,” in 2019 American Control
Conference (ACC), 2019, pp. 4832–4839.
[37] E. M. Wolﬀ, and R. M. Murray, “Optimal control of mixed logical dynamical systems
with long-term temporal logic speciﬁcations,” 2013.
[38] M. Earl, and R. D’Andrea, “Iterative milp methods for vehicle-control problems,” IEEE
Transactions on Robotics, vol. 21, no. 6, pp. 1158–1167, 2005.
[39] M. Yamakita, K. Jimura, and K. Furuta, “Impedance posture control of robot manipulators using euler parameters,” in Proceedings of 35th IEEE Conference on Decision
and Control, vol. 2, 1996, pp. 1964–1966 vol.2.
[40] E. Süli, and D. F. Mayers, An introduction to numerical analysis. Cambridge university
press, 2003.
[41] L. Gurobi Optimization, “Gurobi optimizer reference manual,” 2021. [Online].
Available: http://www.gurobi.com

参考文献

81

[42] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi –A software framework for nonlinear optimization and optimal control,” Mathematical
Programming Computation, vol. 11, no. 1, pp. 1–36, 2019.
[43] L. Niu, and A. Clark, “Control barrier functions for abstraction-free control synthesis under temporal logic constraints,” in 2020 59th IEEE Conference on Decision andControl (CDC), 2020, pp. 816–823.
[44] D. Gundana, and H. Kress-Gazit, “Event-based signal temporal logic tasks: Execution
and feedback in complex environments,” IEEE Robotics and Automation Letters, vol. 7,
no. 4, pp. 10 001–10 008, 2022.
[45] A. Wächter, and L. T. Biegler, “On the implementation of an interior-point ﬁlter linesearch algorithm for large-scale nonlinear programming,” Math. Program., vol. 106,
no. 1, pp. 25–57, 2006.
[46] Y. Gilpin, V. Kurtz, and H. Lin, “A smooth robustness measure of signal temporal logic
for symbolic control,” IEEE Control Systems Letters, vol. 5, no. 1, pp. 241–246, 2021.
[47] R. Alur, and D. L. Dill, “A theory of timed automata,” Theoretical Computer Science,
vol. 126, pp. 183–235, 1994.
[48] X. Xu, P. Tabuada, J. W. Grizzle, and A. D. Ames, “Robustness of control
barrier functions for safety critical control,” IFAC-PapersOnLine, vol. 48, no. 27,
pp. 54–61, 2015, analysis and Design of Hybrid Systems ADHS. [Online]. Available:
https://www.sciencedirect.com/science/article/pii/S2405896315024106[49] K. Sekiguchi, and M. Sampei, “On multi time-scale form of nonlinear systems,”
IFAC Proceedings Volumes, vol. 46, no. 23, pp. 524–529, 2013, 9th IFAC Symposiumon Nonlinear Control Systems. [Online]. Available: https://www.sciencedirect.com/
science/article/pii/S147466701631713X[50] L. Niu, and A. Clark, “Control barrier functions for abstraction-free control synthesis under temporal logic constraints,” in 2020 59th IEEE Conference on Decision andControl (CDC), 2020, pp. 816–823.
[51] L. Lindemann, and D. V. Dimarogonas, “Control barrier functions for signal temporal
logic tasks,” IEEE Control Systems Letters, vol. 3, no. 1, pp. 96–101, 2019.
[52] K. G. Larsen, P. Pettersson, and W. Yi, “Uppaal in a nutshell,” Int. J. Softw.
Tools Technol. Transf., vol. 1, no. 1–2, p. 134–152, 1997. [Online]. Available:
https://doi.org/10.1007/s100090050010

82

参考文献

[53] F. Blanchini, “Set invariance in control,” vol. 35, no. 11, 1999, pp. 1747–1767. [Online].
Available: https://www.sciencedirect.com/science/article/pii/S0005109899001132[54] F. Blanchini, and S. Miani, Set-Theoretic Methods in Control, 1st ed. Birkhäuser Basel,
2007.
[55] S. Documentation, “Simulation and model-based design,” 2023. [Online]. Available:
https://www.mathworks.com/products/simulink.html[56] M. Charitidou, and D. V. Dimarogonas, “Signal temporal logic task decomposition via
convex optimization,” IEEE Control Systems Letters, vol. 6, pp. 1238–1243, 2022.
[57] Y. Chen, X. C. Ding, A. Stefanescu, and C. Belta, “Formal approach to the deployment
of distributed robotic teams,” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 158–
171, 2012.
[58] A. Paraschos, C. Daniel, J. R. Peters, and G. Neumann, “Probabilistic movement primitives,” Advances in neural information processing systems, vol. 26, 2013.
[59] S. Gomez-Gonzalez, G. Neumann, B. Schölkopf, and J. Peters, “Adaptation and robust
learning of probabilistic movement primitives,” IEEE Transactions on Robotics, vol. 36,
no. 2, pp. 366–379, 2020.
[60] A. Paraschos, C. Daniel, J. Peters, and G. Neumann, “Using probabilistic movement
primitives in robotics,” Autonomous Robots, vol. 42, pp. 529–551, 2018.
[61] E. Wan, and R. Van Der Merwe, “The unscented kalman ﬁlter for nonlinear estimation,”in Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No.00EX373), 2000, pp. 153–158.
[62] R. Kandepu, B. Foss, and L. Imsland, “Applying the unscented kalman ﬁlter
for nonlinear state estimation,” Journal of Process Control, vol. 18, no. 7, pp.
753–768, 2008. [Online]. Available: https://www.sciencedirect.com/science/article/pii/
S0959152407001655[63] R. Merwe, and E. Wan, “Sigma-point kalman ﬁlters for probabilistic inference in dynamic state-space models,” Proceedings of the Workshop on Advances in Machine Learning, 2003.

研究業績
研究業績 (発表順に掲載)
学術雑誌論文• Shumpei Tokuda, Masaki Yamakita, Hiroyuki Oyama, Rin Takano, Reblending of
Probabilistic Movement Primitives for Signal Temporal Logic Speciﬁcations, IEEE
Control Systems Letters (L-CSS). (submitted to L-CSS and CDC). (6 章に関連)

国際会議論文• Mizuho Katayama, Shumpei Tokuda, Masaki Yamakita, Hiroyuki Oyama, Fast LTL
Based Flexible Planning for Dual-Arm Manipulation, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020. (3 章 3.3 節に関連)• Shumpei Tokuda, Masaki Yamakita, Hiroyuki Oyama, Rin Takano, Convex Approximation for LTL-based Planning, IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS), 2021. (3 章 3.4, 3.6 節に関連)• Shumpei Tokuda, Masaki Yamakita, Hiroyuki Oyama, Rin Takano, Linear Temporal
Logic-based Mixed-Integer Linear Problem Planning with the Koopman Operator,
Annual Conference of the IEEE Industrial Electronics Society (IECON), 2022. (3 章
3.5, 3.7 節に関連)• Shumpei Tokuda, Masaki Yamakita, Hiroyuki Oyama, Rin Takano, Zeroing Control
Barrier Function with Time Scale Transformation for Time Interval Signal Temporal
Logic Task, IEEE Conference on Control Technology and Applications (CCTA), 2022.
(5 章に関連)• Shumpei Tokuda, Masaki Yamakita, Hiroyuki Oyama, Rin Takano, Robust Signal
Temporal Logic-based Planning for Uncertain External Events, IEEE International
Conference on Robotics and Biomimetics (ROBIO), 2023. (4 章に関連)

83博士論文に関係しないもの
国際会議発表論文• Shumpei Tokuda, Masaki Yamakita, Hierarchical Linearization and Output Zeroing
Control for Wheeled Robots, Annual Conference of the Society of Instrument andControl Engineers of Japan (SICE), 2019
• Shumpei Tokuda, Masaki Yamakita, Hiroyuki Oyama, Generating New Lower Abstract Task Operator using Grid TLI, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.

国内会議発表論文• 大山博之, 伊藤岳大, 佐藤峰斗, 片山瑞穂, 徳田俊平, 山北昌毅, 時相論理を用いた双腕ロボットの自律制御, 第 7 回 制御部門マルチシンポジウム (MSCS2020), 2020

特許
• 日本電気株式会社, 国立大学法人東京工業大学, 高野凜, 大山博之, 山北昌毅, 徳田俊平,
制御システム、制御方法およびプログラム, 特開 2023-047980, 2023

謝辞
本論文は東京工業大学工学院システム制御系山北研究室と日本電気株式会社 データサイ
エンス研究所との共同研究の成果を元に作成されており, 本研究を遂行するにあたり，様々な方々からのご助言やご指導などを賜りました.
特に, 指導教員・論文審査員主査としてご指導・ご助言をくださいました山北昌毅准教授に心より御礼申し上げます. 学士課程から博士課程までの 6 年間の長い間, 数学や制御工学など様々な学問の知識に加えて, 論文執筆や学会発表の方法, 研究の進め方など様々なご指導を賜りました. 6 年間のご指導のおかげで, 多岐にわたる分野の知識や研究に対する考え方に関して多くのことを学び, 研究者として成長することができたと実感しています. 重ねて感謝申し上げます.
本論文の審査員を務めていただき，丁寧な論文審査の過程で様々なご助言をくださいました倉林大輔教授, 三平満司教授，田中正行教授, 畑中健志准教授に深く感謝申し上げます．
日本電気株式会社 データサイエンス研究所の皆様には共同研究を行っていただき，大変お世話になりました．研究の方向性や内容に関する多くの議論を通じて, 研究の質を大きく向上させることができました. 深く感謝申し上げます. 特に大山博之様, 高野凛様, 田屋裕輝様には, 様々な面でご指導・御鞭撻を賜りました．大山様, 高野様には研究室の OB として, 田屋様には学生時代の同期として, 共同研究以外でも学生生活における相談にのっていただき,
大変お世話になりました.
カンボジア工科大学 Sarot Srang 博士とは, UAV 制御の共同研究ができたことを感謝申し上げます. また, カンボジアにお招きいただき講演する機会をいただいたことを嬉しく思っております.
学会の手続きや共同研究等の事務手続きに関して大変お世話になりました小林真理事務官に心よりお礼申し上げます．山北研究室での研究生活において様々な議論をするとともに，日々の充実した生活を共に過ごした山北研究室の学生の皆様に感謝申し上げます．特に, 共同研究グループにて共に研究を行った, 片山瑞穂氏, 福田健二氏とは互いに研究内容について相談することができ, 結果として研究を進める手助けになったと思います. ありがとうございました. また, 山北准教授の奥様には様々な点でご配慮いただき, 大変お世話になりました．
深く感謝申し上げます．加えて, 学士課程から博士課程まで同期として学生生活を共に過ごした 久米啓太氏, 持田峻佑氏, 谷口晃大氏, 横山陽彦氏に感謝申し上げます.
最後に，精神的・経済的に私を支えてくれた家族に心よりお礼申し上げます．

なお，本研究は JST SPRING, Grant Number JPMJSP2106 の助成を受けたものです．ここに記し，感謝の意を表します．

85