電気学会メカトロニクス制御技術委員会
合同委員会 2015/12/2@名古屋工業大学

ヒト並みの運動性能を有するパーソナルロボットの開発
2015年 12月 2日
株式会社 豊田中央研究所

廣瀬 徳晃、但馬 竜介、鋤柄 和俊

1

2

IEEJ MEC 2015/12/2@NIT

研究背景1
Arrival of the aging society in many countries
2013
2050
1970

over 65 years
men

Developmentof Personal Robot
JAPAN
women
to Support Active
Human Life
over 65 years
men

What is TRUE purpose of Personal Robot?
women

USAagehttp://www.ge.com/visualization/aging/

3

IEEJ MEC 2015/12/2@NIT

研究背景2
PR for elder people

Prevention from debility*PR: Personal Robot Service robot

 Walking ourselves

 Near-distance transporter

 Going outside“WALL・E” The Walt Disney Studios

http://www.shenandoahpodiatry.com/

健康寿命の長大化

4

IEEJ MEC 2015/12/2@NIT

パーソナルロボットの概観
Overview of Personal Robot

Driving wheel

Postureactuator

Driving wheel

Roll axis

・Posture Actuator
Bring baggage
Important Specifications⇒ Roll Angle&・ Small Footprint・Two Driving Wheels
Following High Center ofcoexistence ⇒in Pitch
humansociety
Angle
and Traveling Gravity
velocity・ High Traveling Performance
following person

5

IEEJ MEC 2015/12/2@NIT

姿勢アクチュエータの目的
Quick Turning

Vertical Posture on uneven floor:Reaction force ofown weight:Reaction force ofcentrifugal force: Zero Moment Point

N.Hirose et al ``Perosnal Robot Assisting Transportation to Support Active Human Life –Reference
Generation based on Model Predictive Control for Robust Quick Turning-’’ Proc. of ICRA 2014, pp.2223-2230

6IEEJ MEC 2015/12/2@NIT

試作パーソナルロボット

2011

2012

56 kg
ZIC*1

WIPC

2013

2014

23 kg

10.5 kg

LAC*2

MPC*3

ZIC*1 : Zeroing Input Control (Proc. of ICM 2013 @Vicenza, Italy)
WIPC : Wheeled Inverted Pendulum Control
LAC*2 : Lateral Acc. Control (Proc. of IROS 2013@Tokyo, Japan)
MPC*3 : Model Predictive Control (Proc. of ICRA 2014@Hong Kong, China)

7

IEEJ MEC 2015/12/2@NIT

制御システム構成
Feedback Control System for Personal Robot

倒立2輪制御旋回角速度制御②指令生成vdref
rhChf

 refcCwip
 refref

Rmpc

 vr

 ref

C yr
Cr

Lv
Q

Rd rh  rhr


③追従制御

① ロール角制御

その他の研究開発
ユーザの検出、追跡：

小山渚、但馬竜介、田中稔、廣瀬徳晃等：ヒトを活動的にする移動支援ロボット-IDを発信する赤外LEDタグと全方位カメラを用いた利用者の追跡- 計測自動制御学会論文集 Vol.51, No.1

自律移動（自己位置推定）：

伊藤誠悟、廣瀬徳晃等：ヒト並みの運動性能を有する小型移動体の実現 –準静止障害物を考慮した位置推定法- 日本ロボット学会学術講演会 2B2-07

8IEEJ MEC 2015/12/2@NIT

Subject of Posture Control
Desired roll angle

g ：gravity acc.

   tan

1agza ：centripetal acc.

yConventional method
FB control : Roll angle lateraldirection

Reference :  ref   tan

1aˆgâ cannot be estimated precisely

when slipping , idling and etc.

ZMP : Zero Moment PointFig. front view at left turning

Proposed method
Zeroing the acceleration
for the lateral direction

9IEEJ MEC 2015/12/2@NIT

Motion Model and Lateral Acc.
Motion model around roll axis

