クラシックマウスを作ろう!(12)〜台形制御と加減速タイミング〜
初めましての方は初めまして
そうでない方はお疲れ様です。
今回は目標値生成の台形制御と加減速の切り替えタイミングです。
初めの注意点です。
今回はボク自身がやったことがあるプログラム、アルゴリズムですが、
何かしら問題があるなと感じている
ものです。
改善のアイデアには心当たりがありますが、あくまで最初の機体・基礎的な部分をさらっていく目的なのでご留意ください。
台形制御
目標値に追従する方法としてPID制御があることは
前回 :
クラシックマウスを作ろう!(11)〜マウスのモデリングと制御システム〜 - ぱわぷろ活動日誌
書いた通りです。
この目標値はマイクロマウスであれば「重心速度」「重心角速度」の2つであることが多いです。それぞれ並進運動と回転運動を司る目標値です。
前提として、重心速度について述べていきます。角速度も同様に考えてください。
最大速度は変数(パラメータ)なので決め打っておきます。このMAXが出るように加速度やPIDゲイン等のパラメータを調整したり算出します。
では、目標速度を0からいきなりMAXに変えて良いか?
もちろんNOです。制御工学的にはステップ入力と呼ばれるものですが、大きすぎると応答が安定するのに時間がかかります。
したがって目標値は少しずつ上げたり下げたりするのが理想です。最大値になったらそれ以上増やさない事をお忘れなく。
ここまでの前提で目標重心速度を変化させて、加速→等速→減速→停止の動きを作ると次の様な図になります。
このような目標値変化はそのグラフの形から台形制御と呼ばれます。
この重心速度、角速度の台形を上へ下へと沢山作って繋げていくと直進や旋回などの様々な動作を実現できます。
更には、既知の直線での加速も可能になります。
切り替えるタイミング
台形制御で動きを実現できるとして、重要なのは「どこから加減速するか」「どこに止まるか」だと思います。
自律移動においては動きの実現だけでなく、自分の位置を制御しないと壁にぶつかったり、あらぬ方向に進んだりします。
前項で述べたような、速度や角速度を目標値通りに制御する手法では減速するタイミングを間違えると壁に突っ込んだりします。
台形制御に於ける加減速のタイミングは結局のところ
「必要な走行距離と現在の速度、加速度の関係式」で定まります。
加速度は調整されたパラメータ、現在の速度はエンコーダから分かります。
v_tグラフの面積は距離ですね。
原理さえ分かれば簡単な図形問題です。やることは次の通り。
- 現在の速度から減速開始すると止まるまでに動いてしまう距離(三角形の面積)を計算
- 目標距離と現在の走行距離の差分と求めた三角形の面積を比較
- 差分の方が小さくなったら減速開始
これで目標の距離に止まれる減速タイミングを知ることができると思います。
上の図だと止まることが目的なので最終的な速度は0ですが、ここを「最小速度」の変数にしてしまえば直線加速などにも同じ関数が適用できるので幸せになれます。
直線だから飛ばしてカーブの手前で減速したりすると並進速度のグラフはこんなイメージになりますね。
こんなわけで走行の殆どはv_tグラフの扱いによって制御をしているわけです。
プログラミングする際の変数は直線加速も加味すると以下のものになります。
- 走行したい距離
- 初速度
- 最終的な速度
- 加速度
- 最高速度
角速度まで一般化することもできますが処理が複雑になりすぎるのでボクはまだやっていません。
ただ、一般化までやったほうが今後の為になるのは間違いないと思うので進めていくつもりです。
ログ出し
さて、ここまでのシリーズを統合すると速度制御によって真っ直ぐ走行させることができるはずです。
- 目標走行距離を定める
- 重心速度の台形v_tグラフを作る
- PID制御でデューティ比とかを計算
- (角速度は目標値0でPID掛けましょう)
- モータに電圧を印加させる
ここまでやって、やっと真っ直ぐ動かせます。
でも上手くいった場合はいいんですが、想定違いの動きをしたらどうやってデバッグをしますか?
自分の敬愛するマウサーの方々がよく仰っていた言葉があります。
「ロボットの声を聞け」
解釈は色々あります。モータの駆動音を聞いている人、風切り音を聞いている人、タイヤと地面の摩擦音を聞いている人などがいらっしゃいますが、そこまで職人芸チックなことをやるよりも先にできる事があります。
ログを出力して解析する事です。
ログの出力・解析はそれだけでノウハウの塊ですし、個人で工夫されている事が非常に多い部分です。それなのにマウスを作り始めた最初の頃は軽視しがちなので、開発の段階から簡易的な方法でも導入することを強く推奨します。
じゃあまずは何から始めれば良いのか?MATLAB使ったり、GUIで自作してたりと凝ることは可能ですが、1番原始的なのはシリアル通信を用いたprintfデバッグかと思います。
参考リンク:
STM32でUARTをやってみる1 - がれすたさんのDIY日記
実装の方法は多いのですが、マウスではどんなパラメータをログ出しするのでしょうね?
当然デバッグの内容に寄りますが、数値だけでなく、視覚化するのがいいかと思います。
台形制御のデバッグをするなら目的のログをグラフ化するのが1番多いかと思います。
そのためには次の様な変数が必要不可欠でしょう。
- 経過時間
- その時間の目標速度
- その時間の機体速度
- その時間の累積走行距離
これを各時間ごとに出力していくと想定通りの動きをしているのかそれとも検討違いの動きをしているのか分かります。
そして表示されたデータをExcelとかでデバッグするといい感じにグラフ化できるはずです。
どうせなので開発中のいい感じのログと悪い感じのログを出してみました。
オレンジが現在の速度で青が目標値です。
これはボクのログ解析Excelに残っていたデータなのですが、なんで下のがこんな無茶苦茶なのか記憶にありません…。
恐らく壁制御と回転速度制御がどれくらい影響しあっているのか検証していた頃のデータかと思います。
PIDゲインの調整だったり、他の制御との組み合わせによって様々な波形になりますがそれなりに一致するはずなので調整がんばりましょう。理論だった話は制御工学の古典制御で扱う問題なのでそちらに取り組んでもいいかもしれません。
台形制御の違和感
ここまでで一般的な速度制御と台形制御に関する話を進めてきました。
速度の台形制御は様々な方面で多用されていますが、実はこれには理論的に少し問題となる部分があります。
今まで速度に関する話ばかりしていましたが、これはセンサで手軽に入手できるデータが速度でありかつ、目標として非常に分かりやすいからです。ですが、運動方程式なら、まず最初に考えるのは力であり、加速度であり、モータに流れる電流であり、印加電圧です。
つまりは印加電圧と加速度は比例し、感覚的にはほぼ同じものと捉えられます。
それを踏まえた上で速度台形制御をちゃんと考えてみます。
次のグラフは以下のリンクからの転載です。
リンク:
ロボットの軌跡生成:加速度制限を用いて台形速度プロファイルを作成する|Tajima Robotics
実は速度台形制御をしているとき、加速度(=電圧)はデジタルな波形を描いています。RLC回路やばねマスダンパ系などを学んだことがある人は、力や加速度がこんなきれいに変化しないことを知っていると思います。
特に、加速度が急激に変化する部分ですね。
ここの動きを理想に近づけるには、加速度を台形制御することが重要だと言われています。そうすることで急激に変化するのが加加速度、jerkと呼ばれる加速度の微分に対応する物理量になります。
これは自動車の乗り心地への追及などでも取り上げられています。
ここまでは行かなくても、加速度を段階的に上げていったりする工夫がマイクロマウスに取組んでいる人でも多数見られます。
これだけに限らず、本シリーズで紹介した方法は「とりあえず動かす」であって、細かく追及できるものが沢山散りばめられています。
その中で、のどに刺さった魚の小骨のように納得がいかないものもあると思います。
その分野はもしかしたら個人で「興味がある」部分なのかもしれないので是非追及してみてください。
さて、今回はこんなところです。そろそろこのシリーズも終わりが見えてきました。自分の中ではあと2回ぐらいです。次回は姿勢、最終回はズレと迷路関連の紹介をしていこうと思っています。
それが終わるくらいには後輩らに機体を動かしておいて欲しいな…と思いつつ。
では、また。