揭秘全向轮运动学：机动艺术与上下位机通信的智慧桥梁

✨✨ Rqtz 个人主页 : 点击✨✨

🌈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轮的和速度为Vx2+Vy2。在根据夹角60度，可得到a轮的线速度公式(机器人底盘逆时针旋转时)：

其中，由图可知， $\Theta 1$ 为30度， $\Theta 2$ 为60度。 $\omega L$ 由V = $\omega L$ 得到，由于机器人底盘逆时针旋转时，圆周运动在a轮的切向速度与轮子正方向相反,所以为负的 $\omega L$ 。

$Fxy1 = Vx\cos \Theta 1+Vy\sin \Theta 1-\omega L$

$Fxy1 = Vx\sqrt{3}/2+Vy1/2-\omega L$

2. a轮的线速度公式(机器人底盘顺时针旋转时)：

$Fxy1 = Vx\cos \Theta 1+Vy\sin \Theta 1+\omega L$

$Fxy1 = Vx\sqrt{3}/2+Vy1/2+\omega L$

b轮

同理可得

b轮的线速度公式(机器人底盘逆时针旋转时)

$Fxy2 = -Vx\sin \Theta 1+Vy\cos \theta 1-\omega L$

$Fxy2 = -Vx\sqrt{3}/2+Vy1/2-\omega L$

b轮的线速度公式(机器人底盘顺时针旋转时)

$Fxy2 = -Vx\sin \Theta 1+Vy\cos \theta 1+\omega L$

$Fxy2 = -Vx\sqrt{3}/2+Vy1/2+\omega L$

b轮中图的 $\theta 1$ 为60度。

c轮

c轮的线速度公式(机器人底盘逆时针旋转时):

$Fxy3=-Vy-\omega L$

c轮的线速度公式(机器人底盘顺时针旋转时):

$Fxy3=-Vy+\omega L$

全向轮正运动解算公式(顺时针为正方向)

根据ros的标准坐标系（右手定则），在ros中速度大小都是向前，向左为正，逆时针旋转为正，所以结合ros的标准坐标系，为-wL，得出以下公式：

$\begin{bmatrix} Va=Vx\sqrt{3}/2+Vy1/2-\omega L\\ Vb=-Vx\sqrt{3}/2+Vy1/2-\omega L\\ Vc=-Vy-\omega L \end{bmatrix}$

全向轮逆运动解算公式

本质为解三元一次方程组，求解 $Vx,Vy,\omega$

$\begin{bmatrix} Vx = (Va-Vb)/\sqrt{3}\\ Vy=(Va+Vb-2Vc)/3)\\ \omega = -(Va+Vb+Vc)/(3L) \end{bmatrix}$

矩阵形式

$\begin{bmatrix} \sqrt{3}/2 & 1/2& -L\\ -\sqrt{3}/2 & 1/2& -L\\ 0& -1&-L \end{bmatrix}$ $\begin{bmatrix} Vx\\ Vy\\ \omega \end{bmatrix}$ $=$ $\begin{bmatrix} Va\\ Vb\\ Vc \end{bmatrix}$

根据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 0xFE
a,b,c三个电机各两位（高八位/低八位）[][] [][], [][] [][], [][] [][]
异或校验位 [][] $Va=0.4*\sqrt{3}/2+0+0 = 0.4*0.866 \approx 0.35$

上位机发送，下位机解析（将线速度序列化转换为编码器脉冲）

上位机(ros)发送线速度 $Vx,Vy,Vz$ ——>三个轮子线速度 $Va,Vb,Vc$ ——>转换到编码器脉冲数(16位数据) $Na,Nb,Nc$ ——>再取高八位，低八位 $Na1,Na2;Nb1,Nb2;Nc1,Nc2$

其中用有符号的16位数据来存储三个轮子的编码器的脉冲数，数值的正负来表示电机的正转反转，但取过高低八位的数据是无符号的。

发送小车数据到下位机函数实现

