倒立振子型のライントレーサーの紹介

はじめに

こんにちは。社会人1年目もそろそろ終わろうとしている筆者です。 今年度は社会人1年目ということもあり、金銭的にも時間的にも設備的にも学生の頃と比べてリソースが少ないなかでのマイクロマウス大会への参加となりました。 そんな状況の中、速いロボットを作るのは一旦おいておいて、今年度はおもしろ枠を狙って新作を作りました。

おもしろ枠といってもせっかく大会に出るのだから何かしらの目的をもって参加しようと考えました。 また、自分の技術力の向上にもメリットのあるものを作りたいとも思っていました。 そこで、映えと技術的面白さで何かしらの賞の獲得と、自分の技術力向上をねらって、状態フィードバックを用いた倒立振子型のロボトレーサを作ることにしました。

映えと技術的面白さに関しては、倒立振子が倒立したままいい感じにライントレースしたら評判いいだろという安易な考えです。 また、状態フィードバックに関しては、PIDを用いた倒立振子はあまり好きじゃないのと、状態フィードバックを実機で実装したいという思いから状態フィードバックを使うことにしました。

ロボットの紹介

　

このロボットの紹介は、大会での成績、ハードウェア、ソフトウェアの順で解説していきます。 その前に、まずはできあがったものを見せましょう。

できあがって走っている動画がこちら↓。なかなかいい感じに倒立しながらライントレースできていることがわかると思います。 ちゃんと状態フィードバックで倒立してますよ。 今回はあまり攻めたロボットじゃないので、大会前に時間的余裕がありました。なのでステッカーを貼ったり自分のYouTubeの宣伝用のQRコードを貼ったりして遊ぶことができました。

大会での成績

　

全国大会でニューテクノロジー賞をいただきました。 今まで倒立振子のロボトレーサはいくつかあったようですが、僕が作った倒立振子が最も速く走っていたということで頂けました。 うれしいですね。（しめしめ狙い通り...って感じ）

ハードウェア

　

ハードウェアに関してはThe スタンダードな倒立振子という感じにしました。 特徴的なのは、振り子の上部に重り（乾電池）を搭載できるパーツをつけたくらいですかね。 モータはMaxon DCX 10L 4.5Vです。バッテリーはLoPoの2セルです。 詳細は下記のテクニカルデータをご覧ください。

CADデータ

　

CADはFusion360をつかいました。色々バラけてますが気にしないことにします。 Fusion360の無料版を使っており、このライセンスでは第三者が自由にダウンロードできる形で共有はできないようです。 もしCADデータがほしいという方がいらっしゃいましたら連絡ください。

プレビュー機能もあるようなのでつけてみました。好きにグリグリしてください。

ソフトウェア

　

ソフトウェアの根本は昨年度と同じ回路を使用していることもあり、今までのものと同じです。 そこに倒立制御関係を詰め込んだものが以下のリポジトリにあります。軽く整理しましたが雑多な状態です。勘弁してください。

reRo_robotrace_board_codeに実際のコードが、 2024_robotracer_control_designに制御設計などコードがあります。 制御関係に関しては次で詳しく解説します。

制御

　

本ブログのメインとなる制御関係の解説をしていきます。 昔を思い出しながら書いているので間違ってるところがあるかもしれません。間違いに気づいたら教えてください。

さて、状態フィードバックで制御するので、まずは状態方程式を立てます。 状態方程式とは $$\begin{align} &\dot{x}=A\,x + B\,u\\ &y=C\,x \\ \end{align}$$ っていうやつですね。現代制御に触れたことがある方ならおなじみのアイツです。

状態方程式を導くには状態変数$x$を決めて運動方程式を立ててやる必要があります。 細かい立式はトランジスタ技術 2019 7月号を参考にしました。詳しくは本を買って読んでみてください。 ネットで調べても式はでてくるかもしれません。

振り子の角度$\theta_p$、振り子の角速度$\dot \theta_p$、タイヤの角度$\theta_w$、タイヤの角速度$\dot \theta_w$として、状態変数を $$x = \begin{bmatrix} \theta_p \\ \dot \theta_p \\ \theta_w \\ \dot \theta_w \end{bmatrix}$$ として、入力$u$をモータへの印加電圧としたとき、$A$、$B$、$C$は次のようになります。 $$\begin{align} &A = \left(\begin{array}{cccc} 0 & 1 & 0 & 0\\ \frac{a_{22}\,g\,m_{p}\,r_{p}}{\Delta } & 0 & 0 & \frac{a_{21}\,\mathrm{ke}\,\mathrm{kt}\,n^2}{R\,\Delta }\\ 0 & 0 & 0 & 1\\ -\frac{a_{21}\,g\,m_{p}\,r_{p}}{\Delta } & 0 & 0 & -\frac{a_{11}\,\mathrm{ke}\,\mathrm{kt}\,n^2}{R\,\Delta } \end{array}\right) \\ &B = \left(\begin{array}{c} 0\\ -\frac{a_{12}\,\mathrm{kt}\,n}{R\,\Delta }\\ 0\\ \frac{a_{11}\,\mathrm{kt}\,n}{R\,\Delta } \end{array}\right) \\ &C = \left(\begin{array}{cccc} 1 & 1 & 1 & 1 \end{array}\right) \\ \end{align}$$

