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

文章目录

• 前言
• 一、速度解算过程
• 二、里程计计算过程
• • 逆运动学求解速度
• 总结

---

前言

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

---

一、速度解算过程

在该模型中,使用的是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);  // 发送计算好的四轮速度  
}

二、里程计计算过程

里程计计算,可运动微积分求解,固定一定内的短时间,对路程进行计算.

代码如下：

static void RobotCalculate(void){
    // 根据每帧的motors[4]计算更新g_robot
    float deltacounts[]={0,0,0,0};
    float delta_x_o, delta_y_o;
    float delta_x, delta_y;
    float everycount;
    
    g_robot.theta = yaw * PI / 180.0f;
    
    while((g_robot.theta >= 2.0f * PI) || (g_robot.theta < 0))
    {
        if(g_robot.theta >= 2.0f * PI)
            g_robot.theta = g_robot.theta - 2.0f * PI;
        if(g_robot.theta < 0)
            g_robot.theta = g_robot.theta + 2.0f * PI;
    }
    
    everycount = MOTORNUMBER / (2.0f * PI * R);
    
    deltacounts[0] = motors[0].counts - motors[0].last_counts;
    deltacounts[1] = (motors[1].counts - motors[1].last_counts);
    deltacounts[2] = (motors[2].counts - motors[2].last_counts);
    deltacounts[3] = motors[3].counts - motors[3].last_counts;
    
    if(deltacounts[0] > 30000 || deltacounts[0] < -30000)
        deltacounts[0] = 0;
    if(deltacounts[1] > 30000 || deltacounts[1] < -30000)
        deltacounts[1] = 0;
    if(deltacounts[2] > 30000 || deltacounts[2] < -30000)
        deltacounts[2] = 0;
        
    delta_x_o = (-deltacounts[1] + deltacounts[2]) / (2.0f * everycount);
    delta_y_o = (deltacounts[0] + deltacounts[1]) / (2.0f * everycount);


    if (g_robot.theta >= 0 && g_robot.theta < (PI/2.0f))
    {
        delta_x = delta_x_o * sin(PI/2.0f - g_robot.theta) - delta_y_o * sin(g_robot.theta);
        delta_y = delta_x_o * cos(PI/2.0f - g_robot.theta) + delta_y_o * cos(g_robot.theta);
    }
    
    if (g_robot.theta >= (PI/2.0f) && g_robot.theta < PI)
    {
        delta_x = -(delta_x_o * sin(g_robot.theta - PI/2.0f) + delta_y_o * cos(g_robot.theta - (PI/2.0f)));
        delta_y = delta_x_o * cos(g_robot.theta - PI/2.0f) - delta_y_o * sin(g_robot.theta - (PI/2.0f));
    }
    
    if (g_robot.theta >= PI && g_robot.theta < (3.0f*PI/2.0f))
    {
        delta_x = -delta_x_o * cos(g_robot.theta - PI) + delta_y_o * cos(3.0f*PI/2.0f - g_robot.theta);
        delta_y = -(delta_x_o * sin(g_robot.theta - PI) + delta_y_o * sin(3.0f*PI/2.0f - g_robot.theta));
    }
    
    if (g_robot.theta >= (3.0f*PI/2.0f) && g_robot.theta < (2.0f*PI))
    {
        delta_x = delta_x_o * sin(g_robot.theta - 3.0f*PI/2.0f) + delta_y_o * cos(g_robot.theta - 3.0f*PI/2.0f);
        delta_y = -delta_x_o * cos(g_robot.theta - 3.0f*PI/2.0f) + delta_y_o * sin(g_robot.theta - 3.0f*PI/2.0f);
    }
    
    if(delta_x > 0.5f ||delta_x < -0.5f)
        delta_x = 0;
    if(delta_y > 0.5f ||delta_y < -0.5f)
        delta_y = 0;
    
    g_robot.pos_x += delta_x;
    g_robot.pos_y += (delta_y);
     
    motors[0].last_counts = motors[0].counts;
    motors[1].last_counts = motors[1].counts;
    motors[2].last_counts = motors[2].counts;
    motors[3].last_counts = motors[3].counts;

逆运动学求解速度

这个就比较简单了,直接用路程除以时间就可以了.或者知道了电机的四个v[4],通过逆解,应用其中三个的速度即可求得Vx,Vy,omega,上图中也有公式表示,可自行参考.在此次的代码中,把他写在了中断中,通过路程除以时间求得
代码如下：

//定时器3中断服务函数，定时周期0.1s
void TIM3_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //溢出中断
	{
        static float last_x, last_y, last_theta;
		g_robot.vx = (g_robot.pos_x - last_x)/0.1f;
        g_robot.vy = (g_robot.pos_y - last_y)/0.1f;
        g_robot.omega = (g_robot.theta - last_theta)/0.1f;
        last_x = g_robot.pos_x;
        last_y = g_robot.pos_y;
        last_theta = g_robot.theta;
	}
	TIM_ClearITPendingBit(TIM3,TIM_IT_Update);  //清除中断标志位
}

---

总结

在求里程计的时候,踩了一些坑,不过还好解决了.以上仅供参考.