われがわログ

最適化アルゴリズムとかプログラミングについて書きたい

Near-identity diffeomorphismsを用いた差動二輪ロボットの制御

Python Robotics

Turtlebot3をシミュレータ上で動作させて以来、「差動二輪ロボットの駆動則（速度指令値から進行方向速度・回転角速度への変換則）って何があるんだろう」と思っていたのだが、よさげなやつを最近発見したのでメモしておく。

東工大＆Georgia Techによる論文[1]を読んでいたところ、Georgia Tech側のロボットは差動二輪ロボットのため、論文[2]の技術で運動方程式を

\dot{x} = u

なるsingle integrators（なんて訳したらよいのかわからん）としてモデル化しているとの記述があった。

私の頭でもすんなり理解できたので詳細は論文[2]の方を読んでほしいが、 論文[2]のキモはx=[x_1\;x_2]^ T（2次元位置）を並進速度v_1と角速度\omegaで制御するとき、差動二輪ロボットでは\dot{x}_ 1と\dot{x} _ 2を両方同時に制御できないので、 それだったらz=x+ \lambda (R e_1)という新しい変数を考えてやって、これを制御すればよいというもの（違ってたら指摘ください）。 ここで、e_1はロボット固定座標系（進行方向が+X、紙面上向きが+Zの右手系）の一軸方向の単位ベクトルの、ロボット固定座標系での表現、Rはロボット固定座標系から慣性座標系への座標変換行列である。 実際にzを慣性系で時間微分してみると

\dot z = R_\lambda v + (R_\lambda e_1) \dot{\lambda} \quad \text{where} \quad v=[v_1\; \omega]^ T, R_\lambda = [Re_1 | \lambda Re_2 ]

となって並進速度v_1 \in \mathbb{R}と角速度\omega \in \mathbb{R}で\dot{z}を自由に制御できることがわかる。 \lambda(t)=0のときは\omegaが現れないことに注意してほしい。 そして、\lambda (t) \to 0となるようにうまく（論文では \dot{\lambda} = -c _ \lambda (\lambda - \varepsilon), \lambda(0) > \varepsilon >0, c _ \lambda> 0と） 設定したのちにz（これ自体はsingle integratorにできる）を制御してやれば、zを通じてxも所望の値の近傍に到達する、というわけである。なお、zがnear-identity diffeomorphismsの一つである。

制御結果は以下の通り。ダイナミクス制御（制御量が力・トルクレベル）版とキネマティック制御（制御量が速度・角速度レベル）版の両方をPythonで実装した。

dynamics:

https://raw.githubusercontent.com/estshorter/nir/outputs/dynamics.gif

kinematics:

https://raw.githubusercontent.com/estshorter/nir/outputs/kinematics.gif

リポジトリ：

github.com

なお、論文[3]によれば、Georgia Techは\lambdaが固定のものを使っているようだ。

参考文献

  • [1] R. Funada et al., "Coordination of Robot Teams Over Long Distances: From Georgia Tech to Tokyo Tech and Back-An 11,000-km Multirobot Experiment," in IEEE Control Systems Magazine, vol. 40, no. 4, pp. 53-79, Aug. 2020, doi: 10.1109/MCS.2020.2990515.
  • [2] R. Olfati-Saber, "Near-identity diffeomorphisms and exponential ε-tracking and εstabilization of first-order nonholonomic SE(2) vehicles," Proceedings of the 2002 American Control Conference (IEEE Cat. No.CH37301), Anchorage, AK, USA, 2002, pp. 4690-4695 vol.6, doi: 10.1109/ACC.2002.1025398.
  • [3] P. Glotfelter and M. Egerstedt, "A Parametric MPC Approach to Balancing the Cost of Abstraction for Differential-Drive Mobile Robots," 2018 IEEE International Conference on Robotics and Automation (ICRA), Brisbane, QLD, 2018, pp. 732-737, doi: 10.1109/ICRA.2018.8461234.