J    hMg sin   hMa cos J : inertia around roll axis
M : weighth : height of center of gravity

center of gravity

J, M : roll angle
a : centripetal acc.
g : gravity acc.

zayy

Lateral Acc.

hroll axis

aya y  g sin   a cos   hLateral Acceleration

angular acc.

Plant system in proposed method

hs 2  g
 2
 (s)Js  Mgha y (s)Unstable ZeroIEEJ MEC 2015/12/2@NIT

Compensation of Unstable Zeroaya y  g sin   a cos   hLateral Accelerationrefroll

FF1

 ff 



Unstable zero disa  roll  

PRay

Compensation of unstable zeroa ' y  a y  k

FF2PIDk ：freeˆparameter
disaref

a' y10xDisturbanceobserverks

Transfer function from  to a' yCompensation for unstable zerog system for Personal Robot
Fig. Block diagram2of control

a' y (s)s  (h  k ) 2 h  k (s)Js  Mghhk 0

Suppress the effect of unstable zero!!

11

IEEJ MEC 2015/12/2@NIT

実験結果：旋回走行・Quick Turning ( v = 1.67 m/s, ω=1.6 rad/s)

Centripetal Acc. = 0.3 G

Stable posture during steady turning

12IEEJ MEC 2015/12/2@NIT

実験結果：旋回走行
Angular velocity of wheels

Lateral Acceleration
Lateral acc. [m/s2]

angular velocityof wheels [rad/s]

25201510500leftright
5time [s]10150raw
filtered-1
05time [s]100Lateral Acceleration iszero during steady turning

-0.1
-0.2
015:steady turning

Roll angleroll angle [rad]15time [s]1015Stable PostureIEEJ MEC 2015/12/2@NIT

実験結果：片輪スロープ走行・Slope ( v = 1.11 m/s, Maximum height = 0.03 m)

Vertical posture even on slope!!

13

14

IEEJ MEC 2015/12/2@NIT

実験結果：片輪スロープ走行
Roll angle

Lateral Acceleration: robot on slope

0.05
0-0.05-0.1

0.075 rad0 0.5 1 1.5 2 2.5 3 3.5 4 4.5time [s]acc. on Y axis [m/s2 ]roll angle [rad]

0.1
10raw
filtered-1

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5time [s]Roll angle without feedback compensation  sin 10.03
 0.075 [ rad ]0 .4

Vertical posture even on Slope !!

15

IEEJ MEC 2015/12/2@NIT

課題
Angular velocity of wheels

Roll angle

angular velocityof wheels [rad/s]

25roll angle [rad]

20
15

-0.1

10
50left
right

約4[s]

-0.2

00

5

time [s]

10

15

0

5

time [s]1015定常状態での姿勢安定性はＯＫ

俊敏な旋回動作は未達成⇒ロール加速度大で転倒過渡状態での姿勢安定性を考慮した指令生成

16

IEEJ MEC 2015/12/2@NIT

制御システム構成
Feedback Control System for Personal Robot

旋回角速度制御②指令生成vdref
rhChf

 refcCwip
 refref

Rmpc

 vr

 refC yrCrLv
Q

Rr③追従制御① ロール角制御

d rh  rh

17

IEEJ MEC 2015/12/2@NIT

従来制御手法の課題1
従来手法
定常状態（定常旋回、直進走行）でZMPを接地中心に制御：ZMP

直進走行定常旋回

課題
過渡状態（直進走行⇔定常旋回）でのZMPは？？？
旋回応答性を十分確保できない⇒ヒト追従性劣化

18

IEEJ MEC 2015/12/2@NIT

従来手法の課題2
重心位置： p  [ p y pz ]・ZMP方程式(ｘ軸まわりのつり合い) ( p y  q y ) f z   p z  q z  f y  J
p y  h sin 

重心位置pz  h cos  r

YZMPの高さ=0qz  0fy
Jqy  (h cos  r )  h sin fzfz

f y  M v  h 2 sin   h cos

