Control
ここではFPGA内のNios IIが行う車体の速度制御について解説する。
車体速度制御のブロック図を以下に示す。
IMUの測定値には重力加速度の影響が含まれるためMahonyフィルタを応用したGravity Filterによって重力を除去する。
以下にGravity Filterの更新式を示す。
チルダの付いたa,ωがそれぞれ加速度センサーとジャイロセンサーの生の測定値である。
これらの測定値より重力ベクトルgを推定し、生の加速度の測定値からgを引いた値を後の処理で使用する。
ちなみにロドリゲスの回転公式R(ωΔt)はωΔtの大きさが非常に小さいためsin(x)=x, cos(x)=1と近似して実装している。
車体速度はIMUとエンコーダと電流センサーの測定値をカルマンフィルタに入力して推定される。
Velocity Filterの状態方程式を以下に示す。
各車輪の角速度と車体の速度は独立しているものとして扱い、摩擦係数Kfが車輪の角速度と車体速度を結びつけている。単純化のため粘性摩擦係数やクーロン摩擦はモデル化していない。
カルマンフィルタで推定したい状態変数xの要素はVx, Vy, Ω, Kf1..Kf4の7つである。他は定数か、あるいはその時々の測定値を与える。
状態方程式を更新周期Δtで離散化するとカルマンフィルタの状態遷移関数fは以下のようになる。
入力uとして各車輪の角速度ω1..ω4を与える。
状態遷移関数fをx付近で線形化すると以下の式になる。なおEは単位行列である。
信念分布の中心μと信念分布の共分散行列Σの遷移は以下のように書ける。
Rは状態遷移モデルの雑音の共分散行列である。
観測方程式hを以下に示す。
各車輪の角加速度と車体の加速度および角速度を観測値としている。
観測方程式hを線形化すると以下の式になる。
観測値から信念分布μ,Σを以下の式で更新する。
Qは観測雑音の共分散行列である。
単純化のためR, Qは共分散0で分散成分のみ非0の共分散行列としている。
Jetsonからは車体の並進速度X, Y[m/s]と回転速度ω[rad/s]の目標車体速度が与えられる。 目標車体速度はその大きさが制限され、Velocity Filterによって推定された車体速度とともに各成分のPI-P制御器に与えられる。 車体速度に掛かるほうの比例項は速度偏差に掛かる比例項によって生じるキックを抑制するために存在している。 これら2つの比例項は比例ゲインKPを持つが重みαと1-αによって重み付けしている。
目標車体速度は3自由度であるのに対し車輪は4つあるため自由度の差を埋めるために下図の一番右に示す成分Cを制御対象に加える。 成分Cは車体の動きに貢献しない本来は打ち消される成分である。 目標車体速度の成分Cは0である。
速度制御器から出力された目標加速度は各モーターの目標電流が電流制限を上回らないように制限される。
加速度制限は以下の二次計画問題を解くことで行うことができる。
目標加速度をa、モーターの電流制限値をℓとして次の二次計画問題を有効制約法を用いて解くと、モーターの電流制限を超えないように制限された加速度がxとして得られる。
どの加速度成分を優先して割り当てるかは重みwによって設定できる。
4つの変数、8つの不等式制約となかなか重たい計算に見えるが不等式は同時に4つしか有効にならなず、途中で逆行列が必要となる場面では16通りの有効な制約についてあらかじめ逆行列を計算しておけるため見かけほど計算量は多くない。 本プログラムではconstexprな行列演算クラスを作成しコンパイル時に16個の逆行列を求めている。
Velocity Filterによって推定される車体速度から求めた各車輪の位置の地面に対する速度が実際の車輪の速度と異なるとき、その速度差にスリップ抑制ゲインを掛けた値を各モーターコントローラの目標電流に加減算する。 これによりスリップした車輪が過剰な速度で回転することを防ぐことができる。 また車体が完全に浮遊しているときに車輪の速度が発散してしまうことも防ぐことができる。
Phoenixではモーターの駆動電源に昇圧チョッパの出力を用いている。 この昇圧チョッパは双方向ではないためモーターの回生電力が全体の消費電力を上回ると電源電圧が上昇してしまう。 これを防ぐため回生電力が生じる状況で回生エネルギーが閾値を超えたら回生電力を生じさせているモーターをショートさせて減速させる。