こちらの続きです。

ETロボコンと言えば倒立振子だった

ETロボコン2009年大会から採用されたNXT走行体により、ETロボコンは倒立振子全盛期を迎えました。Mathworks社のMatlab/Simulinkにより作成された倒立振子プログラムをNXTで動くようにした先人の努力は素晴らしいものがあり、EV3への変化を経て2019年大会まで採用されていました。
2020年はコロナ禍により、シミュレータ大会が開催されることが決定し、真っ先にあきらめたのが倒立振子の採用でした。倒立振子を実現するためには5ms周期での制御が必須と言われていたのですが、大会で採用されたETロボコンシミュレータとAthrillの組み合わせでは10ms周期以上になることは見えていたため技術的に諦めざるを得ませんでした。

WEB版ETロボコンシミュレータのMiniScriptは5ms周期で動く。

Unityの物理エンジンの制御周期(FixedUpdate)を5msに設定していたこともあって、試しに下記スクリプトでMiniScriptのyieldの性能を図ってみた結果、4.998秒という結果が返ってきましたので、私のPCでは普通に5ms周期で回っていることがわかりました。この感覚はなんかArduinoっぽいです。

t=time
for i in range(1,1000)
yield // 次の制御周期まで待つ
end for 
print time-t

ということで、倒立振子をサポートしようという気持ちが大きくなりました。

EV3way倒立振子モデルの用意

軽量な3Dモデルを利用したかったので、塚本さんが以前作成したものを流用させていただきました。
このモデルに対して、清水さんのパラメータ設定を参考にして、タイヤと本体にRigidBodyとコライダーを設置しました。タイヤの重さ0.05kg、直径約4cm、本体の重さ0.8kg、サイズ幅0.18m×奥行0.08m×高さ0.18mとなっており、本体の下端がタイヤの中心とHingeJoint(モータ)で繋がっています。

倒立振子の原理とジャイロセンサについて

詳しい原理はNXTway-GS(2輪型倒立振子ロボット) C APIなどを参考にしてください。ざっくりというと、一輪車のバランスを取るのと同じようなもので、本体の角度がまっすぐ(0度)になるように、車輪の角度と角速度と、本体の角速度を使ってバランスを取る制御です。
車輪の角度はev3_motor_get_counts_leftとev3_motor_get_counts_rightで取得できます。角速度は後述の倒立振子ライブラリ内で微分するので自分で計算する必要はありません。
本体の角速度はジャイロセンサで計測できます。本シミュレータでは ev3_gyro_sensor_get_rate で取得できます。実機のジャイロセンサはノイズがあり、ドリフトと呼ばれる現象が発生するため、オフセットを適切に設定しないとうまく倒立できません。本シミュレータは今のところノイズもドリフトも入れてないので、簡単に制御できるはずですね。
Unityでジャイロセンサを模擬するために、body.transform.eulerAngles.x を用いて角度を取得しています。bodyは計測対象のGameObjectと考えてください。これを周期で割ることで角速度に変換しています。

倒立振子ライブラリのMiniScriptへの移植

倒立振子ライブラリbalancer.c を脳死モードでMiniScriptに置きかえて、バッテリ補正項を外して、あとはとにかく立つパラメータを探した結果がこちらのスクリプト。なぜこのパラメータで倒立するのか説明できないのですが、立たない理論よりは立つ現実が大事なのです。

balancer={}
// ローパスフィルタ係数(左右車輪の平均回転角度用) 
balancer.A_D = 0.8
// ローパスフィルタ係数(左右車輪の目標平均回転角度用) 
balancer.A_R = 0.996 

// 状態フィードバック係数
// K_F[0]: 車輪回転角度係数
// K_F[1]: 車体傾斜角度係数
// K_F[2]: 車輪回転角速度係数
// K_F[3]: 車体傾斜角速度係数
balancer.K_F = [ -0.870303, -31.9978, -1.1566, -2.78873 ]
// サーボ制御用積分フィードバック係数
balancer.K_I = -0.44721   

// 車体目標旋回角速度係数
balancer.K_PHIDOT = 25.0 * 2.5 
// モータ目標回転角速度係数
balancer.K_THETADOT = 7.5 

// 制御周期[s]
balancer.EXEC_PERIOD = 0.005

// コマンド最大値
balancer.CMD_MAX = 100.0

// 初期化
balancer.ud_err_theta = 0.0
balancer.ud_theta_ref = 0.0
balancer.ud_thetadot_cmd_lpf = 0.0
balancer.ud_psi = 0.0
balancer.ud_theta_lpf = 0.0

