matlab expo pub - MathWorks · MATLAB EXPO 2016 8 Joint Range of Motion [deg] Max. Torque [Nm] Hip...

ヒューマノイドロボットHYDRAの開発におけるMATLABの活用

東京大学 神永 拓

開発の動機

フィールドで実用的に使用できるロボットをどうやったら作れるのか？

パワー

耐久性

操作性制御性

22016/10/19 MATLAB EXPO 2016

なにをしなければならないか

(直感的)操作性・制御性

頑健性 自律性

駆動系

制御システム動作計画

センサ

人型構造 アクチュエータ

柔軟性の実現 機構の改善

多数のセンサ

高速制御

32016/10/19 MATLAB EXPO 2016

力制御系に影響を与える要因

2016/10/19 MATLAB EXPO 2016 4

PlantController

SensorObserver

Reference OutputPlant

Sensor

Backdrivability

Force Measurement

Controller

Observer

バルブを用いない油圧駆動系

MATLAB EXPO 2016 5

Humanoid robots withvalve controlled hydraulics[Cheng2006, Nelson2012, Hyon2015]

Humanoid robot withEHA [Alfayad2011]

2016/10/19

Torque Sensing

Strength

バルブを用いない油圧駆動系

MATLAB EXPO 2016 6

Humanoid robots withvalve controlled hydraulics[Cheng2006, Nelson2012, Hyon2015]

Humanoid robot withEHA [Alfayad2011]

2016/10/19

Torque Sensing

Strength

Torque Sensing

Modularity

電気静油圧アクチュエータ (EHA)

MATLAB EXPO 2016 7

Torque Sensing

Strength

Backdrivability

Built‐In Torque Limiter

Modularity

2016/10/19

Conceptual Structure of the Robot

MATLAB EXPO 2016 8

Joint Range of Motion [deg] Max. Torque [Nm]

Hip Yaw ‐60 ~ 60 60

Hip Roll ‐22.5 ~ 22.5  144

Hip Pitch ‐105 ~ 13  85.5

Knee ‐10 ~ 120  120

Ankle Roll ‐24.2 ~ 24.2 117

Ankle Pitch ‐44 ~ 34 117

Waist Roll ‐22.5 ~ 22.5 147

Waist Pitch 0 ~ 50 94

Clavicle ‐30 ~ 90 60

Shoulder Roll ‐17.5 ~ 117.5 80

Shoulder Pitch ‐27.3 ~ 27.3 105

Shoulder Yaw ‐30 ~ 90 40

Elbow ‐117 ~ 9 82.5

Wrist Yaw ‐10 ~ 110 40

Wrist Roll ‐29.1 ~ 29.1 142.5

Wrist Pitch ‐66 ~ 85 77.2

2016/10/19

EHAのモジュール化

MATLAB EXPO 2016 9

Max. Pressure~7MPa

2016/10/19

閉リンクを用いた関節駆動

1自由度 ２自由度

102016/10/19 MATLAB EXPO 2016

ロボットの構造設計

MATLAB EXPO 2016 11

Modular Design

Parallel‐Serial Hybrid Kinematics

2016/10/19

分散制御系の開発

Friction Compensation, DOB

Status Management

Protocol Stack Handling

Current Control

Serial Comm. Processing

RealtimeRequirement

Amount of Computation

Operational Space Control

Whole‐BodyCoordination

FPGA

MCU (CPU, no OS)

PC (CPU, RTOS)

122016/10/19 MATLAB EXPO 2016

体内LANの種類

Speed ProtocolStack

Realtime

I2C 5Mbps N Y

CAN 1Mbps Y N(Y for siglemaster case)

FlexRay 10Mbps Y Y

CU‐Net 12Mbps Y Y

RS‐485 20Mbps N Y

SPI ~20Mbps N Y

SERCOS‐II 10Mbps Y Y

SERCOS‐III 100Mbps Y Y

