タグ: 四駆

順運動学forward kinematic方程式

メカナム車輪の回転速度はロボット車体中心のx・y軸方向線形速度とz軸まわりの角速度より、以下の式(順運動学方程式)で表される。

// 左右輪距離、前後輪距離
#define WHEEL_SEPARATION_WIDTH DISTANCE_LEFT_TO_RIGHT_WHEEL/2
#define WHEEL_SEPARATION_LENGTH DISTANCE_FRONT_TO_REAR_WHEEL/2
// 回転速度単位：rad / s
wheel_front_left =(linear.x-linear.y-(WHEEL_SEPARATION_WIDTH+WHEEL_SEPARATION_LENGTH)*angular.z)/WHEEL_RADIUS;
wheel_front_right=(linear.x+linear.y+(WHEEL_SEPARATION_WIDTH+WHEEL_SEPARATION_LENGTH)*angular.z)/WHEEL_RADIUS;
wheel_rear_left  =(linear.x+linear.y-(WHEEL_SEPARATION_WIDTH+WHEEL_SEPARATION_LENGTH)*angular.z)/WHEEL_RADIUS;
wheel_rear_right =(linear.x-linear.y+(WHEEL_SEPARATION_WIDTH+WHEEL_SEPARATION_LENGTH)*angular.z)/WHEEL_RADIUS;
// 右に配置されるたモータの逆転が必要
wheel_front_right=-1*wheel_front_right
wheel_rear_right =-1*wheel_rear_right

順運動学方程式の簡単な証明

ロボットがx軸方向へ移動の場合
$$\begin{bmatrix}v_{front-left}\\v_{front-right}\\v_{rear-left}\\v_{rear-right}\end{bmatrix}=\begin{bmatrix}linear.x\\linear.x\\linear.x\\linear.x\end{bmatrix}$$
ロボットがy軸方向へ移動の場合
$$\begin{bmatrix}v_{front-left}\\v_{front-right}\\v_{rear-left}\\v_{rear-right}\end{bmatrix}=\begin{bmatrix}-linear.y\\linear.y\\linear.y\\-linear.y\end{bmatrix}$$
z軸まわりロボット中心をめぐる回転の場合
$$\begin{bmatrix}v_{front-left}\\v_{front-right}\\v_{rear-left}\\v_{rear-right}\\width\\length\end{bmatrix}=\begin{bmatrix}-(width+length)*angular.z/2\\(width+length)*angular.z/2\\-(width+length)*angular.z/2\\(width+length)*angular.z/2\\distance_{left2right}\\distance_{front2rear}\end{bmatrix}$$
以上の式を合わせると、順運動学方程式が導かれる。

逆運動学backward kinematic方程式

ロボット車体中心のx・y軸方向線形速度とz軸まわりの角速度は、メカナム車輪の回転速度より、以下の式(逆運動学方程式)で表される。

linear.x =(wheel_front_left+wheel_front_right+wheel_rear_left+wheel_rear_right)*WHEEL_RADIUS/4;
linear.y =(-wheel_front_left+wheel_front_right+wheel_rear_left-wheel_rear_right)*WHEEL_RADIUS/4;
angular.z=(-wheel_front_left+wheel_front_right-wheel_rear_left+wheel_rear_right)*WHEEL_RADIUS/(4*(WHEEL_SEPARATION_WIDTH+WHEEL_SEPARATION_LENGTH))

関連記事

9軸IMUセンサ 6軸／9軸フュージョン 低遅延 USB出力 補正済み ROS対応