遠心力成分

J MZfz

hZMP

f z  M g  hsin 

回転運動成分

自重 回転運動成分

rfy
ZMP： q  [q y qz ]  0&

従来手法によるロール角指令

 ref  vref refh2g  hrefhrqy  0
ref  0
俊敏な旋回動作 ref  0

ZMPの変化増大 ⇒ 姿勢安定性の低下＝転倒

19

IEEJ MEC 2015/12/2@NIT

提案手法の概要
要求事項： 旋回応答性向上 ＆ ZMPへの上下限制約 の両立モデル予測制御（MPC）の適用
旋回角速度
並進速度指令目標値

ZMPv refv ref

 ref

旋回角速度指令
 refcMPC

 ref

倒立2輪制御旋回角速度制御
ロール角制御v

上限

下限

目的関数

V [k ]   refc   ref  Q1  refc   ref   Yzmp Q2Yzmp
T

旋回角速度の偏差最小化
不等式制約

Ymin  Yzmp  Ymax
ZMPへの上下限制約

TZMPの最小化

20

IEEJ MEC 2015/12/2@NIT

予測モデルの状態方程式
並進速度指令v refv ref

 ref

旋回角速度指令
 refc

MPC

 ref

v

倒立2輪制御旋回角速度制御
ロール角制御





予測モデル
(a) ロール角制御系の応答モデル

 ( s)
r 2r 2
 ref ( s) s  2r s  r 2 s  r

 ref [k ] X [k  1]  AX [k ]  B k[] ref X [k ]  [[k ] [k ]  [k ] [k ]]T(b) 旋回角速度制御系の応答モデル

 (s) e T sref ( s)sTs ：サンプリング周期

ロール角加速度
旋回角速度
ロール角度

21

IEEJ MEC 2015/12/2@NIT

ZMP方程式の線形化
ZMP方程式

fy
J
qy 
 (h cos  r )  h sin ffz
z
ZMP
非線形
f  M g  hsin  f y  M v  h 2 sin   h cosz

非線形モデル予測制御は計算負荷大線形化のための仮定  1・・・ ロール姿勢角は小さい （可動範囲±0.24 rad）

g  g  hsin  ・・・ Z軸方向の回転角加速度成分は重力加速度に比べて小さい・・・ 実速度は予測区間で指令と一致v  vref・・・ 旋回角速度2 は予測区間で旋回角速度指令2と一致 2  refc 2qy ZMP(h  r )vrefg

  h( h  r ) gJ2
()hrh
refc M    h g の1次関数で記述 ⇒ 線形モデル予測制御が可能

IEEJ MEC 2015/12/2@NIT

モデル予測制御による最適化計算

22

N p ステップの予測値を入力信号 U [k ] の1次関数で獲得
線形な予測モデル

 ref [k ]  q y [k  1] 
X [k 1]AX[k]B
 [k ] q [k  2] y   X[k ]ref  zmpU [k ]ZMPの予測値 ： Yzmp  zmpq [k ]  C X [k ]qyy [k  N p ]zmp 
[k[k]1]C X [k ]旋回角速度の予測値 ： [k  2]    X [k ]   U [k ] ref  [k  N p ]

将来の N u ステップ分の入力信号  ref ref
  ref [k ]   [k ] refU [k ]  ref [k  N u ] ref [k  N u ]

目的関数V [k ]   refc   ref  Q1  refc   ref   Yzmp Q2Yzmp

U [k ] の2次形式

Ymin  Yzmp  Ymax

U [k ] の1次不等式

T不等式制約T不等式制約付き２次計画問題 ⇒ 毎サンプリング周期に計算
実際には・・・ 状態不連続性抑制 ＋ ロール軸可動範囲、トルク上下限 を考慮

IEEJ MEC 2015/12/2@NIT： 0.3G旋回シミュレーション結果
v  1.667 m/s (= 時速 6km)

  01.766 rad/s : 0.3G旋回
ZMP [m]

