特集

計測自動制御学会産業論文集
Vol.10, No.23, 190/197（2011）

特集「移動体の誘導制御」

経路の道幅を考慮した脚車輪型ロボット車輪位置のモデル予測制御
大川功∗ ・野 中謙一郎∗∗

Model Predictive Control for Wheel Position of Leg/Wheel Robots considering Passage Width
Isao Okawa∗ and Kenichiro Nonaka∗∗

This paper describes a model predictive control (MPC) application which optimizes wheel positions of a mobile
robot with geometry-variable wheels. Each wheel should be set apart from the others to keep enough stability
for safety operation of mobile robots. However, this requirement tends to decreases the mobility. In this paper,
we propose a method which optimizes wheel positions with respect to passage width using MPC. The proposed
method realizes a good balance between stability and mobility by online optimization of wheel positions during
automatic collision-free travel. Moreover, we reduced the computational cost of MPC by introduction of a control
method for wheel positions with respect to travel distance. Thus it is capable to implement the control system
into low speed computers, like embedded CPUs. We conducted experiments to verify the eﬀectiveness; it is
conﬁrmed that the proposed method is useful to operate the mobile robot with enough stability and mobility.
Key Words: Mobile robot, Optimal control, Wheel position, Model predictive control, Embedded CPU

輪位置の変更や脚による歩行を可能とした研究 3) も報告されてい1. は じ め にる．しかし車体安定性の更なる向上のためには現在時刻における車輪による移動方式は移動時のエネルギー効率が高く，自動車などの輸送機器や移動ロボットに広く採用されている．これらの車両は人や荷物の高速な輸送や各種のマニピュレータによる作業などをそれぞれ目的とする場合が多く，安定性を確保するためにホイールベース・トレッドを十分に広くする必要がある．この結果として，車体が大型化し狭隘な空間での移動・作業が困難となるといった問題が生じる．この問題は自動車などにおける車体の小型化に対する要求の高まりや，移動ロボットの高機能化に向けた搭載機器の増加に伴う高重心化によって顕著となることが予想
この問題に対する一つの解決策として，これまでに車輪位置を可変とすることで状況に応じた走行特性を得る方法が提案されてきた．例を挙げると，先端に能動車輪を持つ 2 リンク脚を変形させることで車体安定化制御を行う研究 1) では，車両の加速度に応じて車輪位置を変化させることで車体の安定化を行っている．
また車両の幅を変更可能な全方向移動車両を車椅子に応用した研究では，重心の移動に応じてホイールベース・トレッドの比を変更することで転倒の危険性を低減させている．さらに脚と能動車輪から構成されるホイールモジュールを複数搭載することで車∗基づいた制御を行い車輪位置を滑らかに変化させる必要がある．
そこでわれわれはこれまでに車輪位置を任意に変更可能な車両
型ロボットを用いて，通路幅の予測に基づく車輪位置の最適化制
御をオンラインで行う手法を提案してきた．提案手法は機体が十分に広い通路を走行する際は車輪間隔を可能な限り広げることで走行時の安定性を高め，障害物付近を走行する際や狭隘な通路では車輪間隔を狭めることで機動性を確保し，安定性と機動性の両立を図る．過去の研究 4) では機体のトレッドを通路幅の予測に基づき数値的に最適化する手法を提案し，一定の安定性を維持しなされ，何らかの対策を講じる必要がある．

2)

加速度や重心位置などの静的な指標に基づく制御に加え，予測に

元 東京都市大学大学院工学研究科 世田谷区玉堤 1-28-1∗∗ 東京都市大学工学部 世田谷区玉堤 1-28-1∗ Graduate School of Engineering, Tokyo City University∗∗ Faculty of Engineering, Tokyo City University(Email: knonaka@tcu.ac.jp)（Received November 30, 2010）

がら隘路を通過させることに成功した．しかし最大限の機体安定性を得るためには機体から離れた位置に置くべき車輪も機体側へ引き寄せており，さらにデスクトップ PC を計算に用いても制御
周期は 10 Hz 程度と計算コストにも課題を残していた．
本論文では制御対象を各車輪の位置として独立に制御し，さらに車輪位置の最適化を計算コストを低減させたモデル予測制御によって行うことで，より高いレベルでの安定性と機動性の両立を実現する手法を示す．モデル予測制御については比較的計算コストの低い方法として C/GMRES アルゴリズム 5) や入力の時間微分を新たな入力として再定義する手法 6) に加え，速度と精度を向上させた修正シューティング法 7) など多くの手法が報告されているが，デスクトップ PC 程度の処理能力を有す計算機を必要とす
ることが多い．これに対してわれわれは脚の逆運動学計算をモデ
ル予測制御から分離し，道程に対する車輪位置の変化を制御入力

c 2010 SICE
TRIA 023/11/1023 

