【阿克曼odom里程计解算】：从下位机到上位机，从原理到代码实现

前言

本文将从阿克曼的里程计计算原理出发，讲解下位机STM32如何对电机编码器数据进行整合计算，再通过串口进行上下位机进行数据转发，最终在Linux板端对下位机发送来的数据进行积分计算并最终通过ROS的odometry数据进行全局广播。
本文使用到的环境及其硬件：
- 下位机：STM32F407VGT6
  - FreeRTOS
- 上位机：RDK-X3
  - ubuntu 22.04LTS
  - ROS2 humble
- 直流减速电机（带霍尔编码器）
- 串口USB转type-C线
本文部分代码参考地平线开源的OriginBots机器人代码

1 阿克曼模型

请添加图片描述

阿克曼模型（Ackermann steering model）是一种用于描述汽车等四轮车辆转向运动的模型。在这种模型中，车辆被简化为一个在水平面上运动的刚体，其转向是由前轮的转向角控制的。因此，在阿克曼模型中，车辆的运动只涉及二维平面，即水平面。
在阿克曼模型中，通常假设车辆的运动发生在x轴和y轴构成的平面上，而z轴方向上的速度分量v_z为0，因为车辆在水平面上运动，不会在垂直方向上加速或减速。同样，由于车辆是沿水平面运动，y轴方向上的速度分量v_y也通常为0，除非考虑车辆在弯道中由于侧滑或侧倾引起的侧向速度分量。
因此，在阿克曼模型中，车辆的速度向量通常简化为：
- v = (v_x, 0, 0)
这里，v_x是车辆沿x轴方向的速度分量，即车辆的前进速度。这个速度向量描述了车辆在水平面上的直线运动，而车辆的转向是通过调整前轮的转向角来实现的，这会影响车辆的运动轨迹，但不会改变车辆在x轴方向上的速度分量。

2 里程计Odometry

2-1 介绍

里程计（Odometry）是机器人学中的一个重要概念，它用于估计机器人的位置和方向。在自主移动机器人中，里程计通常是通过集成编码器数据来估计机器人的移动距离和方向变化。编码器是一种传感器，它可以测量轮子或其他移动部件的旋转，并将这些数据转换成距离和角度的数值。
对于一般的阿克曼模型里程计的数据通常以机器人的坐标系中表示，一般包括：
- 机器人的位置（通常是x和y坐标）
- 方向（通常是航向角yaw）。

2-2 核心原理

核心原理就是通过读取阿克曼模型驱动轮电机的实时编码器换算成阿克曼模型的线速度和角速度信息，并通过积分进一步得到里程计数据的x,y,yaw

2-3 解算步骤

下位机编码器读取线速度：通过电机上霍尔编码器的脉冲数，换算出小车当前的线速度信息和角速度信息。
数据发送：下位机将读取到的阿克曼模型的线速度和角速度信息，通过串口转发给上位机
上位机积分线速度结算里程计：通过下位机发生过来的线速度和角速度信息，进行积分结算
上位机ROS广播：将结算的数据类型转换为ROS的里程计信息，并通过话题通讯的方式在ROS全局广播

3 下位机编码器读取线速度

3-1 编码器

编码器是一种传感器，它将机械运动转换为电信号。它捕获的原始数据通常是脉冲信号的数量，这些脉冲与编码器的旋转次数成正比。
增量编码器：它输出两个相位差90度的信号，通常称为A相和B相。当编码器旋转时，A相和B相的信号会以特定的模式变化，产生一系列脉冲。增量编码器通常还有一个索引脉冲，用于确定编码器的起始位置。
在机器人学中，通常使用增量编码器来测量轮子的旋转。编码器每旋转一周会产生一定数量的脉冲，这个数量称为编码器的分辨率。例如，一个1000线的编码器在旋转一周时会产生1000个脉冲。通过计数这些脉冲，可以计算出轮子的旋转次数和角度。