旋回角速度 [rad/s]

0.1

1.5
1
conv. ref.
conv. res.
prop. ref.
prop. res.

0.5
0-0.5

050% 短縮

0.5

1time [s] 1.5

zero moment
point : q y [m]turning angularvelocity [rad/s]

2

conv. res.
prop. res.

0.05
0-0.05-0.1

0ロール角 [rad]roll angle [rad]0.4

conv. ref.
conv. res.
prop. ref.
prop. res.

0.2
0-0.2
00.5

1time [s] 1.5

actuator torque :  [Nm]

（指令値の50%に到達した時間）

-0.4

230.5
time [s]11.5

アクチュエータトルク [τ]100conv. res.
prop. res.

-10
00.5

1time [s] 1.5

IEEJ MEC 2015/12/2@NIT

実験動画 ： 0.3G旋回

24

25

IEEJ MEC 2015/12/2@NIT

実験結果 ： 0.3G旋回
  01.766 rad/s : 0.3G旋回v  1.667 m/s (= 時速 6km)

ZMP [m]

旋回角速度 [rad/s]
1.5

zero moment
point : q y [m]turning angularvelocity [rad/s]

21
ｼﾐｭﾚｰｼｮﾝと同等性能

0.5

res.
ref.

0-0.5

00.5

10.1
0.05
0-0.05-0.1
上下限制約満足00.5

1.5
time [s]

res.
ref.

0.2
0.1
0-0.1
-0.2

上下限制約満足actuator torque :  [Nm]roll angle [rad]ロール角 [rad]11.5
time [s]アクチュエータトルク [τ]100-1000.5

1time [s]1.5

上下限制約満足
00.5

11.5
time [s]IEEJ MEC 2015/12/2@NIT

実験動画：自由走行（最大0.4G）

26

27

IEEJ MEC 2015/12/2@NIT

実験動画 ： ヒト追従実験
従来手法
ヒト歩行速度
4.0 km/h

蛇行走行（ｵｰﾊﾞｰｼｭｰﾄ、ｱﾝﾀﾞｰｼｭｰﾄ）
提案手法

スムーズ
蛇行なし

28

IEEJ MEC 2015/12/2@NIT

制御システム構成
Feedback Control System for Personal Robot vr②指令生成v
dref
rh

Chf

 refref refc

Cwip

Rmpc

 ref

C yr
Cr

L

v
Q

Rr



③追従制御

① ロール角制御

d rh  rh

29

IEEJ MEC 2015/12/2@NIT

追従制御への要求事項
Target : Adjacent Following with guarantee of No collision and No rolloverRequirements
 Close relative distance No collision (d rh  d min )d min
Top View

 Upper and Lower boundaries for Pitch Angle( min    max)

Collision
Close relative distance

Sudden stop!!

Model Predictive Control

30

IEEJ MEC 2015/12/2@NIT

従来のモデル予測制御の課題
Subject : Human’s behavior in the future is unknown[a] Sudden stops
vr  0vh  0vr

current state

Model Predictive Control
with multiple future prediction

current step

futureCASE B

Max. vel.

Traveling Vel.

vh

current state

Proposed Approach ・・・

future[b] Acceleration to Maximum Vel.

vr

Traveling Vel.

vh

CASE Avh  vmaxvcurrentr  vmaxstep

futurefuture・・・

IEEJ MEC 2015/12/2@NIT

Multiple Future Prediction
Human model : xh [k  1]  xh [ k ]  Ts vh [k ]Robot model : xro [k  1]  Ar xro [k ]  Br v ref [k ]xr [k ]  Cr xro [k ]
[k ]  C xro [k ]31xh : human position
vh : human velocity
Ts : sampling period
xr : robot position : pitch angle

Case A : Sudden stops

X ha   h xh [k ]  h vh [k ]X ra   r xr [ k ]   rVaDrha  X ha  X ra a   xr [k ]  VaVa  va [k ] va [k  1]  va [k  N ]: reference of traveing velocity for case A a ： predicted pitch angle for case A