balancer.DEG2RAD=(pi * 2) / 360 

balancer.ret_pwm_r=0
balancer.ret_pwm_l=0

balancer.control=function(args_cmd_forward, args_cmd_turn, args_gyro, args_gyro_offset, args_theta_m_l, args_theta_m_r)
    tmp_thetadot_cmd_lpf = (((args_cmd_forward / balancer.CMD_MAX) * balancer.K_THETADOT) * (1.0 - balancer.A_R)) + (balancer.A_R * balancer.ud_thetadot_cmd_lpf)
    tmp_theta = (((balancer.DEG2RAD * args_theta_m_l) + balancer.ud_psi) + ((balancer.DEG2RAD * args_theta_m_r) + balancer.ud_psi)) * 0.5
    tmp_theta_lpf = ((1.0 - balancer.A_D) * tmp_theta) + (balancer.A_D * balancer.ud_theta_lpf)
    tmp_psidot = (args_gyro - args_gyro_offset) * balancer.DEG2RAD
    
    tmp = [ balancer.ud_theta_ref, 0.0, tmp_thetadot_cmd_lpf, 0.0 ]
    tmp_theta_0= [ tmp_theta, balancer.ud_psi, (tmp_theta_lpf - balancer.ud_theta_lpf) / balancer.EXEC_PERIOD, tmp_psidot ]
    tmp_pwm_r_limiter = 0.0
    for tmp_0 in range(0,3)
        tmp_pwm_r_limiter = tmp_pwm_r_limiter + (tmp[tmp_0] - tmp_theta_0[tmp_0]) * balancer.K_F[tmp_0]
    end for
    tmp_pwm_r_limiter =(tmp_pwm_r_limiter*20 + (balancer.K_I * balancer.ud_err_theta)*2)
    tmp_pwm_turn = (args_cmd_turn / balancer.CMD_MAX) * balancer.K_PHIDOT

    tmp_pwm_l_limiter = tmp_pwm_r_limiter + tmp_pwm_turn
    balancer.ret_pwm_l = tmp_pwm_l_limiter
    tmp_pwm_r_limiter = tmp_pwm_r_limiter-tmp_pwm_turn
    balancer.ret_pwm_r = tmp_pwm_r_limiter
    
    balancer.ud_err_theta = ((balancer.ud_theta_ref - tmp_theta) * balancer.EXEC_PERIOD) +balancer.ud_err_theta
    balancer.ud_theta_ref = (balancer.EXEC_PERIOD * tmp_thetadot_cmd_lpf) + balancer.ud_theta_ref
    balancer.ud_thetadot_cmd_lpf = tmp_thetadot_cmd_lpf
    balancer.ud_psi = (balancer.EXEC_PERIOD * tmp_psidot) + balancer.ud_psi
    balancer.ud_theta_lpf = tmp_theta_lpf
end function

speed=40 //前進速度
str=0 //旋回速度
for i in range(0,10000)
    // 倒立振子制御
    balancer.control(speed,str, ev3_gyro_sensor_get_rate,0,ev3_motor_get_counts_left,ev3_motor_get_counts_right)
    ev3_motor_set_power_left  balancer.ret_pwm_l
    ev3_motor_set_power_right balancer.ret_pwm_r
    yield
end for

HackEVをEV3Wayに変更して Run Scriptを押すと倒立振子で前進します。

MiniScriptの倒立振子ライブラリ化

自分で最適なパラメータを見つけたい人は前述の方法で頑張ってもらうとして、エンジョイ勢はもっと簡単に倒立振子を体験したいと思うことでしょう。ということで、MiniScriptからさらにC#に移植して、MiniScriptから呼び出せる関数として、balance_init、balance_control、balance_pwm_l、balance_pwm_rを用意しました。使い方は下記の通り。これで誰でも簡単に倒立振子を楽しめますね。

speed=40 //前進速度
str=0 //旋回速度
balance_init // 倒立振子初期化
for i in range(0,3000)
    // 倒立振子制御
    balance_control speed,str, ev3_gyro_sensor_get_rate,0,ev3_motor_get_counts_left,ev3_motor_get_counts_right
    ev3_motor_set_power_left  balance_pwm_l
    ev3_motor_set_power_right balance_pwm_r
    yield
end for

最後に

本稿では二輪ロボットEV3wayをシミュレータ上で倒立振子させる方法について説明しました。さらに、ライントレースと組みあわせることで古き良きETロボコン気分を体験することができます。ぜひお試しください。