3-2 解算原理1: 单编码器速度解算

我们拿阿克曼模型后轮驱动的其中一个电机A来计算电机A的线速度(m/s):
$MOTOR_A.Encoder = \frac{(Encoder_A - prEncoder_A) \times CONTROL_FREQUENCY \times Wheel\_perimeter}{Encoder\_precision}$
Encoder_A_prEncoder_A_pr 是编码器A的原始脉冲数。（捕获到的原始数据）
CONTROL_FREQUENCY 是控制频率，即每秒更新的次数。
Wheel_perimeter 是车轮的周长，单位:m。
Encoder_precisionEncoder_precision 是编码器的分辨率，即每转产生的脉冲数。
具体结算步骤如下：
1. 我们将编码器A的原始脉冲数乘以控制频率，得到每秒的脉冲数。
2. 然后将每秒的脉冲数乘以车轮的周长，得到每秒车轮移动的距离。
3. 最后，每秒车轮移动的距离除以编码器的分辨率，得到每秒轮子的转速，即车轮线速度。
算法伪代码如下，通过读取原始左右编码器A,B的原始数据，根据上述公式，可以计算出两轮分别的线速度。

void Get_Velocity_Form_Encoder(void)
{
	 
	float Encoder_A_pr,Encoder_B_pr; 
	
	Encoder_A_pr= Read_Encoder(2); 
	Encoder_B_pr=Read_Encoder(3);
	
		MOTOR_A.Encoder=Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  		
}

3-3 解算原理2: AB双编码器线速度合成

正如小结一所属，在阿克曼模型中，v_y和v_z均为0，因此我们只需要综合后轮左右编码器AB的速度进行合成线速度即可
对于双编码器线速度合成，我们可以通过取左右后轮编码器速度的平均值来估算车辆的整体线速度。
- v_x = (MOTOR_A.Encoder + MOTOR_B.Encoder) / 2
代码实现如下:

Send_Data.Sensor_Str.X_speed =((MOTOR_A.Encoder+MOTOR_B.Encoder)/2);

3-4 解算原理3: AB双编码器角速度合成

请添加图片描述

上图为例子,假设左右电机分别在较短的T时间区间内向前移动的位移分别是SA和SB,此时阿克曼模型车应该是存在有θ的偏航角(为了方便看图中把车正过来看了)
那么通过基础的三角关系我们可以得出 $\tan\theta = \frac{SB - SA}{L}$
- 其中L表示两主动轮AB之间的距离，单位为m
然后我们通过数学关系可知，当较短的T时间区间内， $\theta$ 的取值范围很小，当 $\theta$ 的取值范围很小时候，我们有如下数学关系 $\theta\approx\tan\theta$
那么上述式子可以转为： $\theta \approx \frac{SB - SA}{L}$
又因为SA和SB是左右电机分别在较短的T时间区间内向前移动的位移： $\theta \approx \frac{(Vb - Va)*T}{L}$
两边同时除以极短的时间T则有： $w=\frac{\theta}{T} \approx \frac{(Vb - Va)}{L}$
故我们就得到了AB双编码器角速度合成，w：rad/s
代码实现如下:

Send_Data.Sensor_Str.Z_speed = ((MOTOR_B.Encoder-MOTOR_A.Encoder)/Wheel_spacing);

4 数据转发（上）：下位机发送

4-1 下位机串口发送

通过上一小节，我们已经拿到了阿克曼小车的线速度和角速度了

Send_Data.Sensor_Str.X_speed =((MOTOR_A.Encoder+MOTOR_B.Encoder)/2); 
Send_Data.Sensor_Str.Z_speed = ((MOTOR_B.Encoder-MOTOR_A.Encoder)/Wheel_spacing);

然后我们指定数据帧头和数据帧尾，填充发送buffer
- 线速度和角速度乘以1000是我们约定好的
- 把线速度和角速度都拆分为两个八位数据再发送