Case B : Acceleration to Maximum Vel.

X hb   h xh [k ]  h vh [k ]   h vmax
X rb   r xr [k ]   rVbDrhb  X hb  X rb
 b   xr [ k ]  Vbvmax : assumed maximum velocityVb  vb [k ] vb [k  1]  vb [k  N ]: reference of traveing velocity for case B

 b ： predicted pitch angle for case B

32

IEEJ MEC 2015/12/2@NIT

Optimization Problem
Cost function

minimizeJ  Drha
refarefbVVInequality constraintDmin  Drha min   a

Drhb  QDrha
TDrhb  b    max

Drhb  a ： predicted pitch angle for case A b ： predicted pitch angle for case B

Variable vectors Va Vb can be derived by solving QP problem

Va  va [k ] va [k  1]  va [k  N ]Vb  vb [k ] vb [ k  1]  vb [k  N ]Indipendent current control inputs
refAdditional equality constraint v
 va [k ]  vb [k ]v ref : actual control input

33

IEEJ MEC 2015/12/2@NIT

Simulation Results (6km/h)
Simulation condition : straight walking at 6km/h and sudden stop
Robot Traveling Velocity
traveling velocity [m/s]

traveling velocity [m/s]

HumanTraveling Velocity
Top View
210

Personal Robot01

User
23
time [s]

51

2.0m0

Inequality
constraint1

23time [s]4510126Collision

3time [s]456FALLING

Pitch Angle

conv. 1
conv. 2prop.

2conv. 1
conv. 2
prop.

0Not Close

306pitch angle [rad]relative distance [m]Relative Distance

42conv. 1
conv. 2
prop.

0.2
0-0.2
01

Inequality
constraint

2

3
time [s]456

34

IEEJ MEC 2015/12/2@NIT

Simulation Results (6km/h)
Simulation condition : straight walking at 6km/h and sudden stop
Robot Traveling Velocity
traveling velocity [m/s]

traveling velocity [m/s]Human Traveling Velocity
21000.511.5

2time [s]

2.5

3

3.50

pitch angle [rad]relative distance [m]Close Followingconv. 1
conv. 2
prop.

1

00.5

Inequality
constraint

1

1.5

00.5

1Pitch Angle

20conv. 1
conv. 2
prop.

1

4

Relative Distance32

2time [s]

2.5

3

3.5

NO CRASH

4

1.5

2
time [s]

2.5

3

3.5

4

Within Desired Range
conv. 1
conv. 2
prop.

0.2
0-0.2
00.5

Inequality
constraint

1

1.5

2
time [s]

2.5

3

3.5

4

35

IEEJ MEC 2015/12/2@NIT

Simulation with Multibody Dynamics ModelMultibody Dynamics Model by 3D drawing sheet
20 rigid bodies
9 revolution joints
3 input forces
2 contact difinitions
Green Cylinder : Human being model
Overall view

Enlarged view

36

IEEJ MEC 2015/12/2@NIT

付録1：ヒト検出、追跡 レーザ距離センサ
× ユーザの識別 × コスト アクティブマーカ+全方位カメラ
ロボット



全方位カメラ課題：
 カメラサンプリング周期大 カメラサンプリング周期のばらつき大マーカとカメラの同期困難

パーティクルフィルタを用いた識別手法開発

IEEJ MEC 2015/12/2@NIT

付録1: 人追従実験
 結果：ロボットによる個人識別を実現

37

38

IEEJ MEC 2015/12/2@NIT

付録2：自己位置推定
道案内機能の開発
自律移動制御（自己位置推定）

俊敏な姿勢変化によるセンサーデータのゆれ

豊富ではない計算資源

3次元地図の利用

輝度（レーザの反射強度）付き多次元正規分布を用いた3次元地図による位置推定（NDT-I MCL)
伊藤誠悟、廣瀬徳晃等：ヒト並みの運動性能を有する小型移動体の実現 –準静止障害物を考慮した位置推定法- 日本ロボット学会学術講演会 2B2-07