とする手法を導入することで計算コストを大きく低減させ，組込め，本研究では 3 つの最適化問題について独立に最適性を考える．

み CPU のみを制御に用いたオンライン計算の実装に成功した．

Fig. 2 は制御システムのフローチャートである．まずセンシン

2. システム構成

グユニットによって自己位置・姿勢の推定と共に障害物の検出とポリゴンへの近似を行い，この結果を組込み CPU へ送信する．

2. 1 実験機

障害物の検出は回避動作を必要とする機体周辺の障害物のみに対

本研究では Fig. 1 に示す，脚車輪型ロボット 9), 10) を実験機

し，測域センサによって得られた走査データを基に行う．測域セとして使用している．Fig. 1 (a)，(b) はそれぞれ機体全体の鳥瞰

ンサは 240◦ の範囲に対して最長 5 m までの距離を 100 ms の周図，右前の脚部分を示す．本機体は先端に DC モータによって駆期で走査可能であり，これを 2 台使用して全方向の障害物を検出動する能動車輪を有する，3 リンクスカラ型の脚を 4 本持つ．こする．つぎに組込み CPU において，障害物の配置に対して最適の脚を変形させることで，各車輪を水平面上の可動範囲内で任意な回避経路の生成を行い 8) ，この結果に基づいて経路を追従可能に配置しての走行や全方向移動が可能となっている．また本研究な機体速度，角速度と通路幅を計算する．なお実験機の姿勢角はでは車輪が常に機体の外郭に配置されるように制御を行うため，任意に指定することが可能であるが，本論文では姿勢角が常に回

車輪位置の変化に伴い機体の縦横方向のサイズが変化する．具体

避軌道の接線方向を向くように角速度を制御している 9) ．続いて的なサイズやその他のパラメータは Table 1 の通りである．な

通路幅に対して最適な参照車輪位置をモデル予測制御によって計

お脚関節のアクチュエータとして角度指示型の RC サーボモータ

算し，経路を追従可能な各車輪の速度と参照車輪位置を実現するを使用し，運動学に基づいて関節角度の制御を行っているため，最適な脚の関節角度を逆運動学計算によって決定する 10) ．そし

詳細な質量や慣性モーメントの値は制御に必要としない．

てサーボモータへ関節角度を送信し，DC モータへは各車輪の速制御用の演算装置としてはルネサスエレクトロニクス社製の組度から計算したトルクの値をそれぞれ送信する．このように制御

込み CPU の SH7058F を搭載しており，動作周波数は 80 MHz，に関する計算はすべて組込み CPU において計算される．本論文

内蔵メモリは 1024 kB である．これは自動車の燃料噴射制御にで扱う内容は障害物回避経路の生成と通路幅の計算，そして参照

用いられているものと同型であり，頑強で信頼性が高いがオンラ

車輪位置のモデル予測制御による最適化とする．その他の要素にインでの障害物回避経路生成やモデル予測制御を実装するためについては著者らの過去の研究 8)〜10) を参照していただきたい．

は計算コストの低減が必要である．本研究では提案手法の計算コ

後者の参照車輪位置の最適化においては，通路幅に対して最適

ストの低さを示すために，この CPU を 1 台のみ使用して提案制な関節角度を直接求めることも考えられるが非線形システムの最御手法を実現する．センサ類としてはレーザー測域センサ 2 台

適制御問題を解くことになり，さらに車輪位置の変更には一定の（北陽電機社製，URG-04LX-UG01）とセンサのドライブ用小型

時間を必要とするため入力に関する拘束条件を考慮する必要があコンピュータで構成されるセンシングユニットを搭載しており，Table 1 Speciﬁcation of experimental robot

自己位置・姿勢の推定と障害物の検出が可能である．ただしセンParameter
Length, Width, Height
Tire diameter
WeightMax. longitudinal velocity
Max. lateral velocity
Max. angular velocity

サドライブ用小型コンピュータはセンシングの用途のみに使用
し，制御には用いていない．

2. 2 制御システムの概要
提案手法は最適な回避経路の生成と追従，モデル予測制御による通路幅に対して最適な参照車輪位置の計算，そして参照車輪位置を達成しながら回避経路の追従を可能とする最適な関節角度

Value500∼700, 290∼700, 230 mm
57 mm
7.7 kg
1.0 m/s
1.0 m/s
3.0 rad/sの計算という，3 つの最適化問題を階層的に行う．これらを一つの最適化問題として扱うことは計算コストの観点から困難なた

(a)

(b)

Fig. 1 Experimental robot

Fig. 2 Flowchart of the control system191

る．そのため既存の数値解法を用いて回避経路の生成と共にオン

望ましい経路へ復帰することや，急激な変化が無く滑らかな経路