Ethernet 1000Mbps Y N

EtherCAT 100Mbps Y Y

清水ら2001

Kaneko et al. 2011

Sugihara et al. 2005

Lohmeier et al. 2009

Englsberger et al. 2014

2016/10/19 MATLAB EXPO 2016 13

制御ハードウェアのブロック図

2016/10/19 MATLAB EXPO 2016 14

MCU Board Driver Board Joint Actuation

Sensor Interface Board

F/T Sensor

Accelerometer

FOG

Sensor Interface Board

駆動系制御ハードウェアMCU + FPGA

FPGA+SiC

LVDS

15

MD

MD

Master(RT PC)

EtherCAT

1 1..5

2016/10/19 MATLAB EXPO 2016

計算の階層構造

FOC Current Control

FOC Current Control

FOC Current Control

EHA Position / Pressure Control

FOC Current Control

EHA Position / Pressure Control

Parallel – Serial Linkage Conversion

Whole Body Motion Generation Robot Status Management

Congnitive Systems

FPGA

MCU

RT‐PC

PC

High

Low

Real Tim

e Re

quire

men

t

16

ROS

EtherCAT

LVDS

1 kHz

5kHz

1 kHz

20kHz

shm + sem

2016/10/19 MATLAB EXPO 2016

パラレルリンクの等価開リンク系への変換

MATLAB EXPO 2016 172016/10/19

閉リンクのシンボリックな計算

2016/10/19 MATLAB EXPO 2016 18

Rp = [cos(q_1),0,sin(q_1);0,1,0;‐sin(q_1),0,cos(q_1)];Rr = [1,0,0;0,cos(q_2),‐sin(q_2);0,sin(q_2),cos(q_2)];pend_1 = [xend;yend;zend];pend_2 = [‐xend;yend;zend];pcyl_01 = [xcyl;ycyl;z_1+zcyl];pcyl_02 = [‐xcyl;ycyl;z_2+zcyl];pend_01 = Rp*Rr*pend_1;pend_02 = Rp*Rr*pend_2;d1 = pend_01‐pcyl_01;d2 = pend_02‐pcyl_02;l1sq = norm(d1)^2;l2sq = norm(d2)^2;z_1_sol = solve(l1sq==lc^2,z_1);z_2_sol = solve(l2sq==lc^2,z_2);

z_1_sol =(lc^2 ‐ abs(ycyl ‐ yend*cos(q_2) + zend*sin(q_2))^2 ‐abs(xend*cos(q_1) ‐ xcyl + zend*cos(q_2)*sin(q_1) + yend*sin(q_1)*sin(q_2))^2)^(1/2) ‐ zcyl ‐ xend*sin(q_1) + zend*cos(q_1)*cos(q_2) + yend*cos(q_1)*sin(q_2)zend*cos(q_1)*cos(q_2) ‐ (lc^2 ‐abs(ycyl ‐ yend*cos(q_2) + zend*sin(q_2))^2 ‐abs(xend*cos(q_1) ‐ xcyl + zend*cos(q_2)*sin(q_1) + yend*sin(q_1)*sin(q_2))^2)^(1/2) ‐ xend*sin(q_1) ‐ zcyl + yend*cos(q_1)*sin(q_2)

Symbolic MathToolboxによる求解

さらにシンボリックな微分によりヤコビ行列を計算

FPGA(Verilog HDL)

複雑なマルチプロセッサシステムの組み込み開発

2016/10/19 MATLAB EXPO 2016 19

Control Objectives

MCU(C/C++)

Working with multiple language Difficulty of simulation

Simulinkによる電流ベクトル制御系の統合シミュレーションと自動コード生成

2016/10/19 MATLAB EXPO 2016 20

FPGA2MCU FPGA1

電流ループシミュレーション結果

2016/10/19 MATLAB EXPO 2016 21

Simulinkによるモジュール化設計

2016/10/19 MATLAB EXPO 2016 22

