麦克纳姆轮运动学解算一、麦克纳姆轮介绍了解过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_{tx}$ 表示 X 轴运动的速度，即前后方向，定义向前为正；
$v_{ty}$ 表示 Y 轴运动的速度，即左右方向，定义向左为正；
$\overrightarrow{\omega}$ 表示 yaw 轴自转的角速度，定义逆时针为正。
2.2 计算轮子轴心位置的速度如下图所示，以右前轮为例，蓝色的方框代表轮子，定义以下变量：
$\overrightarrow{r}$为从底盘中心指向轮子轴心的矢量；
$\overrightarrow{v}$为轮子轴心的速度矢量；
$\overrightarrow{v_r}$为轮子轴心沿垂直于$\overrightarrow{r}$的方向（即切线方向）的速度分量；
可以计算出：
将$\overrightarrow{r}$分解为$r_x$和$r_y$，分别计算轮子轴心在X、Y轴的速度分量：
其他三个轮子同理
2.3计算与地面接触的辊子速度由2.2算得的轮子轴心速度，可以分解为沿辊子轴方向的$\overrightarrow{v_\parallel}$ 和垂直辊子轴方向的 $\overrightarrow{v_\perp}$ ，如图所示
其中$\overrightarrow{v_\perp}$用于让辊子空转，可以忽略
定义一个沿辊子方向的单位矢量$\hat{e}$，对于右前轮来说，$\hat{e}=\frac{1}{\sqrt{2}}\cdot\hat{i}+\frac{1}{\sqrt{2}}\cdot\hat{j}$
则沿轴线的速度为$\overrightarrow{v}$在$\hat{e}$方向的投影：
2.4 计算轮子的转速（和地面接触点的线速度）如图所示，轮子转速为$v_w$
由于辊子与轮轴呈45°角，则$v_\omega$可求得：
将2.2求出的$\left\{\begin{matrix}v_x=v_{tx}+\omega\cdot{r_y} \\v_y=v_{ty}+\omega\cdot{r_x}\end{matrix}\right.$带入上式，可求出此轮的转速：
结合以上四个步骤，可以根据底盘运动状态解算出四个轮子的转速：
以上方程组就是O形麦轮底盘的逆运动学模型。
2.5 代码实现12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849//参数宏定义#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）让我们可以通过四个轮子的速度，计算出底盘的运动状态。可以直接根据逆运动学模型中的三个方程解出来，比如：
转换为底盘坐标系下对时间求积分即为里程计变化量
3.2 代码实现123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475//参数宏定义#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