ヒューマノイドロボット HYDRA の 開発における MATLAB の活用 東京大学神永拓
開発の動機 フィールドで実用的に使用できるロボットをどうやったら作れるのか? パワー 耐久性 制御性 操作性 2016/10/19 MATLAB EXPO 2016 2
なにをしなければならないか ( 直感的 ) 操作性 制御性 人型構造 柔軟性の実現 頑健性 アクチュエータ 機構の改善 自律性 多数のセンサ 高速制御 制御システム 駆動系 動作計画 センサ 2016/10/19 MATLAB EXPO 2016 3
力制御系に影響を与える要因 Backdrivability Reference Controller Plant Output Observer Sensor Force Measurement 2016/10/19 MATLAB EXPO 2016 4
バルブを用いない油圧駆動系 Strength Torque Sensing Humanoid robots with valve controlled hydraulics [Cheng2006, Nelson2012, Hyon2015] 2016/10/19 Humanoid robot with EHA [Alfayad2011] MATLAB EXPO 2016 5
バルブを用いない油圧駆動系 Strength Torque Sensing Torque Sensing Modularity Humanoid robots with valve controlled hydraulics [Cheng2006, Nelson2012, Hyon2015] 2016/10/19 Humanoid robot with EHA [Alfayad2011] MATLAB EXPO 2016 6
電気静油圧アクチュエータ (EHA) Strength Backdrivability Torque Sensing Built In Torque Limiter Modularity 2016/10/19 MATLAB EXPO 2016 7
Conceptual Structure of the Robot 2016/10/19 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 MATLAB EXPO 2016 8
EHA のモジュール化 Max. Pressure ~7MPa 2016/10/19 MATLAB EXPO 2016 9
閉リンクを用いた関節駆動 1 自由度 2 自由度 2016/10/19 MATLAB EXPO 2016 10
ロボットの構造設計 Modular Design Parallel Serial Hybrid Kinematics 2016/10/19 MATLAB EXPO 2016 11
分散制御系の開発 Realtime Requirement Serial Comm. Processing Current Control FPGA Protocol Stack Handling MCU (CPU, no OS) Status Management Friction Compensation, DOB PC (CPU, RTOS) Whole Body Coordination Operational Space Control 2016/10/19 MATLAB EXPO 2016 Amount of Computation 12
体内 LAN の種類 Speed Protocol Realtime Stack I2C 5Mbps N Y CAN 1Mbps Y N(Y for sigle master 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 Englsberger et al. 2014 Lohmeier et al. 2009 2016/10/19 MATLAB EXPO 2016 13
制御ハードウェアのブロック図 MCU Board Driver Board Joint Actuation FOG F/T Sensor Accelerometer 2016/10/19 Sensor Interface MATLAB BoardEXPO 2016 Sensor Interface Board 14
MCU + FPGA 駆動系制御ハードウェア EtherCAT Master (RT PC) LVDS 1 1..5 MD MD FPGA+SiC 2016/10/19 MATLAB EXPO 2016 15
計算の階層構造 PC Congnitive Systems Low RT PC MCU FPGA LVDS ROS shm + sem EtherCAT EHA Position / Pressure Control FOC Current Control Whole Body Motion Generation Parallel Serial Linkage Conversion FOC Current Control FOC Current Control Robot Status Management EHA Position / Pressure Control 1 khz 1 khz 5kHz FOC Current Control 20kHz Real Time Requirement High 2016/10/19 MATLAB EXPO 2016 16
パラレルリンクの等価開リンク系への変換 2016/10/19 MATLAB EXPO 2016 17
閉リンクのシンボリックな計算 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); Symbolic Math Toolbox による求解 さらにシンボリックな微分によりヤコビ行列を計算 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) 2016/10/19 MATLAB EXPO 2016 18
複雑なマルチプロセッサシステムの組み込み開発 Control Objectives MCU (C/C++) FPGA (Verilog HDL) Working with multiple language Difficulty of simulation 2016/10/19 MATLAB EXPO 2016 19
Simulink による電流ベクトル制御系の統 合シミュレーションと自動コード生成 MCU FPGA1 FPGA2 2016/10/19 MATLAB EXPO 2016 20
電流ループシミュレーション結果 2016/10/19 MATLAB EXPO 2016 21
Simulink によるモジュール化設計 Fixed Logic Logic under work 2016/10/19 MATLAB EXPO 2016 22
PWM 波形生成 t on 上アーム 有効な high 時間 下アーム t d 短絡防止時間 t off 有効な low 時間 キャリア波形 2016/10/19 MATLAB EXPO 2016 23
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) begin clk2_cnt <= clk2_cnt + 4b 1; if(clk2_cnt==4 b0) begin clk2 <= 1 b1; end else begin clk2 <= 1 b0; 内部でのイベント発生 end end always @(posedge clk2) begin /* slow process */ end これはやりたくない reg [3:0] clk2_cnt; reg clk2; always @(posedge clk) begin clk2_cnt <= clk2_cnt + 4b 1; if(clk2_cnt==4 b0) begin clk2 <= 1 b1; end else begin clk2 <= 1 b0; end end always @(posedge clk) begin if(clk2 ==1 b1) begin /* slow process */ end end 外部クロック同期でのイベント実行 2016/10/19 MATLAB EXPO 2016 27
MATLAB/Simulink を組み込みに 使う上での難しさ 極めてリソースが限られている場合の難しさ リソースの再利用 データ幅の自由度 FPGA ならではの難しさ 純粋なクロックのみを使ったマルチレート制御 各社提供 IP の利用の難しさ マイコンならではの難しさ 自分で書いた C の関数を使いにくい あまりスケールが大きくなるとシミュレーションが遅くなる 2016/10/19 MATLAB EXPO 2016 28
最適化を用いた可動域の計算 2016/10/19 MATLAB EXPO 2016 29
CAD とのインテグレーション SolidWorks MATLAB Optimization Toolbox Macro Interface 2016/10/19 MATLAB EXPO 2016 30
開発した制限装置 2016/10/19 MATLAB EXPO 2016 31
ヒューマノイドロボット HYDRA 2016/10/19 Height 1800mm Weight 130kg MATLAB EXPO 2016 32
まとめ PC Rob. System Congnitive Systems RT PC E Coder Whole Body Motion Control Parallel Serial Linkage Conversion Robot Status Management Simulink Real Time Sym Math MCU EHA Position / Pressure Control EHA Position / Pressure Control E Coder FPGA FOC Current Control FOC Current Control FOC Current Control FOC Current Control HDL Coder 2016/10/19 MATLAB EXPO 2016 33
結論 1. ヒューマノイドロボット Hydra の設計を紹介した 2. Hydra のメカトロニクスを例にとり,MATLAB/ Simulink がどのように開発に用いられたのか紹介した C/HDL の統合的な開発 シミュレーションのメリット 独自コードを組み込んだ開発の難しさ 3. SolidWorks との統合による Optimization Toolbox を用いた最適化設計を紹介した 4. 開発したロボットの動作を紹介した 2016/10/19 MATLAB EXPO 2016 34