ライン計算を行うことは困難であると考えられる．そこで本研究であることが望まれる．これらの要求に対しては，たとえば人工では計算コストを低減するために，通路幅に対して最適な関節角ポテンシャル場法 11) のような既存の方法によっても達成可能で度を求める問題を，通路幅に対する最適な参照車輪位置をモデルある．しかし本研究では回避経路の法線方向に対して通路幅の測予測制御によって求める問題と，参照車輪位置に対して最適な関定を行うため，通路幅の測定範囲において区分的に滑らかな関数

節角度を逆運動学に基づいて数値的に計算する問題に分離させてで回避経路を表現する必要がある．また通路幅の測定範囲を最適扱う．モデル予測制御は後述する道程に対する車輪位置の変化をな参照車輪位置を計算する際の評価区間以上とする必要から，回制御入力とする手法の導入によって線形システムの 2 次形式評

避経路も機体中心から一定の範囲における障害物の配置を考慮し価関数に対する拘束条件なしの最適制御問題に変換することが可て決定する必要があり，このような経路を既存の手法によってオ能であり，繰り返し計算を行わずに最適解を得ることが可能となンラインで生成することは難しいと考えられる．そこで本研究でる．また逆運動学計算は操舵角に関する拘束や複数の解が存在すは回避経路を多項式曲線として表現し，各項の係数を障害物の配るなどの特徴から数値的に行う必要があるが計算コストの低減が置に対して最適な回避経路となるように数値計算によって決定す

行われており，組込み CPU のような小規模な演算装置においてる．本論文では 3 次多項式関数を回避経路として導入する．

も十分高速に計算可能である．

ỹ (x̃) = k3 x̃3 + k2 x̃2 + k1 x̃ + k0

3. 座標変数の設定(1)

最適化の対象は k3 ∼ k0 である．多項式の次数は任意に設定すFig. 3 に以下の議論で使用する座標系と変数を示す．まずることが可能であるが，計算コストの観点から 3 次とした．多Fig. 3 (a) において，X-Y 座標は慣性座標系であり，x-y 座標

項式曲線は少ないパラメータで滑らかな経路の表現が可能で，3はロボット固定座標系を表す．また慣性座標系に対するロボット

次以上の多項式は障害物回避と目標経路への復帰が可能なので，固定座標系の回転角を θ とし，ロボットの慣性座標を (X, Y, θ)(1) の係数を最適化すれば上記の要求は全て満たされる．

と表す． xij , y ij は車輪のロボット固定座標であり，右肩の添

また経路の更新は毎回の制御周期ごとに行われ，更新の際に前え字は各車輪について，00:左前，01:右前，10:左後，11:右後を回の結果と連続した経路を生成するためには k1 と k0 が以下の境表す．つぎに x̃-ỹ 座標は原点が常に機体中心にあり，方向が慣性

界条件を満たす必要がある．なお ỹp (x̃) は前回生成した経路で座標と一致する座標系である．本研究では，たとえば廊下を一方あり，ΔX は経路を更新するまでの機体 X 座標の変化量である．

向に走行する場合のように，慣性座標の少なくとも一つが単調増加する状況を想定している．この想定の下では障害物回避経路を
曲線 ỹ (x̃) として x̃-ỹ 座標上で生成することで計算が簡単化される．続いて Fig. 3 (b) において，sij 軸は各車輪の中心を原点とdỹp (x̃) 
k1 =dx̃ x̃=ΔX(2)

k0 = ỹp (ΔX)

(3)

する障害物回避経路に沿った座標軸であり，4 つの車輪について

このように低次の 2 項については解析的に係数が決定されるたそれぞれ設定する．また ys (x̃) は任意の x̃ 座標において障害物

め，数値計算の対象は k3 と k2 の 2 つとなる．なお経路を慣性座回避経路に直交する座標である．

標系における曲線として生成することも可能であるが，(2)，(3)式に対応する計算が複雑化する．

4. 障害物回避経路の生成

4. 2 パラメータの数値最適化

4. 1 多項式経路の導入

k3 と k2 の最適化は評価関数を最小化する値を探索することで障害物回避を伴う移動ロボットの自律移動経路としては障害物行う．評価区間を x̃ ∈ [0, x̃h ] として Nx̃ 個の評価点に離散化し，との衝突を避けることに加え，障害物が存在しない場合に本来のx̃k = x̃h · k/Nx̃ (k = 1, 2, · · · , Nx̃ ) を k 番目の評価点におけるx̃ 座標の値とする．また ỹd (x̃) を機体の周囲に障害物が存在しな
い場合に最適な経路となる基準経路とし，dn (x̃) (n = 1, 2, · · · )を Fig. 4 に示す n 番目の障害物までの最短距離 4) として，k3 ，

k2 をつぎのように更新する．
 2
  
J1 = ỹ x̃k − ỹd x̃k2 
dỹ (x̃) 
J2 =


dx̃
k

(4)
(5)

x̃=x̃

No

J3 =
(a)