なお、各パラメータは次のとおりです。 $$\begin{align} &a_{11}=m_{p}\,{r_{p}}^2+2\,m_{p}\,r_{p}\,r_{w}+\left(m_{p}+m_{w}\right)\,{r_{w}}^2+J_{p}+J_{w} \\ &a_{12}=\left(m_{p}+m_{w}\right)\,{r_{w}}^2+m_{p}\,r_{p}\,r_{w}+J_{w} \\ &a_{21}=\left(m_{p}+m_{w}\right)\,{r_{w}}^2+m_{p}\,r_{p}\,r_{w}+J_{w} \\ &a_{22}=J_{m}\,n^2+\left(m_{p}+m_{w}\right)\,{r_{w}}^2+J_{w} \\ &\Delta =a_{11}\,a_{22}-a_{12}\,a_{21} \\ \end{align}$$

記号	単位	説明
$\mathrm{m_w}$	[$\mathrm{kg}]$	タイヤの重さ
$\mathrm{m_p}$	[$\mathrm{kg}]$	振子の重さ
$\mathrm{r_w}$	[$\mathrm{m}]$	タイヤの半径
$\mathrm{r_p}$	[$\mathrm{m}]$	振子の重心までの距離
$\mathrm{J_w}$	[$\mathrm{kg \cdot m^2}]$	タイヤのイナーシャ
$\mathrm{J_p}$	[$\mathrm{kg \cdot m^2}]$	振子のイナーシャ
$\mathrm{g}$	[$\mathrm{m/s^2}]$	重力加速度
$\mathrm{n}$	[$\mathrm{-}]$	減速比
$\mathrm{k_t}$	[$\mathrm{Nm/A}]$	トルク定数
$\mathrm{k_e}$	[$\mathrm{V/(rad/s)}]$	起電力定数
$\mathrm{R}$	[$\mathrm{\Omega}]$	内部抵抗

$A$と$B$と$C$が求まったので、可制御性とか可観測性とかを調べます。 可観測性は$C$が全部1で全部観測できるので良しとします。 可制御性も当然満たしているはずですが、一応↓の式で確認しておきます。 $$\begin{align} \begin{vmatrix} B & AB & A^2 B & A^3B \end{vmatrix} \ne 0 \end{align}$$

ここまで来たらゲインが決められます。 今回は最適レギュレータで決めます。 $Q$と$R$を試行錯誤的に決めて、MATLABのlqr関数で以下のように求められます。

f = lqr(A, B, Q, R);
					

モデル化の誤差などが当然あるので、一発ではうまく倒立しませんでした。$Q$と$R$の値を変えて頑張って調整しました。 なお、$Q$は4×4の対角行列、$R$はスカラーです。 $Q$は収束の速さを、$R$は制御量の大きさに関係します。 具体的には、$Q$を大きくすると収束が早くなり、$R$を大きくすると制御量が小さくなります。 $Q$が大きいと機敏に動いて、$R$が大きいと省エネで動くってイメージですね。

あとは求まったゲインをロボットの中で使ってやればいいだけです。例えばC++で以下のような感じです。 第1引数は状態変数で、エンコーダやIMUから計測したロボットの角度や角速度などです。第2引数はフィードバックゲインです。 この関数の戻り値をモータの入力に与えればOKです。 ただ、この入力はモータへの電圧なので、実際はDuty比などへ変換する必要があります。

double stateFeedback(const double x[4], const double f[4])
{
	return ((-f[0] * x[0] + -f[1] * x[1]) + -f[2] * x[2]) + -f[3] * x[3];
}
					

以上で無事に倒立はしますがロボトレーサは前に進まなければなりません。上記の制御だと、すべての状態変数を0に近づけるように制御するので、その場にとどまろうとします。 これを解決するためにサーボ系を組んでみたり、状態変数をいじってみたりしましたがうまくいきませんでした。 最終的には苦肉の策として、タイヤの角度の状態$\theta_w$にかかるフィードバックゲインを0にして倒立したまま自由に動けるようにしました。 この状態で重心を進行方向の方に若干ずらし、無理やり前に進むようにしました。 もっと賢いやり方をご存知の方は教えてください...

これで倒立しながら前に進むことができたので、あとはライン追従です。 これは何も難しいことはしていなくて、対向2輪型のロボットは並進方向（倒立の制御）と回転方向（ライン追従の制御）は独立して制御できます。 なので以下のプログラムのようなイメージで動いちゃいます。 inverted_dutyが倒立するためにモータに与えるDutyで、rotaion_ratio_がライン追従するためにモータに与えるDutyです。 これで晴れて倒立しながらライントレースができました。

double left_dytu, right_duty;
left_duty = inverted_duty + rotation_ratio_;
right_duty = inverted_duty - rotation_ratio_;

drive_motor_->setDuty(left_duty, right_duty);
					

まとめ

　

毎度おなじみ、大体の情報はすべて公開したつもりです。 チャレンジングなロボットでしたがちゃんと動いて完走したし、大会で賞も頂けたしで大成功と言えるのではないでしょうか。 来年度は倒立振子をベースにひと味違うやつも作れたらいいなぁと思ってます。もしかしたら作りたいものが完成するのは再来年度くらいになるかも？くらいのスケジュール感です。 みんなも興味が湧いたら倒立振子を作ってみましょう！