void data_transition(void)
{
	Send_Data.Sensor_Str.Frame_Header = FRAME_HEADER; //针头
	Send_Data.Sensor_Str.Frame_Tail = FRAME_TAIL;    //帧尾
	//线速度，乘以1000是约定
	Send_Data.Sensor_Str.X_speed = ((MOTOR_A.Encoder+MOTOR_B.Encoder)/2)*1000; 
	//角速度	，乘以1000是约定
	Send_Data.Sensor_Str.Z_speed = ((MOTOR_B.Encoder-MOTOR_A.Encoder)/Wheel_spacing)*1000;


	//填充数据
	Send_Data.buffer[0]=Send_Data.Sensor_Str.Frame_Header; 
    Send_Data.buffer[1]=Flag_Stop; 
    //把线速度和角速度都拆分为两个八位数据再发送
	Send_Data.buffer[2]=Send_Data.Sensor_Str.X_speed >>8; 
	Send_Data.buffer[3]=Send_Data.Sensor_Str.X_speed ;    
	Send_Data.buffer[4]=Send_Data.Sensor_Str.Y_speed>>8;  
	Send_Data.buffer[5]=Send_Data.Sensor_Str.Y_speed;     
	Send_Data.buffer[6]=Send_Data.Sensor_Str.Z_speed >>8; 
	Send_Data.buffer[7]=Send_Data.Sensor_Str.Z_speed ;    
	
	//数据校验位
	Send_Data.buffer[8]=Check_Sum(8,1); 
	//帧尾
	Send_Data.buffer[9]=Send_Data.Sensor_Str.Frame_Tail; 
}

void USART1_SEND(void)
{
  unsigned char i = 0;	
	
	for(i=0; i<10; i++)
	{
		//串口发送
		usart1_send(Send_Data.buffer[i]);
	}	 
}

至此下位机的相关工作就已经结束了，通过编码器的一系列处理和计算我们成功的把阿克曼模型的线速度(m/s)和角速度(rad/s)通过串口发送给上位机了。

5 数据转发（下）：上位机接收

从这里开始就是上位机部分的内容。

5-1 serial::Serial

serial::Serial 是一个用于在C++中处理串行通信的库。它提供了跨平台的串行端口访问功能，可以在Windows、Linux和macOS等操作系统上使用。这个库简化了串行通信的编程过程，使得开发者可以更容易地发送和接收数据。
安装：

sudo apt install ros-humble-serial

下面是一个基础的使用上下位机通讯的基础代码：

#include <iostream>
#include <serial/serial.h>

int main() {
    // 创建一个serial::Serial对象
    serial::Serial my_serial("/dev/ttyUSB0", 9600, serial::Timeout::simpleTimeout(1000));

    // 检查串行端口是否成功打开
    if (my_serial.isOpen()) {
        std::cout << "串行端口已打开" << std::endl;

        // 发送数据
        std::string data = "Hello, Serial Port!";
        size_t bytes_written = my_serial.write(data);

        // 检查发送的数据长度是否正确
        if (bytes_written == data.length()) {
            std::cout << "数据发送成功" << std::endl;
        } else {
            std::cout << "数据发送失败" << std::endl;
        }

        // 关闭串行端口
        my_serial.close();
    } else {
        std::cout << "串行端口打开失败" << std::endl;
    }

    return 0;
}

5-2 数据解算

由于下位机发送线速度和角速度时候进行了先乘以1000在分开高低八位的处理，故上位机在进行解算的时候需要数据解算

float Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
{
    float data_return;
    short transition_16;
    transition_16 = 0;
    transition_16 |=  Data_High<<8;
    transition_16 |=  Data_Low;
    data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001;
    return data_return;
  }

我们将解算完后的数据存储在自定义结构体Vel_Pos_Data中，同时我们设置了两组变量：
- Robot_Pos：存储解算后的阿克曼模型里程计的坐标信息(X,Y, $\theta$ )
- Robot_Vel：得到下位机发送的线速度数据（给X）和角速度数据（给Z）