(b)

1
n (x̃k )2
d
n=1

(6)

Nx̃

J (k3 , k2 ) :=Fig. 3 Coordinates and variables(J1 + w2 J2 + w3 J3 )
k=1

192

(7)


a (x̃)  =
x̃n
ỹn


=

1+
x̃
ỹ



dỹ (x̃)
dx̃

2

|Δỹ|

+ a (x̃) · ρ

(10)
(11)

つぎに polygon (x̃n , ỹn ) を点 (x̃n , ỹn ) が障害物の内側ならば 1，外側ならば 0 を返す関数とし，計算点 (x̃, ỹ) において

polygon (x̃n , ỹn ) = 1 を満たす ρ を回避経路の左右に対して以下のアルゴリズムで求める．ただし |Dmax | は通路幅の絶対値の上限であり，車輪位置の可動範囲に対して十分に大きく設定する．
Fig. 4 The shortest distance to an obstacle

Algorithm 1 Find ρ such that polygon (x̃n , ỹn ) = 1(k3 , k2 ) = arg min J (k3 , k2 )Input: x̃, ỹ
Output: ρ
1: Initialize as ρ = 1
2: Calculate a (x̃) by equation (9)3: Calculate a (x̃)  by equation (10)4: while a (x̃)  · ρ ≤ |Dmax | do
5:
Calculate x̃n , ỹn by equation (11)6:
if polygon (x̃n , ỹn ) = 1 then7:
return ρ
8:end if9:ρ←ρ+1
10: end while
11: return ρ(8)

J1 は経路を基準経路に近づけるための評価，J2 は経路の急激な変化を抑えるための評価，J3 は障害物への接近に対するペナルティをそれぞれ表している．また w2 ，w3 は評価の重みを表す正の定数である．(7) 式の評価関数を最小にする k3 と k2 が，J1 ∼ J3の各評価をバランスよく小さくする最適な係数となる．(8) 式の数値計算は BFGS アルゴリズムによる準ニュートン法 12) によって実装しており，(7) 式の評価関数は値や勾配の計算において初等関数の計算を一切必要としない．また障害物が静止しており検出が理想的に行われる場合，k3 ∼ k0 がステップ状に変化することは無いため数値計算の初期値に前回の結果を用いることで計算

ここで計算点 (x̃, ỹ) における通路幅を D (x̃) とする．このとの高速化が見込める．特に，浮動小数点演算ユニットの実装が十き D (x̃) は先の計算で求めた ρ を用いて次式で計算できる．

分ではない組込み CPU においても高速な演算が可能である．

D (x̃) = ±a (x̃)  · ρ

5. モデル予測制御による車輪位置最適化(12)

なお (12) 式の符号は Δỹ と同様に回避経路の左側に対して正，5. 1 通路幅の計算

右側に対して負とする．以上の計算をモデル予測制御で考慮されまず通路幅の計算方法について述べる．通路幅は回避経路上のる範囲に対応した計算点において行うことで，通路幅が計算され測定点における法線の方向に，経路の左右それぞれに対して計算る．本手法は平方根の計算回数が ND 回で実行可能であり，比を行う．計算を行う範囲はロボットの後輪位置周辺の通路幅も考較的高速に計算を行うことができる．また (10) 式の平方根部分慮するために回避経路生成における評価区間よりも後方に広く設をマクローリン展開することでより高速に計算を行うことも可能

定し，計算点の総数を ND とする．ここで浮動小数点演算ユニッであるが，誤差が拡大する．よって本研究では精度を優先し，すトの実装が十分ではない計算機においても高速な演算を実現する

べての計算点において平方根の計算を行う．

ためには，初等関数の計算回数を可能な限り減らす必要がある．

5. 2 車輪位置の最適化

最も単純な計算方法として法線の関数を解析的に導出して障害物続いて最適な参照車輪位置をモデル予測制御によって求める方を近似した多角形の辺との交点を求める方法が考えられるが，こ法について述べる．本論文では機体のロール方向の安定性に対しの方法では平方根の計算を必要とする交点 – 計算点間の距離計算て支配的な y ij 座標のみを制御の対象とし，xij 座標は固定する．

を左右合わせて 2ND 回行う必要がある．そこで，われわれは比

また車輪は可動範囲内で任意の配置とすることが可能であり，車較的高速に計算可能な方法として，以下のように計算を行う．

輪位置の通路幅に対する最適性と参照車輪位置に対する最適性を同一と見なせると仮定する．ここでつぎのシステムを考える．

まず回避経路上の任意の計算点 (x̃, ỹ) における法線に沿った，回避経路の左側を向いたベクトルを a (x̃) = [Δx̃, Δỹ]Tdy ij (t)= uij (t)dtとする．このとき a (x̃) と a (x̃) ，さらに ρ を媒介変数とするパラ(13)

