ROS机器人学习——麦克纳姆轮运动学解算
麦克纳姆轮运动学解算
一、麦克纳姆轮介绍
了解过Robomaster的同学都知道，RM战车所用的轮子均为麦克纳姆轮，这种轮子安装方式与普通轮子无异，可安装于平行轴上，但是麦克纳姆轮可以实现全向移动，即前后运动、水平移动、绕中心自转。正因为以上优点，许多工业上的全向移动平台都会应用这种轮子。缺点也有，就是不耐磨，需要定期更换。
麦克纳姆轮由两部分组成：轮毂和辊子，轮毂为轮子的主体，辊子为轮毂周围的类似椭球体的小轮子，轮毂和辊子都有自己的轴，且轮毂轴与辊子轴夹角为45°（可以为其他角度但45°角最为常见）
麦轮的安装方式也有讲究，虽然都是同轴安装，但与普通轮子不同，麦轮分为左旋和右旋两种，在一个四轮底盘上需要用两个左旋和两个右旋。安装方式为O型，如图所示：
左图为安装后你看到的样子，右图为四个轮子与地面接触的辊子围成的形状，也就是“O形”
这里的O形指的是与地面接触的辊子围成的形状噢，不要再问为什么左图看起来是个X了
二、麦克纳姆轮运动学模型
1. 基础知识
1.1 坐标系统
在ROS机器人中，坐标系统使用右手定义
对于ROS机器人，如果以它为坐标系的原点，那么
- x轴：前方
- y轴：左方
- z轴：上方
如图所示：
除此之外，对于旋转运动，也使用右手定义：
根据右手定义，围绕 z轴正旋转 是 逆时针旋转
1.2 测量单位
ROS使用公制 ：
- 线速度：
m/s - 角速度：
rad/s
1.3 轮子序号定义
左前1 右前2
左后3 右后4
2. 逆运动学解析
逆运动学模型（inverse kinematic model）得到的公式可以根据底盘的运动状态解算出四个轮子的速度。
2.1 底盘运动的分解
刚体在平面内的运动可以分解为三个独立分量：X轴平动、Y轴平动、yaw 轴自转。底盘的运动也可以分解为三个量：
如下图所示:
- v t x v_{tx} vtx 表示 X 轴运动的速度，即前后方向，定义向前为正；
- v t y v_{ty} vty 表示 Y 轴运动的速度，即左右方向，定义向左为正；
- ω → \overrightarrow{\omega} ω 表示 yaw 轴自转的角速度，定义逆时针为正。
2.2 计算轮子轴心位置的速度
如下图所示，以右前轮为例，蓝色的方框代表轮子，定义以下变量：
- r → \overrightarrow{r} r为从底盘中心指向轮子轴心的矢量；
- v → \overrightarrow{v} v为轮子轴心的速度矢量；
- v r → \overrightarrow{v_r} vr为轮子轴心沿垂直于 r → \overrightarrow{r} r的方向（即切线方向）的速度分量；
可以计算出：
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \overrightarro…
将 r → \overrightarrow{r} r分解为 r x r_x rx和 r y r_y ry，分别计算轮子轴心在X、Y轴的速度分量：
{ v x = v t x + ω ⋅ r y v y = v t y + ω ⋅ r x \left\{\begin{matrix} v_x=v_{tx}+\omega\cdot{r_y} \\ v_y=v_{ty}+\omega\cdot{r_x} \end{matrix}\right. {vx=vtx+ω⋅ryvy=vty+ω⋅rx
其他三个轮子同理
2.3计算与地面接触的辊子速度
由2.2算得的轮子轴心速度，可以分解为沿辊子轴方向的 v ∥ → \overrightarrow{v_\parallel} v∥ 和垂直辊子轴方向的 v ⊥ → \overrightarrow{v_\perp} v⊥ ，如图所示
其中 v ⊥ → \overrightarrow{v_\perp} v⊥用于让辊子空转，可以忽略
定义一个沿辊子方向的单位矢量 e ^ \hat{e} e^，对于右前轮来说， e ^ = 1 2 ⋅ i ^ + 1 2 ⋅ j ^ \hat{e}=\frac{1}{\sqrt{2}}\cdot\hat{i}+\frac{1}{\sqrt{2}}\cdot\hat{j} e^=21⋅i^+21⋅j^
则沿轴线的速度为 v → \overrightarrow{v} v在 e ^ \hat{e} e^方向的投影：
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \overrightarr…
2.4 计算轮子的转速（和地面接触点的线速度）
如图所示，轮子转速为 v w v_w vw
由于辊子与轮轴呈45°角，则 v ω v_\omega vω可求得：
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ v_w&=\frac{v_…
将2.2求出的 { v x = v t x + ω ⋅ r y v y = v t y + ω ⋅ r x \left\{\begin{matrix} v_x=v_{tx}+\omega\cdot{r_y} \\ v_y=v_{ty}+\omega\cdot{r_x} \end{matrix}\right. {vx=vtx+ω⋅ryvy=vty+ω⋅rx带入上式，可求出此轮的转速：
v w = v t x + v t y + ω ( r x + r y ) v_w=v_{tx}+v_{ty}+\omega(r_x+r_y) vw=vtx+vty+ω(rx+ry)
结合以上四个步骤，可以根据底盘运动状态解算出四个轮子的转速：
{ v w 1 = v t x − v t y − ω ( r x + r y ) v w 2 = v t x + v t y + ω ( r x + r y ) v w 3 = v t x + v t y − ω ( r x + r y ) v w 4 = v t x − v t y + ω ( r x + r y ) \left\{\begin{matrix} v_{w1}=v_{tx}-v_{ty}-\omega(r_x+r_y)\\ v_{w2}=v_{tx}+v_{ty}+\omega(r_x+r_y)\\ v_{w3}=v_{tx}+v_{ty}-\omega(r_x+r_y)\\ v_{w4}=v_{tx}-v_{ty}+\omega(r_x+r_y) \end{matrix}\right. ⎩⎪⎪⎨⎪⎪⎧vw1=vtx−vty−ω(rx+ry)vw2=vtx+vty+ω(rx+ry)vw3=vtx+vty−ω(rx+ry)vw4=vtx−vty+ω(rx+ry)
以上方程组就是O形麦轮底盘的逆运动学模型。
2.5 代码实现
//参数宏定义
#define ENCODER_RESOLUTION 1440.0 //编码器分辨率, 轮子转一圈，编码器产生的脉冲数
#define WHEEL_DIAMETER 0.058 //轮子直径,单位：米
#define D_X 0.18 //底盘Y轴上两轮中心的间距
#define D_Y 0.25 //底盘X轴上两轮中心的间距
#define PID_RATE 50 //PID调节PWM值的频率
double pulse_per_meter = 0;
float rx_plus_ry_cali = 0.3;
double angular_correction_factor = 1.0;
double linear_correction_factor = 1.0;
double angular_correction_factor = 1.0;
/**
* @函数作用：运动学解析参数初始化
*/
void Kinematics_Init(void)
{
//轮子转动一圈，移动的距离为轮子的周长WHEEL_DIAMETER*3.1415926，编码器产生的脉冲信号为ENCODER_RESOLUTION。则电机编码器转一圈产生的脉冲信号除以轮子周长可得轮子前进1m的距离所对应编码器计数的变化
pulse_per_meter = (float)(ENCODER_RESOLUTION/(WHEEL_DIAMETER*3.1415926))/linear_correction_factor;
float r_x = D_X/2;
float r_y = D_Y/2;
rx_plus_ry_cali = (r_x + r_y)/angular_correction_factor;
}
/**
* @函数作用：逆向运动学解析，底盘三轴速度-->轮子速度
* @输入：机器人三轴速度 m/s
* @输出：电机应达到的目标速度（一个PID控制周期内，电机编码器计数值的变化）
*/
void Kinematics_Inverse(int16_t* input, int16_t* output)
{
float v_tx = (float)input[0];
float v_ty = (float)input[1];
float omega = (float)input[2];
static float v_w[4] = {0};
v_w[0] = v_tx - v_ty - (r_x + r_y)*omega;
v_w[1] = v_tx + v_ty + (r_x + r_y)*omega;
v_w[2] = v_tx + v_ty - (r_x + r_y)*omega;
v_w[3] = v_tx - v_ty + (r_x + r_y)*omega;
//计算一个PID控制周期内，电机编码器计数值的变化
output[0] = (int16_t)(v_w[0] * pulse_per_meter/PID_RATE);
output[1] = (int16_t)(v_w[1] * pulse_per_meter/PID_RATE);
output[2] = (int16_t)(v_w[2] * pulse_per_meter/PID_RATE);
output[3] = (int16_t)(v_w[3] * pulse_per_meter/PID_RATE);
}
3. 正运动学解析
3.1 正运动学模型
正运动学模型（forward kinematic model）让我们可以通过四个轮子的速度，计算出底盘的运动状态。可以直接根据逆运动学模型中的三个方程解出来，比如：
{ v t x = v 4 + v 3 2 v t y = v 3 − v 1 2 ω = v 2 − v 3 2 ( r x + r y ) \left\{\begin{matrix} v_{tx}=\frac{v_4+v_3}{2}\\ v_{ty}=\frac{v_3-v_1}{2}\\ \omega=\frac{v_2-v_3}{2(r_x+r_y)} \end{matrix}\right. ⎩⎨⎧vtx=2v4+v3vty=2v3−v1ω=2(rx+ry)v2−v3
转换为底盘坐标系下对时间求积分即为里程计变化量
3.2 代码实现
//参数宏定义
#define ENCODER_MAX 32767
#define ENCODER_MIN -32768
#define ENCODER_LOW_WRAP ((ENCODER_MAX - ENCODER_MIN)*0.3+ENCODER_MIN)
#define ENCODER_HIGH_WRAP ((ENCODER_MAX - ENCODER_MIN)*0.7+ENCODER_MIN)
#define PI 3.1415926
//变量定义
int32_t wheel_turns[4] = {0};
int32_t encoder_sum_current[4] = {0};
/**
* @函数功能：正向运动学解析，轮子编码值->底盘三轴里程计坐标
* @输入：编码器累加值
* @输出：三轴里程计 x y yaw
*/
void Kinematics_Forward(int16_t* input, int16_t* output)
{
static double dv_w_times_dt[4]; //轮子瞬时变化量dxw=dvw*dt
static double dv_t_times_dt[3]; //底盘瞬时变化量dxt=dvt*dt
static int16_t encoder_sum[4];
//将左面轮子编码器累加值乘以-1，以计算前进的距离
encoder_sum[0] = -input[0];
encoder_sum[1] = input[1];
encoder_sum[2] = -input[2];
encoder_sum[3] = input[3];
//编码器计数溢出处理
for(int i=0;i<4;i++)
{
if(encoder_sum[i] < ENCODER_LOW_WRAP && encoder_sum_current[i] > ENCODER_HIGH_WRAP)
wheel_turns[i]++;
else if(encoder_sum[i] > ENCODER_HIGH_WRAP && encoder_sum_current[i] < ENCODER_LOW_WRAP)
wheel_turns[i]--;
else
wheel_turns[i]=0;
}
//将编码器数值转化为前进的距离，单位m
for(int i=0;i<4;i++)
{
dv_w_times_dt[i] = 1.0*(encoder_sum[i] + wheel_turns[i]*(ENCODER_MAX-ENCODER_MIN)-encoder_sum_current[i])/pulse_per_meter;
encoder_sum_current[i] = encoder_sum[i];
}
//要计算坐标所以变回来
dv_w_times_dt[0] = -dv_w_times_dt[0];
dv_w_times_dt[1] = dv_w_times_dt[1];
dv_w_times_dt[2] = -dv_w_times_dt[2];
dv_w_times_dt[3] = dv_w_times_dt[3];
//计算底盘坐标系(base_link)下x轴、y轴变化距离m与Yaw轴朝向变化rad 一段时间内的变化量
dv_t_times_dt[0] = ( dv_w_times_dt[3] + dv_w_times_dt[2])/2.0;
dv_t_times_dt[1] = ( dv_w_times_dt[2] - dv_w_times_dt[0])/2.0;
dv_t_times_dt[2] = ( dv_w_times_dt[1] - dv_w_times_dt[2])/(2*wheel_track_cali);
//积分计算里程计坐标系(odom_frame)下的机器人X,Y,Yaw轴坐标
//dx = ( vx*cos(theta) - vy*sin(theta) )*dt
//dy = ( vx*sin(theta) + vy*cos(theta) )*dt
output[0] += (int16_t)(cos((double)output[2])*dv_t_times_dt[0] - sin((double)output[2])*dv_t_times_dt[1]);
output[1] += (int16_t)(sin((double)output[2])*dv_t_times_dt[0] + cos((double)output[2])*dv_t_times_dt[1]);
output[2] += (int16_t)(dv_t_times_dt[2]*1000);
//Yaw轴坐标变化范围控制-2Π -> 2Π
if(output[2] > PI)
output[2] -= 2*PI;
else if(output[2] < -PI)
output[2] += 2*PI;
//发送机器人X轴y轴Yaw轴瞬时变化量，在ROS端除以时间
output[3] = (int16_t)(dv_t_times_dt[0]);
output[4] = (int16_t)(dv_t_times_dt[1]);
output[5] = (int16_t)(dv_t_times_dt[2]);
}
参考文献：
【1】https://zhuanlan.zhihu.com/p/20282234
【2】https://blog.csdn.net/shixiaolu63/article/details/78496457