延安免费做网站,淮南网站开发,做网站用com还是cn好,先做网站后付款✨✨ Rqtz 个人主页 : 点击✨✨
#x1f308;Qt系列专栏:点击
#x1f388;Qt智能车上位机专栏: 点击#x1f388; 本篇文章介绍的是有关于全向轮运动学分析#xff0c;单片机与上位机通信C代码以及ROS里程计解算的内容。
目录
大纲 ROS#xff08;机器人操作系统Qt系列专栏:点击
Qt智能车上位机专栏: 点击 本篇文章介绍的是有关于全向轮运动学分析单片机与上位机通信C代码以及ROS里程计解算的内容。
目录
大纲 ROS机器人操作系统
全向轮运动学分析
a,b,c三轮
a轮
c轮
全向轮正运动解算公式(顺时针为正方向)
全向轮逆运动解算公式
矩阵形式
单片机与上位机通信
通信协议的设定共9位
上位机发送下位机解析将线速度序列化转换为编码器脉冲
举例说明
下位机发送上位机解析将编码器脉冲反序列化转换为线速度
为什么说高8位大于等于128就为负数
线速度到轮子编码器数值转速的转化系数
里程计速度解算
公式总结 大纲
全向轮运动学结算主要涉及到
各个轮子的速度解算顺逆时针各个轮子和速度公式速度正交分解示意图
单片机与上位机通信主要涉及到
如何将解算后的轮子转速转换为编码器数值并按照设定通信协议发送下位机如何接收下位机反馈编码器数值并转化为轮子转速涉及上位机发送与接收通信协议以及数值解编码
里程计解算主要是涉及到:
根据轮子转速和imu偏航角度得到机器人在x,y轴行使距离结合ROS机器人操作系统发布里程计数据
文章最后附公式总结 ROS机器人操作系统 ROS机器人操作系统Robot Operating System是专为机器人软件开发所设计出来的一套电脑操作系统架构。本文的运动学分解将结合ROS机器人操作系统进行进一步的应用。 全向轮运动学分析 全向轮Omni-wheels以其独特的运动能力和灵活性成为了众多研究者和技术爱好者关注的焦点。不同于传统的轮式移动系统全向轮能够在水平面上实现任意方向的平滑移动无需改变轮子的方向或进行复杂的转向操作。这种革命性的移动方式不仅极大地拓宽了机器人的应用范围也为自动化、物流、服务机器人等领域带来了前所未有的可能性。 图一为90度转轴驱动图二为电机直接驱动。 a,b,c三轮 全向轮a,b,c三个轮子的线速度分别为Va,Vb,Vc,机器人底盘整体的x轴线速度为Vx,整体的y轴线速度为Vy(以ros中的坐标系为准)机器人底盘逆时针旋转的角速度为w轮子距离底盘中心的距离为L。其中轮子与水平线的夹角为120度。 以顺时针方向为正方向。
a轮 1. 对于a轮来讲按照图中顺时针的正方向将机器人整体x轴线速度与y轴线速度正交分解以Vx为例将其分解到沿轮子方向Vx2和垂直于轮子方向Vx1;以Vy为例将其分解到沿轮子方向Vy2和垂直于轮子方向Vy1。 按照图中顺时针的正方向a轮的和速度为Vx2Vy2。在根据夹角60度可得到a轮的线速度公式(机器人底盘逆时针旋转时) 其中由图可知为30度为60度。由V 得到 由于机器人底盘逆时针旋转时圆周运动在a轮的切向速度与轮子正方向相反,所以为负的。 2. a轮的线速度公式(机器人底盘顺时针旋转时) b轮 同理可得
b轮的线速度公式(机器人底盘逆时针旋转时) b轮的线速度公式(机器人底盘顺时针旋转时) b轮中图的为60度。
c轮 c轮的线速度公式(机器人底盘逆时针旋转时): c轮的线速度公式(机器人底盘顺时针旋转时): 全向轮正运动解算公式(顺时针为正方向)
根据ros的标准坐标系右手定则在ros中速度大小都是向前向左为正逆时针旋转为正所以结合ros的标准坐标系为-wL得出以下公式 全向轮逆运动解算公式
本质为解三元一次方程组求解 矩阵形式 根据ros速度回调函数的线角速度分解轮子编码器数值函数
/*目标速度回调函数*/
void cmd_velCB(const geometry_msgs::Twist msg)
{ target_Vx msg.linear.x;target_Vy msg.linear.y; target_Vz msg.angular.z;// ROS_INFO(x %lf,target_Vx);// ROS_INFO(y %lf,target_Vy);// ROS_INFO(z %lf,target_Vz);//逆时针旋转target_Va target_Vx*K1 target_Vy*0.5 - L*target_Vz;target_Vb -target_Vx*K1 target_Vy*0.5 - L*target_Vz;target_Vc - target_Vy - L*target_Vz; //线速度转换为各轮子转速target_a k_master*target_Va;target_b k_master*target_Vb;target_c k_master*target_Vc;// printf(Va%dfm/s, Vb%dfm/s, Vc%dfm/s \n,target_a,target_b,target_c);/*发送小车数据到下位机*/send_Data();
}
单片机与上位机通信
通信协议的设定共9位
包头0xFF 0xFEa,b,c三个电机各两位高八位/低八位[][] [][], [][] [][], [][] [][]异或校验位 [][]
上位机发送下位机解析将线速度序列化转换为编码器脉冲 上位机(ros)发送线速度——三个轮子线速度——转换到编码器脉冲数(16位数据)——再取高八位低八位 其中用有符号的16位数据来存储三个轮子的编码器的脉冲数数值的正负来表示电机的正转反转但取过高低八位的数据是无符号的。
发送小车数据到下位机函数实现
/*发送小车数据到下位机*/
void send_Data()
{//发送标志位s_buffer[0] 0xFF;s_buffer[1] 0xFE; //A电机速度s_buffer[2] target_a8;//取高八位s_buffer[3] target_a0x00ff;//取低八//B电机速度s_buffer[4] target_b8;s_buffer[5] target_b0x00ff;//C电机速度s_buffer[6] target_c8;s_buffer[7] target_c0x00ff;//异或校验位2-7位校验unsigned char check_num 0;for(int i2;i8;i){check_num ^ s_buffer[i];// printf( %d ,s_buffer[i]);}s_buffer[8]check_num;//发送9位数据my_serial.write(s_buffer,9);
}
如何将一个16位数据取到高低八位
取到高八位方法右移八位8
取到低八位方法与上0x00ff(0x00ff)
举例说明
若上位机ros发送仅有Vx 0.4m/s的x轴线速度Vy 0, 0.
(1).带入前面提到的正运动学公式得到a,b,c轮子的线速度 (2).由轮子线速度得到编码器数值
线速度到轮子编码器数值转速的转化系数为 k 320//0.152下方有解释
相乘即可 (3).分别取到高八位低八位 Na 233,233为正数10进制233转为二进制233 128643281——1110 1001(8位)——转为16位 0000 0000 1110 1001
高八位0000 0000 1110 1001 右移8位 8 得到 0000 0000 ——0 A电机高8位
低八位0000 0000 1110 1001 与上0x00ff(0x00ff) 得到 1110 1001 ——233 A电机低8位 Nb -233,-233为负数需要转为补码的形式,-233的绝对值233转为二进制——0000 0000 1110 1001 ——取反加1 得到1111 1111 0001 0111
高八位1111 1111 0001 0111右移8位 8 得到 1111 1111 ——255 B电机高8位
低八位1111 1111 0001 0111 与上0x00ff(0x00ff) 得到 0001 01111 ——23 B电机低8位 因此得到一个结论是如果有一个电机的的高8位为255或者大于等于128就为负数那么该电机一定是在反转 下位机发送上位机解析将编码器脉冲反序列化转换为线速度
由上述可知上位机向下位机发送了一帧数据
0xFF 0xFE 00 233 255 23 00 00 校验位如果下位机反馈回来的数据同样也是
0xFF 0xFE 00 233 255 23 00 00 那么我们如何将这一帧数据算回机器人底盘的线速度Vx,Vy与角速度Vz呢
解答 首先判断数据是正数还是负数判断方法为看最高位符号位是0还是1是0在则为正数是1则为负数。 对于A电机的两位 00 233高8位的最高位为0则为正数所以还原后的A电机的编码器脉冲数为0*256233*1 233。高8位乘以256低8位乘以1的原因是16位的数据拆分为高8位低8位,高8位基数为2^8256,低8位基数2^01。 对于B电机的两位 255 23高8位1111 1111 最高位为1所以为负数负数以补码的形式存在高8位取反0000 0000 低8位取反加123二进制0001 0111 取反 1110 1000 加1后 1110 1001 转为二进制后为233,因为是负数所以加个负号为-233。 对于c电机为0.
所以反序列化后的编码器数值
Na 233 Nc -233 Nc 0 再得到轮子的线速度当时发给下位机的时候乘以的是那个转化系数这时候接收下位机转换后得除以那个转化系数(320//0.152)。
Va 233 / (320//0.152) 0.35
Vb -233 / (320//0.152) -0.35
Vc 0
带入前面提到的逆运动学公式得到底盘整体的VxVy,Vz线速度 和上位机发送的一摸一样。
上位机解码下位机数据的代码实现
/*右移七位判断第一位符号位0是否为1是的话就是负数*/
if(r_buffer[0]71){
/*补码转原码后减一*/
r_buffer[0]~r_buffer[0];
r_buffer[1]~(r_buffer[1]-0x01);
real_Va -(r_buffer[0]*256r_buffer[1])/ k;// printf(a电机反转);}
else real_Va (r_buffer[0]*256r_buffer[1])/ k; if(r_buffer[2]71){
r_buffer[2]~r_buffer[2];
r_buffer[3]~(r_buffer[3]-0x01);
real_Vb -(r_buffer[2]*256r_buffer[3])/ k;
// printf(b电机反转);
}
else real_Vb (r_buffer[2]*256r_buffer[3])/ k;
if(r_buffer[4]71){
r_buffer[4]~r_buffer[4];
r_buffer[5]~(r_buffer[5]-0x01);
real_Vc -(r_buffer[4]*256r_buffer[5])/ k;
}
else real_Vc (r_buffer[4]*256r_buffer[5])/ k;
为什么说高8位大于等于128就为负数
为什么说高8位大于等于128就为负数因为对于一个int16位数据他的数据总数位2^16 65535,又因为是有符号的所以正数和负数各占一半最大正整数为32767最小负整数为-32767. 32767——二进制0111 1111 1111 1111最高位为0高八位数值为1276432168421 -32767——绝对值的二进制0111 1111 1111 1111——取反1000 0000 0000 0000——加1——1000 0000 0000 0001(最高位符号位为1高八位数值为128) 所以说高八位128是正数与负数的连接处也是电机正转与反转的连接处。
总结公式
若高八位最高位符号位为0即正数时 若高八位最高位符号位为1即负数时 Na1为高8位Na2为低8位~为取反,k值为编码器数值到车轮线速度的转化系数。
线速度到轮子编码器数值转速的转化系数
车轮线速度到编码器数值公式推算
设车轮直径L编码器CPR 为N采样周期为T单周期下编码器反馈计数值为n车轮线速度为v
显然单周期T时间下车轮行进距离s v*T
又s π*L * n/N
∴v [π*L /( T*N ) ]*n
本车中L 0.152m编码器CPR 32000采样周期为10ms 0.01s
∴ v [π*0.152/( 0.01*32000 ) ]*n (π*0.152/320)*n
里程计速度解算 如图
底盘向左偏移了yaw个弧度将Vx和Vy分解到X_dis和y_dis坐标轴上得到 在接收下位机的循环中获取时间
current_time ros::Time::now();//获得当前时间
double dt (current_time - last_time).toSec();//转换成秒
last_time current_time;
计算里程计数据公式累加 在ros中发布里程计数据 //里程计累计x行驶距离X_dist (real_Vx*cos(Yaw)-real_Vy*sin(Yaw))*dt;//里程计累计y行驶距离Y_dist (real_Vx*sin(Yaw)real_Vy*cos(Yaw))*dt; //四元数变量geometry_msgs::Quaternion odom_quat tf::createQuaternionMsgFromYaw(Yaw); //定义里程计对象nav_msgs::Odometry odom;//载入里程计时间戳odom.header.stamp current_time; //里程计的父子坐标系odom.header.frame_id odom;odom.child_frame_id base_link; //里程计位置数据x,y,z,方向odom.pose.pose.position.x X_dist; odom.pose.pose.position.y Y_dist;odom.pose.pose.position.z 0.0;odom.pose.pose.orientation odom_quat; //载入线速度和角速度odom.twist.twist.linear.x real_Vx;odom.twist.twist.linear.y real_Vy;odom.twist.twist.angular.z real_Vz;if(!real_Vx || !real_Vy || !real_Vz){odom.pose.covariance {1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9};odom.twist.covariance {1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e-9};}else{odom.pose.covariance {1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3};odom.twist.covariance {1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0,0, 0, 1e6, 0, 0, 0,0, 0, 0, 1e6, 0, 0,0, 0, 0, 0, 1e6, 0,0, 0, 0, 0, 0, 1e3};}
其中odom_quat指的是根据偏航角yaw来获取四元数数据并放入odom里程计的话题类型中。
公式总结
ROS标准坐标系下全向轮正运动解 ROS标准坐标系下全向轮逆运动解 ROS标准坐标系下全向轮正运动解矩阵形式 轮子二进制编码器数据解码到轮子线速度
若高八位Na1最高位符号位为0即正数时 若高八位Na1最高位符号位为1即负数时,~为取反 K为线速度到轮子编码器数值转速的转化系数
ROS标准坐标系下里程计运动累加解算 上述内容如果有误请及时指正批评谢谢大家