メータ表示での法線の方程式は，つぎのように表現できる．なおしかし車輪位置の変化には一定の時間を要するため，uij (t) はつ(x̃n , ỹn ) は法線上の点の座標を表し，Δỹ は経路の左側では正，ぎの不等式拘束条件を満足する必要がある．ただし |umax | は車

右側では負の値として必要とする計算精度に応じて設定する．

輪位置変化速度の絶対値の上限である．

dỹ (x̃)a (x̃) = [Δx̃, Δỹ] = −Δỹ, Δỹdx̃
T ij 
u (t) ≤ |umax |

T

(9)
193

(14)





れ y ij sij の可動範囲における最小，最大の値であり，左右の不等式拘束条件をモデル予測制御で扱う方法としては，(i) ダ
ミー入力を導入して等式拘束条件に変換した後にラグランジュの

車輪で符号と大きさが対称の値とする．

未定乗数法を用いる方法，(ii) ペナルティ関数を評価関数に加えつぎに評価区間に対応する範囲の通路幅をもとに，目標状態量

る方法，などが挙げられる．しかしこれらの方法では非線形な関の計算を行う．本研究では目標状態量を評価区間全体に渡って障数を利用することが多く，解析的なアプローチが困難になる．

害物や通路壁面への衝突を避けられる一定の値として求めること

そこで，つぎの sij 軸に関する微分方程式を導入する．なお sij

で，ステップ状に変化する目標値に追従させる問題とする．(12)座標は機体の移動に伴う各車輪の道程に対応する．

式で求めた通路幅は x̃ 座標に対する関数として表現されるため，  dy sij
ij
sij
(15)
=
μ
dsij
 


このとき uij (t) と μij sij の関係は V = dsij /dt を機体の評価区間に対応する範囲の通路幅を得るためには x̃ 座標 – sij 座

ij

標間の関係を考える必要があり，この関係は次式のようになる．

移動する速さとして，次式で表される．

 
uij (t) = μij sij · V



dsij
=
dx̃

1+

dỹ (x̃)
dx̃

2
(20)

また s 軸の原点は車輪の中心に設定しているため，sij = 0 に対(16)応する x̃ 座標はつぎのように求まる．

(16) 式より，V を変化させることで uij (t) を調節することが可x̃ = xij cos θ − y ij sin θ

能なことがわかる．よって (14) 式の拘束をすべての車輪に対し

(21)

 
て満足する上限の V は，V ij sij を各車輪について拘束を満足

ただし本研究ではロボットが (5) 式の効果によって傾きが小さくする上限の速さ，Vd を速さの指令値として次式で計算できる．

なった回避経路の接線方向を向いて走行するものとして，初等関

⎧
⎨ Vd
  ⎪


V ij sij =  umax ⎪⎩ ij ij 
μ (s )

V = min V

ij



i,j

s

ij


 
if |umax | ≥ μij sij  · Vd 
if |umax | < μij sij  · Vd数の計算回数を減らすために θ  0 と見なす．一般の θ = 0 はルックアップテーブルで高速に近似計算可能である．このとき x̃
座標から sij 座標への変換は (20) 式を積分することで次式とし

(17)



て得られる．

(18)sすなわち (15) 式のシステムを使用することで，線形システムの

ij



 x̃
=

xij

1+

dỹ (x̃)
dx̃

2
dx̃

(22)

拘束条件なしの最適制御問題に変換することができる．以上よここで通路幅の測定を行う x̃ 座標を x̃l (l = −1, 0, 1, 2, · · · ) とり，本研究では (15) 式のシステムに対してモデル予測制御を適して，Δx̃ ごとに測定するものとする．まずは θ  0 の近似によ  
用し，以降では y sij を状態量，μij sij を入力として扱う．

る誤差を考慮して x̃ = xij − Δx̃ における通路幅を l = −1 の結ij

果として保存し，そして l = 0 以降の通路幅を (22) 式を用いてまずはオンライン計算における繰り返しの冒頭にて，評価区間
を状態量のモデル予測制御における初期値 y ij (0) に応じて動的

以下のアルゴリズムで求める．

かつ各車輪に対して独立に設定する．これは評価区間を一定とす 
Algorithm 2 Measure D x̃l

ると障害物が評価区間よりも僅かに広い間隔をおいて点在するような環境において車輪位置が振動し，かえって機体の安定性をInput: xij 
Output: Series of D x̃l
1: Initialize as sij = 0, l = 0, x̃l = xijij
2: while sij ≤ sh do 3:Calculate D x̃l by equation (12) l4:Store D x̃5:
Calculate dsij /dx̃ by equation (20)6:sij ← sij + dsij /dx̃ Δx̃
7:x̃l+1 ← x̃l + Δx̃
8:l ← l+1
9: end while