39

IEEJ MEC 2015/12/2@NIT

付録2: 特徴
NDT(Normal Distribution Transform) とは
NDT : 平均と分散


NDT*1では，ある一定範囲の距離計測結果に対し，位置の平均と分散を計算し，その平均分散で表される多次元正規分布を位置推定のための特徴とする．

レーザセンサによる距離計測結果

輝度情報によってパーティクルの重み計算

輝度
白線

高建築物
芝生
アスファルト低IEEJ MEC 2015/12/2@NIT

付録2: 実験動画

40

IEEJ MEC 2015/12/2@NIT

まとめ
 まとめ
 ヒトに追従するパーソナルロボットの提案
 横加速度フィードバック制御
 モデル予測制御による指令軌道生成
 モデル予測制御による追従制御法

 今後の課題 実証実験など

41

IEEJ MEC 2015/12/2@NIT

文献リスト(パーソナルロボットの制御)







廣瀬徳晃、津坂祐司、清水毅、山内智裕、白石孝、谷中壯弘、``リーン機構を有するパーソナルモビリティのロール運動モ
デルと姿勢安定化制御の提案'‘, 日本機械学会論文集, Vol.81, No.826, pp.1-12, 2015
廣瀬徳晃、但馬竜介、鋤柄和俊、小山渚、田中稔、伊藤誠悟、``モデル予測制御に基づくパーソナルロボットの姿勢安定化制御'' 電気学会論文誌Ｄ,Vol. 134, No.3, pp.172-181, 2015
小山渚、但馬竜介、田中稔、廣瀬徳晃、鋤柄和俊、藤井亮暢、``ヒトを活動的にする移動支援ロボット -IDを発信する赤外LEDタグと全方位カメラを用いた利用者の追跡- '' 計測自動制御学会論文集、Vol.51, No.1, pp.49-56, 2015
廣瀬徳晃、但馬竜介、鋤柄和俊、津坂祐司、``横加速度フィードバックによるパーソナルロボットの姿勢安定化制御'' 日本
ロボット学会誌、Vol.32, No.2, pp.163-171, 2014
廣瀬徳晃、但馬竜介、鋤柄和俊、津坂祐司、``不安定極を有するフィードバック補償を用いた小型移動体の姿勢安定化制御'‘, 電気学会論文誌Ｄ, Vol.133, No.3, pp.282-289, 2013
N. Hirose, R. Tajima, and K. Sukigara, ``Personal Robot Assisting Transportation to Support Active Human Life Following Control based on Model Predictive Control with Multiple Future Predictions-'' Proc. of IEEE International
Conference on Intelligent Robots and Systems(IROS), pp.5395-5402, 2015
N. Hirose, R. Tajima, and K. Sukigara, ``Personal Robot Assisting Transportation to Support Active Human Life Human-Following method based on Model Predictive Control for Adjacency without Collision-'' Proc. of IEEE
International Conference on Mechatronics(ICM), pp.76-81, 2015
N. Hirose, R. Tajima, and K. Sukigara, ``Personal Robot Assisting Transportation to Support Active Human Life Reference Generation based on Model Predictive Control for Robust Quick Turning -'' Proc. of IEEE International
Conference on Robotics and Automation(ICRA), pp.2223-2230, 2014
N. Hirose, R. Tajima, and K. Sukigara, ``Personal Robot Assisting Transportation to Support Active Human Life Posture Stabilization based on Feedback Compensation of Lateral Acceleration -'' Proc. of IEEE International
Conference on Intelligent Robots and Systems(IROS), pp.659-664, 2013
N. Hirose, R. Tajima, K. Sukigara, and Y. Tsusaka, ``Posture Stabilization for a Personal Mobility Robot using
Feedback Compensation with an Unstable Pole'' Proc. of IEEE International Conference on Mechatronics(ICM),
pp.804-809, 201342