/*发送小车数据到下位机*/
void send_Data()
{
    //发送标志位
    s_buffer[0] = 0xFF;
    s_buffer[1] = 0xFE;   
    //A电机速度
    s_buffer[2] = target_a>>8;//取高八位
    s_buffer[3] = target_a&0x00ff;//取低八
    //B电机速度
    s_buffer[4] = target_b>>8;
    s_buffer[5] = target_b&0x00ff;
    //C电机速度
    s_buffer[6] = target_c>>8;
    s_buffer[7] = target_c&0x00ff;
    //异或校验位（2-7位校验）
    unsigned char check_num =0;
    for(int i=2;i<8;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, $\omega$ = 0.

(1).带入前面提到的正运动学公式得到a,b,c轮子的线速度：

$Va=0.4\ast \sqrt{3}/2+0+0 \approx 0.35$

$Vb=-0.4\ast \sqrt{3}/2+0+0 \approx -0.35$

$Vc = 0$

(2).由轮子线速度得到编码器数值

线速度到轮子编码器数值（转速）的转化系数为 k = 320/ $\pi$ /0.152（下方有解释）

相乘即可

$Na=0.35\ast(320/\pi /0.152) = 233$

$Nb=-0.35\ast(320/\pi /0.152) = -233$

$Nc=0$

(3).分别取到高八位，低八位

Na = 233,233为正数，10进制233转为二进制（233 = 128+64+32+8+1）——>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*256+233*1 = 233。（高8位乘以256，低8位乘以1的原因是：16位的数据拆分为高8位，低8位,高8位基数为2^8=256,低8位基数2^0=1。）

对于B电机的两位 255 23，高8位1111 1111 最高位为1，所以为负数，负数以补码的形式存在，高8位取反0000 0000 ，低8位取反加1，23二进制0001 0111 取反 1110 1000 加1后 1110 1001 转为二进制后为233,因为是负数，所以加个负号为-233。

对于c电机为0.

所以反序列化后的编码器数值

Na = 233 Nc = -233 Nc = 0

再得到轮子的线速度，当时发给下位机的时候乘以的是那个转化系数，这时候接收下位机转换后得除以那个转化系数(320/ $\pi$ /0.152)。

Va = 233 / (320/ $\pi$ /0.152) = 0.35

Vb = -233 / (320/ $\pi$ /0.152) = -0.35

Vc = 0

带入前面提到的逆运动学公式得到底盘整体的Vx，Vy,Vz线速度：

$Vx = (Va-Vb)/\sqrt{3} = 0.4$

$Vy=0$

$Vz =0$

和上位机发送的一摸一样。

上位机解码下位机数据的代码实现：

/*右移七位，判断第一位符号位0是否为1，是的话就是负数*/
if(r_buffer[0]>>7==1){
/*补码转原码后减一*/
r_buffer[0]=~r_buffer[0];
r_buffer[1]=~(r_buffer[1]-0x01);
real_Va = -(r_buffer[0]*256+r_buffer[1])/ k;
 // printf("a电机反转");
 }
else real_Va = (r_buffer[0]*256+r_buffer[1])/ k;                        
                            
if(r_buffer[2]>>7==1){
r_buffer[2]=~r_buffer[2];
r_buffer[3]=~(r_buffer[3]-0x01);
real_Vb = -(r_buffer[2]*256+r_buffer[3])/ k;
//  printf("b电机反转");
}  
else real_Vb = (r_buffer[2]*256+r_buffer[3])/ k;
if(r_buffer[4]>>7==1){
r_buffer[4]=~r_buffer[4];
r_buffer[5]=~(r_buffer[5]-0x01);
real_Vc = -(r_buffer[4]*256+r_buffer[5])/ k;
}
else real_Vc = (r_buffer[4]*256+r_buffer[5])/ k;

为什么说高8位大于等于128就为负数，

为什么说高8位大于等于128就为负数，因为对于一个int16位数据，他的数据总数位2^16 = 65535,又因为是有符号的，所以正数和负数各占一半，最大正整数为32767，最小负整数为-32767.

32767——>二进制0111 1111 1111 1111（最高位为0，高八位数值为127（64+32+16+8+4+2+1））

-32767——>绝对值的二进制0111 1111 1111 1111——>取反1000 0000 0000 0000——>加1——>1000 0000 0000 0001(最高位符号位为1，高八位数值为128)

所以说高八位128是正数与负数的连接处，也是电机正转与反转的连接处。

总结公式

若高八位最高位符号位为0，即正数时

$Va = (Na1*256+Na2*1)/K$

若高八位最高位符号位为1，即负数时

$Va =( -[\sim Na1*256+\sim (Na2-0x01)])/K$

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坐标轴上得到

$x = Vx\ast \cos(yaw)-Vy\ast \sin (yaw)$

$y = Vx\ast \sin(yaw)+Vy\ast \cos (yaw)$

在接收下位机的循环中，获取时间

current_time = ros::Time::now();//获得当前时间
double dt = (current_time - last_time).toSec();//转换成秒
last_time = current_time;

计算里程计数据公式（累加）

$x += (Vx\ast \cos(yaw)-Vy\ast \sin (yaw))\ast dt$

$y += (Vx\ast \sin(yaw)+Vy\ast \cos (yaw))\ast dt$

在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标准坐标系下全向轮正运动解：

$\begin{bmatrix} Va=Vx\sqrt{3}/2+Vy1/2-\omega L\\ Vb=-Vx\sqrt{3}/2+Vy1/2-\omega L\\ Vc=-Vy-\omega L \end{bmatrix}$

ROS标准坐标系下全向轮逆运动解：

$\begin{bmatrix} Vx = (Va-Vb)/\sqrt{3}\\ Vy=(Va+Vb-2Vc)/3)\\ \omega = -(Va+Vb+Vc)/(3L) \end{bmatrix}$

ROS标准坐标系下全向轮正运动解（矩阵形式）：

$\begin{bmatrix} \sqrt{3}/2 & 1/2& -L\\ -\sqrt{3}/2 & 1/2& -L\\ 0& -1&-L \end{bmatrix}$ $\begin{bmatrix} Vx\\ Vy\\ \omega \end{bmatrix}$ $=$ $\begin{bmatrix} Va\\ Vb\\ Vc \end{bmatrix}$

轮子二进制编码器数据解码到轮子线速度

若高八位Na1最高位符号位为0，即正数时

$Va = (Na1*256+Na2*1)/K$

若高八位Na1最高位符号位为1，即负数时,~为取反

$Va =( -[\sim Na1*256+\sim (Na2-0x01)])/K$

K为线速度到轮子编码器数值（转速）的转化系数

ROS标准坐标系下里程计运动（累加）解算：

$x += (Vx\ast \cos(yaw)-Vy\ast \sin (yaw))\ast dt$

$y += (Vx\ast \sin(yaw)+Vy\ast \cos (yaw))\ast dt$

上述内容如果有误，请及时指正批评，谢谢大家！