失うことを避けるために必要な手順である．障害物を検出して車輪を機体側に引き寄せる際は複数の障害物が連続して存在する可能性を考慮し，評価区間を伸長した上で最小通路幅を検出し，車輪位置が可能な限り振動しないようにする．一方で障害物が検出されない場合は，不必要に評価区間が長いと遥かに前方の障害物を検出して車輪位置が縮むことになるので，評価区間を必要最





ij
低限に縮める．以上より評価区間を sij ∈ 0, sijh とし，sh を

y ij (0) の関数として次式のように変化させる．
⎧

smax − smin  ij
ij⎪⎪
y (0) − ymin
+ smax⎪− ij
ij⎪
ymax − ymin
⎪
⎪
⎪
⎪
⎨
(w.r.t. left side)

sij
=
h
smax − smin  ij
ij
⎪
⎪ ij
y
+ smax
(0)
−
y
⎪
max
ij
⎪
⎪
ymax − ymin⎪⎪⎪⎩(w.r.t. right side)(20) 式は (10) 式の平方根部分と一致するため実際には計算する必要がなく，初等関数の計算回数を抑えることができる．以上(19)で求めた通路幅をもとに，目標状態量 ydij を次式で決定する．

 ydij = min D x̃l + 
l(23)

ただし smin ，smax はそれぞれ最短，最長の sijh であり，すべて

 は通路幅に対する余裕を表すパラメータであり，機体左側の車

ij
ij
，ymax
はそれぞ
の車輪に対して同一の正の値とする．また ymin輪に対しては正，右側の車輪に対しては負の値を設定する．また

194

目標状態量が車輪の可動範囲を超えた場合は値を飽和させる．



以上の計算の後に，最適な入力の計算を行う．ここで e sij := 
y ij sij − ydij を状態量と目標状態量の誤差として定義し，評価関数をつぎのように設定する．なお以降では標記を簡単にするために添え字 ij は省略する．

 shJ := φ (e (sh )) +0

L (e (s) , μ (s)) ds

1
Sf e (sh )22
1
1
L (e (s) , μ (s)) = Qe (s)2 + Rμ (s)222
φ (e (sh )) =

(24)
(25)
(26)

(24) 式の評価関数に対して最適な入力は，滑らかな車輪位置の変化によって状態量を目標状態量に追従させる入力となる．また

Sf ，Q，R は重みを表す正の定数である．続いて λ を共状態量
Fig. 5 Experimental environment

として，ハミルトニアンをつぎのように定義する．

Table 2 Experimental parameters

H (e (s) , μ (s) , λ (s)) = L (e (s) , μ (s)) + λ (s) μ (s)Parameter
x̃h
Nx̃
w2
w3
Δỹ

(27)
ここで変分法に基づき 1 次の最適性の必要条件を求めると，次式が得られる．

de (s)= μ (s) , e (0) = yr (0) − yddsdλ (s)= −Qe (s) , λ (sh ) = Sf e (sh )ds∂H= Rμ (s) + λ (s) = 0
∂μ

(28)

|Dmax |
|umax |
Vd
smin
smax

(29)
(30)

(28)∼(30) 式の連立微分方程式の解となる μ (s) が最適な入力となる．(28)∼(30) 式を解くために (30) 式を μ (s) について解い−e (0) = yr (0) − yd ,λ (sh ) = Sf e (sh )

ij
ymin
ij
ymax

Δx̃

Sf
Q
R
Δt

0.05 m (L)-0.35 m (R)
0.35 m (L)-0.05 m (R)0.2 m-0.2 m (L)
0.2 m (R)
0.1
1.0
0.05
1 ms

更新することで，最適な値へと更新される．なお Δt は参照車輪

⎤

1
e (s)
R⎦
λ (s)
0

 ⎡
0
d e (s)=⎣ds λ (s)−Q

Parameter Value

参照車輪位置は最適な入力を μo := μ (0) として次式のようにた結果を (28) 式に代入すると，つぎの e (s) と λ (s) に関する常微分方程式が得られる．

Value
1.0 m
5
0.8
0.01
0.05 m (L)-0.05 m (R)
0.5 m
0.2 m/s
0.8 m/s
0.7 m
0.9 m

位置の更新周期である．

y ij ← y ij + μo V Δt

(35)

以上のように，本手法はモデル予測制御の最適な入力を求めるた(31)

めに繰り返し計算を必要としない．また初等関数の計算回数も少(31) 式はリカッチ微分方程式の解 S (s) を用いて繰り返し計算をなく，組込み CPU においても高速に最適な参照車輪位置を計算行わずに解くことが可能であり，本手法の場合 S (s) は次式を数可能である．

値積分することで得られる．

dS (s)
S (s)2=−Qds
R

6. 検 証 実 験(32)

6. 1 実験環境

