はじめに

移動ロボットにおける経路追従制御は,自律移動を実現するための重要な技術の一つです.モータードライバーとしてOdriveを使用した場合,高精度なモーター制御加え,モーターのエンコーダーを用いたオドメトリを計算することできます.本記事では,オドメトリを利用して自律移動ロボットの位置を推定し,Odriveによるモーター制御を用いた経路追従の実装方法について説明します.

動作環境

動作環境は以下とおりです.

項目	バージョン
ubuntu	22.04
ROS	ROS2 humble

使用した機材

・Odrive v3.6

・インホイールモーター

・バッテリー

・jetson orin nano

オドメトリの計算

オドメトリは,ロボットの現在位置と向きを推定するために使用される方法で,各ホイールの回転数をもとに計算します.
ホイールの回転数を ODrive モーターコントローラから取得し,ロボットの位置と向きを更新しています.

ソースコード

1. エンコーダの位置取得

各ホイールのエンコーダ値は次のように取得しています.

self.new_pos_r = self.encoder_cpr * self.odrv0.axis0.encoder.pos_estimate
self.new_pos_l = self.encoder_cpr * self.odrv0.axis1.encoder.pos_estimate

ここで,self.encoder_cpr はエンコーダの1回転あたりのカウント数 (CPR: Counts Per Revolution) です.pos_estimate は ODrive が提供するエンコーダの現在の位置（回転数）です.これを CPRで掛けることで, カウント単位での現在の位置が求められます.

2. 各ホイールの移動量計算

次に,現在のエンコーダ値と前回のエンコーダ値との差分を取り,ホイールがどれだけ回転したかを計算します.

delta_pos_r = self.new_pos_r - self.old_pos_r
delta_pos_l = self.new_pos_l - self.old_pos_l

ここで,delta_pos_r と delta_pos_l はそれぞれ右ホイールと左ホイールの回転量です.この回転量をもとに,車体がどれだけ前進したか,どれだけ旋回したかを計算します.

3. オーバーフローの処理

エンコーダは一定のカウント範囲を超えると値がリセットされます.そのため,オーバーフローが発生していないか確認し,必要に応じて補正を行います.

half_cpr = self.encoder_cpr / 2.0
if delta_pos_r > half_cpr:
    delta_pos_r = delta_pos_r - self.encoder_cpr
elif delta_pos_r < -half_cpr:
    delta_pos_r = delta_pos_r + self.encoder_cpr

4. 回転数を距離に変換

エンコーダ値から得たホイールの回転量を距離に変換します.ホイールの回転数をタイヤの円周と掛け合わせて,どれだけの距離を移動したかを計算します.

delta_pos_r_m = delta_pos_r / self.m_s_to_value
delta_pos_l_m = delta_pos_l / self.m_s_to_value * (-1)

5. ロボットの前進距離と回転角度の計算

左右のホイールがどれだけ前進したかを使って,ロボット全体がどれだけ前進し,どれだけ回転したかを計算します.

d = (delta_pos_r_m + delta_pos_l_m) / 2.0  # 前進距離
th = (delta_pos_r_m - delta_pos_l_m) / self.tire_tread  # 旋回角度
xd = math.cos(th)*d
yd = -math.sin(th)*d

6. 新しい位置と向きの更新

現在の位置 (self.x, self.y) と向き (self.theta) を,計算した前進距離と回転角度を使って更新します.

self.x += math.cos(self.theta)*xd - math.sin(self.theta)*yd
self.y += math.sin(self.theta)*xd + math.cos(self.theta)*yd
self.theta = (self.theta + th) % (2*math.pi)

経路追従制御

本実験では，Pure pursuit法を用いて移動ロボットを目標経路に追従させます．Pure pursuit法の詳細については,前回の記事を御覧ください.オドメトリによって推定したロボットの現在位置から,目標経路に向かうための,制御入力 $v, \omega$を求めます.

モーター制御

Odriveの制御方式には，位置制御，速度制御，トルク制御など様々な制御方式が存在し，ユーザーは Odriveのコンフィグレーションファイル内で制御方式を指定することが可能です.本記事では,左右輪それぞれに対する目標回転角速度を指令値として速度制御する.これは,PID制御により実装されているため，制御パラメータのチューニングが必要になります．ここで，左右の車輪の回転角速度 ωL, ωRは以下の式で表されます.

\begin{equation}
\left\{\begin{aligned}
\omega_L & =\frac{v-d \omega}{r} \\
\omega_R & =\frac{v+d \omega}{r}
\end{aligned}\right.
\end{equation}

ここで, $r$ は車輪の半径, $d$ はロボットの中心から車輪までの距離です. この $\omega_L, \omega_R$ に左右の車輪の回転速度が追従するよう制御します.

odriveのコンフィグレーション

実装

実装に必要なコード

はじめに,ロボットを走行させ追従させるための経路を作成します.

source ~/ros2_ws/install/setup.bash
ros2 launch pure_pursuit_planner save_path.py

作成した経路にロボットを追従させます.

ros2 launch pure_pursuit_planner odrive_odom_pure_pursuit.py

参考文献

謝辞

この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.