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

🕗 发布于 2024-11-07 20:27 数学建模 c++ 机器学习 ROS ubuntu

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

🌈Qt系列专栏:点击

🎈Qt智能车上位机专栏: 点击🎈

本篇文章介绍的是有关于全向轮运动学分析，单片机与上位机通信C++代码以及ROS里程计解算的内容。

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

举例说明：

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

大纲

全向轮运动学结算主要涉及到：

各个轮子的速度解算
顺逆时针各个轮子和速度公式
速度正交分解示意图

单片机与上位机通信主要涉及到：

如何将解算后的轮子转速转换为编码器数值并按照设定通信协议发送下位机
如何接收下位机反馈编码器数值并转化为轮子转速
涉及上位机发送与接收通信协议以及数值解编码

里程计解算主要是涉及到:

根据轮子转速和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$

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

原文地址：https://blog.csdn.net/m0_75192474/article/details/143567742

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！

上一篇：【测试工具篇一】全网最强保姆级教程抓包工具Fiddler（2）
下一篇：地图框架之mapbox——（二）

【go从零单排】上下文（context）
在 Go 语言中，上下文（context）是一个重要的概念，主要用于管理请求的生命周期、传递取消信号、超时控制以及传递请求范围内的值。上下文通常与并发编程相关，尤其是在处理 HTTP 请求和其他 I/
阅读更多2024-11-15
Ubuntu 20.04 配置开发环境(持续更新)
搜狗输入法不能显示中文。
阅读更多2024-11-15
在ubuntu上安装ubuntu22.04并ros2 humble版本的docker容器记录
"registry-mirrors" : ["https://你的.mirror.swr.myhuaweicloud.com"],4、已经打包了ubuntu22
阅读更多2024-11-15
如何在 Ubuntu 22.04 上安装 ownCloud
ownCloud 是一个开源的个人云存储平台，它允许用户在本地服务器上存储和同步文件，提供了一个类似于 Dropbox 或 Google Drive 的服务，但是更加注重隐私和数据控制。文件存储：用户
阅读更多2024-11-15
idea 删除本地分支后，弹窗 delete tracked brank
在 IntelliJ IDEA 中，删除本地分支后弹出 “Delete tracked branch” 的提示，这表示删除的是一个跟踪分支（tracked branch）。具体来说，当前删除的本地分
阅读更多2024-11-15
「JVM详解」
JVM：全称 Java Virtual Machine，即 Java 虚拟机，一种规范，本身是一个虚拟计算机，直接和操作系统进行交互，与硬件不直接交互，而操作系统可以帮我们完成和硬件进行交互的工作
阅读更多2024-11-15
WPF中Prism框架的简单使用
自动匹配导入4.写Login.XAML代码和LoginViewModel代码LoginViewModel如下：5.创建其他的UserControl 并学好MainWindow.XAML和MainWin
阅读更多2024-11-15
Linux——GPIO输入输出裸机实验
在程序启动时，BSS段会被清零，并且其大小会被计算到程序的总内存占用中，尽管它在磁盘上的表示可能非常小或甚至没有。在程序启动时，BSS段会被清零，并且其大小会被计算到程序的总内存占用中，尽管它在磁盘上
阅读更多2024-11-15
planRAG运行记录
运行planRAG过程记录
阅读更多2024-11-15
vue面试题8|[2024-11-14]
vue面试题
阅读更多2024-11-15

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

大纲

ROS（机器人操作系统）

全向轮运动学分析

a,b,c三轮

a轮

c轮

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

全向轮逆运动解算公式

矩阵形式

单片机与上位机通信

通信协议的设定（共9位）

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

举例说明：

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

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

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

里程计速度解算

公式总结

相关文章