なお (32) 式は解析的に S (s) について解くことも可能であるが，提案手法を実験機へ実装し，システムの有効性を制御性能と計算解に現れる双曲線関数の計算を避けて以降の計算を簡単化するたコストの低さの観点から検証する．Fig. 5 は屋内に設置した実験

めに本研究では数値的に解く．このとき e (s) は S (s) を含む次フィールドと障害物の配置を表しており，車輪位置を最適化しなが式を数値積分することで得られる．

ら 2 つの障害物を回避した後にボトルネック部分を通過させる実験

de (s)
S (s)
=−
e (s)
ds
R

を行う．回避経路の生成における基準経路を ỹd (x̃) = 2.5 − Y m(33)(Y = 2.5 m) の直線とし，他の各種パラメータについては Table

そして λ (s) は e (s) と S (s) から，つぎのように計算できる．

λ (s) = S (s) e (s)2 のように設定した．なお表中の (L)，(R) はそれぞれ機体の左側，右側の車輪に対する値であることを表している．

(34)

e (s) と λ (s) を (30) 式に代入し，再び μ (s) について解いたものが最適な入力となる．

195

6. 2 実験結果を実現可能な，計算コストの低い手法であると判断できる．

実験結果を Fig. 6∼Fig. 10 に示す．機体と障害物との位置関

