麦克纳姆轮速度分解计算及里程计计算

麦克纳姆轮速度分解计算及里程计计算

---

前言

麦克纳姆轮在现如今的机器人应用中十分广泛,经我自己的查阅资料和整理,得到下面的解算过程

---

一、速度解算过程

在该模型中,使用的是O-正方形模型为例,具体计算模型如下图所示(左上为1,右上为2,右下为3,左下为4)

如有不理解的地方,可参见以下链接:麦轮正运动学解算原理
在计算中,是在stm32中对速度及里程计进行解算,具体代码如下:

void ChassisVelSet(float vx, float vy, float omega){
       
    /* 运动学计算 */
    float v1, v2, v3, v4;
    
    /* 速度限幅 */
    vx = vx < VEL_LIM ? vx : VEL_LIM;
    vy = vy < VEL_LIM ? vy : VEL_LIM;
    omega = omega < OMEGA_LIM ? omega : OMEGA_LIM;
    
//    // 单位m/s,C表示横向与纵向轮子间距离的和,所以除以2
//    v1 = (vy + vx) - omega * C / 2.0f; //左上轮,单位为rad/s
//    v2 = (vy - vx) + omega * C / 2.0f; //右上
//    v3 = (vy + vx) + omega * C / 2.0f; //右下
//    v4 = (vy - vx) - omega * C / 2.0f; //左下
//    
//    // 单位rpm 原始式子
//    v1 = ((vy + vx) / (2.0f * PI * R) - omega * C / (2.0f * 2.0f * PI * R))*60.0f/(2.0f*PI); 
//    v2 = ((vy - vx) / (2.0f * PI * R) + omega * C / (2.0f * 2.0f * PI * R))*60.0f/(2.0f*PI); 
//    v3 = ((vy + vx) / (2.0f * PI * R) + omega * C / (2.0f * 2.0f * PI * R))*60.0f/(2.0f*PI);
//    v4 = ((vy - vx) / (2.0f * PI * R) - omega * C / (2.0f * 2.0f * PI * R))*60.0f/(2.0f*PI);
        
    v1 = 2.0f * ((vy + vx) / R - omega * C / (2.0f * R))*60.0f; 
    v2 = 2.0f * ((vy - vx) / R + omega * C / (2.0f * R))*60.0f; 
    v3 = 2.0f * ((vy + vx) / R + omega * C / (2.0f * R))*60.0f;
    v4 = 2.0f * ((vy - vx) / R - omega * C / (2.0f * R))*60.0f;
       
    /* 运动学计算End */
    WheelsVelSet(v1,v2,v3,v4);