typedef struct __Vel_Pos_Data_
{
	float X;
	float Y;
	float Z;

} Vel_Pos_Data;

 Vel_Pos_Data Robot_Pos;
 Vel_Pos_Data Robot_Vel;

6 上位机积分线速度结算里程计

6-1 目标和变量定义

在数据解算过后获取完下位机发送来的数据我们有：
- Robot_Vel.X:机器人线速度,单位：m/s
- Robot_Vel.Z:机器人角速度，单位:rad/s
那么现在我们需要通过积分来计算机器人里程计的坐标信息：
- Robot_Pos.X:机器人X坐标
- Robot_Pos.Y:机器人Y坐标
- Robot_Pos.Z:机器人偏航角 $\theta$

6-2 原理

请添加图片描述

如上图所示，在极短的采样时间Sampling_Time(图中为t)，机器人的移动轨迹可以近似估计为一条直线。
那么通过几个关系我们就可以得知短时间内机器人的位移： $\delta x=V*SamplingTime*\cos\theta$ $\delta y=V*SamplingTime*\sin\theta$
那么对于时间不断进行累计我们得到 $\int V \times SamplingTime \times \cos\theta \, dt$ $\int V \times SamplingTime \times \sin\theta \, dt$
$\theta=\int w dt$
那么通过上述积分公式,我们便得到了里程计的坐标信息(X,Y)以及小车的偏航角YAW

6-3 代码实现

积分在代码中实现为采样时间Sampling_Time内的求和进行计算

Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) ) * Sampling_Time;
Robot_Pos.Y+=1.125*(Robot_Vel.X * sin(Robot_Pos.Z)) * Sampling_Time;
Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time;

其中1.03和1.125是矫正因子(通过实际标定进行得出的矫正因子)

7 上位机ROS广播

剩下的事情就非常简单了，对于熟悉ROS的朋友们肯定不陌生,这里稍微提一嘴

7-1 nav_msgs::Odometry

nav_msgs::Odometry 是 ROS (Robot Operating System) 中的一个消息类型，用于表示机器人的位姿（位置和方向）和速度。这个消息类型在导航相关的任务中非常重要，比如在机器人进行自主导航、路径规划、SLAM（Simultaneous Localization and Mapping）等应用中。
nav_msgs::Odometry 消息包含以下主要字段：

std_msgs::Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance

7-2 欧拉角和四元数

四元数（Quaternion）和欧拉角（Euler Angles）是两种用于描述空间中旋转的方式，它们在机器人学、计算机图形学、航空航天等领域都有广泛的应用。

7-2-1 欧拉角

请添加图片描述

欧拉角是一种描述物体在三维空间中旋转的方法，它使用三个角度来表示旋转。这三个角度分别绕三个互相垂直的轴旋转，通常这三个轴被称为旋转轴。在机器人学和计算机图形学中，常用的欧拉角表示方法有：
- Z-Y-X（或 Yaw-Pitch-Roll）: 绕 z 轴旋转（偏航角），然后绕新的 y 轴旋转（俯仰角），最后绕新的 x 轴旋转（滚转角）。
- X-Y-Z: 绕 x 轴旋转，然后绕新的 y 轴旋转，最后绕新的 z 轴旋转。
- Y-X-Z: 绕 y 轴旋转，然后绕新的 x 轴旋转，最后绕新的 z 轴旋转。
欧拉角的优势在于直观，易于理解和实现。然而，欧拉角存在一个问题，即万向节锁（Gimbal Lock），当两个旋转轴重合时，第三个轴的旋转将不再独立，这会导致丢失一个自由度。

7-2-2 四元数