3.5係の比較を容易にするために，Fig. 6∼10 の横軸はすべて機体中
心の X 座標として統一している．まず Fig. 6 は慣性座標におけ3る機体中心と各車輪の軌道を表しており，中央の曲線が機体中心の軌道，上下の曲線が各車輪の軌道である．また軌道の上下に描

]
m
[ 2.5
Y

かれた長方形は障害物の輪郭を表しており，軌道を跨ぐ四角形
は各車輪の中心を直線で結んだものである．Fig. 6 から車輪位置
を通路幅に対して変化させながら障害物回避を行っていること

2が確認でき，障害物付近を走行する際は障害物の表面に対して

 = ±0.2 m 程度の余裕をもって通過していることがわかる．ま1.5

た 2 つ目の障害物とボトルネックを形成する障害物との間隔は

0

1

2

3

0.85 m であるが，この区間において左側の車輪の位置は変化しijていない．この際は評価区間が sh = smin = 0.7 m から伸長さ

X [m]

4

5

6

7

Fig. 6 Trajectories of the robot and wheels

れており，sijh を固定とすると車輪位置が振動する．したがって，
評価区間を変化させることの効果が確認できた．
つぎに Fig. 7 は回避経路のパラメータ k3 ，k2 ，k1 ，k0 の変化を表し，Fig. 8 はモデル予測制御における最適な入力 μij
o の変化

0.3

を表す．Fig. 7 において k3 ，k2 ，k1 ，k0 がステップ状に変化す

0.2

ることはなく，更新を行う前後では連続した経路が生成されていることがわかる．Fig. 8 は Fig. 6 との比較から，最適解の変動が

0.1

k0
,1
k
,2 0
k
,3
k

回避経路の生成とモデル予測制御の評価区間に障害物が出入りする瞬間において大きくなっていることがわかる．また Fig. 8 において μijo は対応する車輪が障害物近傍に達するまでに収束して-0.1

おり，車輪位置の変化が通路幅の減少する位置においてすでに完-0.2了していることがわかる．
続いて Fig. 9 は μijo から求めたロボットの移動する速さ V の

k3
k3

-0.3
0

理論値と，(14) 式の拘束条件を満たす上限の速さを表している．

1

2

3

X [m]

4

k2
k2

5

k1
k1

k0
k0

6

7

Fig. 9 より，減速が必要な場合には拘束条件を満たす最大の速さ
Fig. 7 k3 , k2 , k1 , k0 of the collision-free trajectory

で走行させていることが確認できる．また Fig. 8 との比較から，減速させる位置は車輪位置の変化が急激な位置と対応していることがわかる．以上より，滑らかに障害物を回避しながら基準軌道に追従する経路が生成され，車輪位置も目標位置に追従するよう

0.8に最適化されているため，目標とする制御が達成されていることが確認できる．よって提案手法は，制御性能の上では目的とする

0.450Hz であり，やはり評価区間に障害物が出入りする瞬間におい

]no
isn 0
eim
do -0.4[Nμ-0.8て低下がみられる．しかし本研究では動力学に基づく制御のよう-1.2システムを十分に実現していると判断できる．
そして Fig. 10 は回避経路最適化，参照車輪位置のモデル予測
制御，脚関節の数値計算による逆運動学計算の全体での制御周波数を表している．最小の値は X = 2.7，4.0 m 付近における

ij o

な高い制御周波数を必要とする制御手法を用いずにシステムを構-1.6

築しており，また例として V = 0.8 m/s で走行しながら 50Hz

Front-left0

1

2

Front-right3

4

Rear-left5X [m]の周波数で制御を行った場合では機体が 16 mm 進むごとに最適解が更新される．よって計算コストの観点からも実用上十分な性

Fig. 8 Optimal inputs computed by MPC

能が達成されていると考えられ，提案手法は組込み CPU のような小規模な演算装置への実装が可能であり，さらには回避経路の生成や車輪位置の制御を伴いながらも比較的速い速度による移動

196

Rear-right
6

7

参

1

]s 0.6
/
m
[
V0.4
0.2

V
0

1

2

3

4

5

Upper limit of V
6

7

X [m]
Fig. 9 Locomotion speed

700
600
500

]z
H[ 400kc
ol 300C
200
100

Clock of the optimal control

0
0

1

2

3

X [m]

4

5

6

文

献

1）高橋隆行，横田尚大，王志東，森善一，中野栄二：車両幅可変型
全方向移動ロボットの機構と制御，日本ロボット学会誌，19-5，
638/645 (2001)2）M. Wada and H. Asada: A Holonomic Omnidirectional
Vehicle with a Reconﬁgurable Footprint Mechanism and
Its Application to Wheelchairs, Proceedings of 1998 IEEE
International Conference on Robotics and Automation,
774/780(1998)
3）大和秀彰，奥村悠，古田貴之：ホイールモジュールによる移動機構を備えた次世代自律移動ロボット Hallucige-nia01 ，計測と制御，45-3，254/259 (2006)
4）大川功，野中謙一郎：多関節脚を有する車両のリアルタイムトレッ
ド最適化を用いた障害物回避，高速信号処理応用技術学会誌，12-1，
25/30 (2009)5）T. Ohtsuka: A continuation/GMRES method for fast computation of nonlinear receding horizon control, Automatica,
40-4, 563/574 (2004)
6）村山明拡，山北昌毅：非線形モデル予測制御による可変バルブ機
構付きエンジンの制御，電学論 C，130-5，828/833 (2010)7）A. Trent, R. Venkataraman and D. Doman: Trajectory Generation Using A Modiﬁed Simple Shooting Method, Proceedingsof 2004 IEEE Aerospace Conference, 2723/2729(2004)8）I. Okawa and K. Nonaka: Optimal Online Generation of Obstacle Avoidance Trajectory running on a Low Speed Embedded CPU for Vehicle, Proceedings of 2010 IEEE MultiConference on Systems and Control, 1257/1262(2010)9）野中謙一郎，河村哲郎：3 軸スカラ型の脚を有する車両ロボットの最適脚配置と非線形軌道追従制御，日本機械学会 第 10 回「運動と振動の制御」シンポジウム講演論文集, 334/339(2007)10）野中謙一郎：3 軸スカラ脚を有する車両型ロボットの連続な操舵の実現，計測自動制御学会 第 8 回 制御部門大会資料 (2008)11）C. I. Connolly, J. B. Burns and R. Weiss: Path Planning Using
Laplace’s Equation, Proceedings of 1990 IEEE International
Conference on Robotics and Automation, 2102/2106(1990)
12）福島雅夫：数理計画入門，朝倉書店 (2000)

0.8

0

考

7

Fig. 10 Computational clock of the optimization［著 者 紹 介］

7. ま と め

大川

本論文では車両の安定性と機動性の両立を目的として，車輪位

功（正会員）
2011 年 3 月東京都市大学大学院工学研究科機械システム工学専攻修了．在学中は車両型ロボットを対象とした障害物回避，最適制御，組込みシ
ステムによる制御の研究に従事．現在は (株) デン
ソーに勤務し，車両制御システムの研究・開発を行う．

置が可変な車両型ロボットを用いた障害物回避経路の生成手法とモデル予測制御を用いた車輪位置の通路幅に対する最適化手法を提案した．また道程に対する車輪位置の変化を制御入力とすることで最適解を繰り返し計算なしで得られる問題への変換を行い，計算コストの低いシステムを実現した．そして提案手法を組込み
野

CPU のみを制御に使用する実験機へ実装し，障害物を回避しな

謙一郎（正会員）
1997 年 3 月東京工業大学情報理工学研究科博士課程修了．同年 4 月に武蔵工業大学（現，東京都市大学）工学部機械システム工学科助手，講師
を経て，2008 年 4 月から准教授．2000 年 4 月から 1 年間ボストン大学客員研究員．開ループ振動
安定化，スライディングモード制御，ビークル制
御に関する研究を行う．

がらモデル予測制御によって車輪位置を最適化する実験を行った．この結果，提案手法は制御性能・計算コストともに実用上十分なものであることが確認され，車両の安定性と機動性の両立を期待できる結果が得られた．
謝辞

中

本研究は平成 22 年度科研費補助金（基盤研究 C，21560475）により実施しました．また研究に際して多くの助力を頂いた東京都市大学の鈴木勝正教授，実験に協力して頂いた同大学の高橋直樹氏に深く感謝を申し上げます．

197