マイクロマウス研修(kora編)[33] 目標軌道への追従方法の検討


こんにちは。koraです。

前々回までにマウスの自己位置を推定できるようになったので、今度はマウスに辿らせたい目標軌道を生成していきたいと思います。

目標軌道の生成方法

マウスは目標点に追従するものとして、その目標点を移動させることで軌道とします。
速く走るためには斜め走行や180度ターンなど多様な軌道を考えるべきですが、今回は確実に動かすことを目標として、直進と90度のスラロームの2つだけ考えます。

目標軌道の追従方法

目標軌道から、目標点の速度(Vr)と角速度(ωr)が分かりますので、これをマウスで再現させます。ただしこの際、目標点とマウスのズレをフィードバックさせることで、より目標軌道に近い経路を走行できるようにします。

考え方はシンプルです。マウスから見て目標点の前後のズレをXe、左右のずれをYe、角度のズレをθeとしたとき、ズレを反映した目標速度(V)と目標角速度(ω)を以下の式で求めます。

V = Vr * cos(θe) + Kx * Xe
ω = ωr + Vr * (Ky * Ye + Kθ * sin(θe))

ただし、Kx、Ky、Kθはそれぞれ前後、左右、角度のズレに掛けるゲインです。
この追従方法はKanayama Control Methodというそうです。こちらを参考にさせていただきました。

ここで求めた新しい目標速度と目標角速度に対し、マウスの速度と角速度を一致させるようにモーターの出力を決定してマウスを追従させます。

軌道追従の実装

以下の手順で軌道追従できると思います。②~⑤を1ms周期で繰り返し、目標点が目標軌道の終点に達したら①に戻って新たな目標軌道を設定します。

①目標軌道を設定
②オドメトリによりマウスの位置と速度を取得
③目標軌道に沿って目標点の位置を更新
④上式で目標速度と目標角速度を計算
⑤目標速度と目標角速度に追従するようモーター出力を決定

次回

今回考えた軌道追従を実現するには、速度と角速度を追従させる制御が必要です。次回はこれを実装したいと思います。


Posted in DCマウス研修