前言

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

一、速度解算过程

在该模型中,使用的是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_robotfloat 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);  //清除中断标志位
}

总结

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

麦克纳姆轮速度分解计算及里程计计算相关推荐

麦克纳姆轮底盘-正反向运动学-里程估计
此篇博客转自本作者在古月居的博客:https://www.guyuehome.com/33978 前言: 大概在大一的时候由于Robocon比赛就开始调试麦克纳姆轮底盘.恰好,最近有个项目需要用到这个 ...
麦克纳姆轮速度分解再分析
关注同名微信公众号"混沌无形",有趣好文! 原文链接:https://mp.weixin.qq.com/s/GWhlXsuY6QYyoZydaSYpjQ(包含原文PDF百度云下载链 ...
四轮控制算法麦轮_四轮麦克纳姆轮巡检机器人运动控制方法与流程
本发明属于巡检机器人运动控制技术领域,尤其涉及一种四轮麦克纳姆轮巡检机器人运动控制方法. 背景技术: 移动式机器人与固定基座的机器人相比,具有更大.更灵活的工作空间,但同时轮式运动引入了非完整约束.作 ...
麦克纳姆轮转速计算（运动解算）（_学习笔记__）
这篇文章只做为一篇学习笔记发布出来,总结了我学习上遇到过的问题和我的一些思考,可以帮助大家快速学习以及学习后的二次启发,说不定能解决你目前遇到的问题呢.只讲了麦轮的轮子转速是怎样计算出来的,关于它的特 ...
关于麦克纳姆轮的速度分解，以及多方向控制
参考文章: [1]李彦文,楼佩煌,楼航飞,翟晶晶.协同搬运系统中的麦克纳姆轮AGV运动控制研究[J].机械制造与自动化,2021,50(06):224-227.DOI:10.19344/j.cnki. ...
【控制】麦克纳姆轮 Mecanum 小车模型分析
文章目录简介单个麦轮受力分析单个麦轮速度分析正运动学逆运动学附 Ref: 简介移动机器人的运动学模型决定了如何将车轮速度映射到机器人的本体速度,而动力学模型则决定着如何将车轮扭矩映射到机 ...
ROS麦克纳姆轮底盘制作（上）
目录 1.底盘硬件框图 2.麦克纳姆轮模型介绍 2.1 麦克纳姆轮下的运动学模型 2.2 正解运动学模型 2.3 逆解运动学模型 3 底盘软件框架 4 参考资料在这一个专题中,我们将介绍如何搭建利用 ...
一文读懂麦克纳姆轮全向移动原理及剖析
扣扣技术交流群:460189483 参考文章如下,计算过程小白可能看不懂,于是做进一步补充,写出该文 https://zhuanlan.zhihu.com/p/20282234?utm_source= ...
基于STM32的麦克纳姆轮小车的寻迹实现
个人博客:http://www.chenjianqu.com/ 原文链接:http://www.chenjianqu.com/show-27.html 本项目用STM32F407,麦克纳姆轮小车,八路 ...

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

文章目录

前言

一、速度解算过程

二、里程计计算过程

逆运动学求解速度

总结

麦克纳姆轮速度分解计算及里程计计算相关推荐

最新文章

热门文章