Fixed Logic

Logic under work

PWM波形生成

2016/10/19 MATLAB EXPO 2016 23

ton

tofftd

有効なhigh時間

有効なlow時間

短絡防止時間

上アーム

下アーム

キャリア波形

State Flowによる実装

2016/10/19 MATLAB EXPO 2016 24

MATLAB/Simulinkを使うメリット

• 複雑なロジックを検証しつつ実装できる

• 複数のプラットフォームに渡る開発を統合的に行うことができる

• 可視化が容易

2016/10/19 MATLAB EXPO 2016 25

開発の高速化

バグの最小化

MATLAB/Simulinkを組み込みに使う上での難しさ

• 極めてリソースが限られている場合の難しさ– リソースの再利用

– データ幅の自由度

• FPGAならではの難しさ– 純粋なクロックのみを使ったマルチレート制御

– 各社提供IPの利用の難しさ

• マイコンならではの難しさ– 自分で書いたCの関数を使いにくい

• あまりスケールが大きくなるとシミュレーションが遅くなる

2016/10/19 MATLAB EXPO 2016 26

reg [3:0] clk2_cnt;reg clk2;

always @(posedge clk) beginclk2_cnt <= clk2_cnt + 4b’1;if(clk2_cnt==4’b0) beginclk2 <= 1’b1;

end else beginclk2 <= 1’b0;

endend

always @(posedge clk) beginif(clk2 ==1’b1) begin

/* slow process */end

end

生成しにくいロジックの例

2016/10/19 MATLAB EXPO 2016 27

reg [3:0] clk2_cnt;reg clk2;

always @(posedge clk) beginclk2_cnt <= clk2_cnt + 4b’1;if(clk2_cnt==4’b0) beginclk2 <= 1’b1;

end else beginclk2 <= 1’b0;

endend

always @(posedge clk2) begin/* slow process */

end

内部でのイベント発生

外部クロック同期でのイベント実行

これはやりたくない

MATLAB/Simulinkを組み込みに使う上での難しさ

• 極めてリソースが限られている場合の難しさ– リソースの再利用

– データ幅の自由度

• FPGAならではの難しさ– 純粋なクロックのみを使ったマルチレート制御

– 各社提供IPの利用の難しさ

• マイコンならではの難しさ– 自分で書いたCの関数を使いにくい

• あまりスケールが大きくなるとシミュレーションが遅くなる

2016/10/19 MATLAB EXPO 2016 28

最適化を用いた可動域の計算

2016/10/19 MATLAB EXPO 2016 29

CADとのインテグレーション

SolidWorks

MATLAB

OptimizationToolbox

MacroInterface

2016/10/19 MATLAB EXPO 2016 30

開発した制限装置

2016/10/19 MATLAB EXPO 2016 31

ヒューマノイドロボットHYDRA

MATLAB EXPO 2016 32

Height 1800mmWeight 130kg

2016/10/19

まとめ

FOC Current Control

FOC Current Control

FOC Current Control

EHA Position / Pressure Control

FOC Current Control

EHA Position / Pressure Control

Parallel – Serial Linkage Conversion

Whole Body Motion Control Robot Status Management

Congnitive Systems

FPGA

MCU

RT‐PC

PC

332016/10/19 MATLAB EXPO 2016

HDL Coder

E Coder

Sym Math

SimulinkReal‐Time

E Coder

Rob.System

結論

1. ヒューマノイドロボットHydraの設計を紹介した

2. Hydraのメカトロニクスを例にとり，MATLAB/ Simulinkがどのように開発に用いられたのか紹介した– C/HDLの統合的な開発

– シミュレーションのメリット

– 独自コードを組み込んだ開発の難しさ

3. SolidWorksとの統合によるOptimization Toolboxを用いた最適化設計を紹介した

4. 開発したロボットの動作を紹介した

2016/10/19 MATLAB EXPO 2016 34