四元数是一种数学表示方法，它使用四个数值来描述一个三维空间的旋转。四元数由一个标量部分和三个向量部分组成，通常表示为 𝑞=𝑤+𝑥𝑖+𝑦𝑗+𝑧𝑘q=w+xi+yj+zk，其中 𝑤w 是标量部分，𝑥,𝑦,𝑧x,y,z 是向量部分，𝑖,𝑗,𝑘i,j,k 是四元数的虚单位。
四元数用于描述旋转的优势在于它没有万向节锁的问题，并且在进行复合旋转时不会产生累积误差。此外，四元数在计算旋转时比欧拉角更稳定，因为四元数的旋转不会产生绕数轴的无限旋转。

7-3 欧拉角转四元数-tf2库

值得一提的是,ROS的odom数据进行存储的是四元数，这里我们需要将偏航角和其他俯仰和翻滚合并为欧拉角RPY=(0,0,yaw)然后换算为四元数
tf2 是 ROS (Robot Operating System) 中的一个库，用于处理机器人中的坐标变换。在机器人学中，坐标变换是核心概念之一，因为机器人通常需要在多个坐标系中操作。tf2 提供了一种机制来跟踪和管理这些坐标系，并允许在不同的坐标系之间进行转换。
在 tf2 库中，你可以使用 tf2::Quaternion 类来表示四元数，并使用 tf2::Quaternion::setEulerYPR 方法来从欧拉角（通常以偏航-俯仰-滚转（Yaw-Pitch-Roll）的顺序给出）设置四元数。
欧拉角转四元数：

#include <tf2/Quaternion.h>

int main() {
    // 假设我们有一个欧拉角（偏航-俯仰-滚转）
    double yaw = 0.0;   // 偏航角
    double pitch = 0.0; // 俯仰角
    double roll = 0.0;  // 滚转角

    // 创建一个四元数对象
    tf2::Quaternion quaternion;

    // 使用欧拉角设置四元数
    quaternion.setEulerYPR(yaw, pitch, roll);

    // 输出四元数的各个分量
    std::cout << "Quaternion (w, x, y, z): " << quaternion.w() << ", " 
              << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << std::endl;

    return 0;
}

四元数转欧拉角：

#include <tf2/Quaternion.h>

int main() {
    // 假设我们有一个四元数
    tf2::Quaternion quaternion(0.0, 0.0, 0.0, 1.0); // 四元数 (w, x, y, z)

    // 创建一个变量来存储欧拉角
    double yaw, pitch, roll;

    // 从四元数获取欧拉角
    quaternion.getEulerYPR(yaw, pitch, roll);

    // 输出欧拉角
    std::cout << "Euler Angles (Yaw, Pitch, Roll): " << yaw << ", " 
              << pitch << ", " << roll << std::endl;

    return 0;
}

7-4 odom数据发布

如下

void origincar_base::Publish_Odom()
{
    tf2::Quaternion q;
    q.setRPY(0,0,Robot_Pos.Z);
    geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);
    
    origincar_msg::msg::Data robotpose;
    origincar_msg::msg::Data robotvel;
    nav_msgs::msg::Odometry odom;

    odom.header.stamp = rclcpp::Node::now();
    odom.header.frame_id = odom_frame_id;
    odom.child_frame_id = robot_frame_id;

    odom.pose.pose.position.x = Robot_Pos.X;
    odom.pose.pose.position.y = Robot_Pos.Y;

    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;


    odom.twist.twist.linear.x =  Robot_Vel.X;
    odom.twist.twist.linear.y =  Robot_Vel.Y;
    odom.twist.twist.angular.z = Robot_Vel.Z; 

    robotpose.x = Robot_Pos.X;
    robotpose.y = Robot_Pos.Y;
    robotpose.z = Robot_Pos.Z;

    robotvel.x = Robot_Vel.X;
    robotvel.y = Robot_Vel.Y;
    robotvel.z = Robot_Vel.Z;

    odom_publisher->publish(odom);
    robotpose_publisher->publish(robotpose);
    robotvel_publisher->publish(robotvel); 
}