四轮独转底盘轮式里程计实现记录

zylyehuo2026-09-04 20:37

博客地址：https://www.cnblogs.com/zylyehuo/

1. 部署环境

1.1 软件与底盘环境

  • 操作系统：Ubuntu 22.04
  • ROS 版本：ROS2 Humble
  • 底盘类型：四轮独立驱动 + 四轮独立转向（4WID + 4WIS）
  • 控制包：ss4d4wctrl_pkg
  • 轮式里程计话题：/wheel_odom
  • 消息类型：nav_msgs/msg/Odometry
  • 里程计核心计算线程：约 100 Hz
  • ROS2 /wheel_odom 发布频率：50 Hz

当前代码中的底盘几何参数为：

cpp 复制代码 
const float CNST_WHEEL_SEPERATION = 0.480f; // 左右轮距 [m]
const float CNST_WHEEL_BASE       = 0.685f; // 前后轴距 [m]
const float CNST_WHEEL_RADIUS     = 0.110f; // 车轮半径 [m]

当前轮式里程计直接使用底层已经换算成 SI 单位的四轮实际线速度，因此里程计核心算法中不再重复把 RPM 转换为 m/s。实际使用的反馈量为：

text 复制代码 
wheel_fl_spd_m_act    左前轮实际线速度 [m/s]
wheel_fr_spd_m_act    右前轮实际线速度 [m/s]
wheel_bl_spd_m_act    左后轮实际线速度 [m/s]
wheel_br_spd_m_act    右后轮实际线速度 [m/s]

dir_fl_pos_m_act      左前轮实际转向角 [deg]
dir_fr_pos_m_act      右前轮实际转向角 [deg]
dir_bl_pos_m_act      左后轮实际转向角 [deg]
dir_br_pos_m_act      右后轮实际转向角 [deg]

转向角进入运动学计算前统一从度转换为弧度：

cpp 复制代码 
fl_steering = dir_fl_pos_m_act * PI / 180.0;
fr_steering = dir_fr_pos_m_act * PI / 180.0;
rl_steering = dir_bl_pos_m_act * PI / 180.0;
rr_steering = dir_br_pos_m_act * PI / 180.0;

1.2 底盘运动模式

当前底盘运动模式定义如下：

mode 含义 里程计模型
1.0 平移 Parallel Model
2.0 双阿克曼 Dual Ackermann Model
3.0 原地自旋 Spinning Model
4.0 急停 不累计里程
5.0 只转向、不行走 不累计里程
7.0 边调整转向边平移 当前按 Parallel Model 计算
其它 非有效运动状态 不累计里程

整体数据流为：

text 复制代码 
四轮实际线速度 + 四轮实际转向角 + 当前运动模式
                    │
                    ▼
          模式对应的车辆运动学模型
                    │
                    ▼
              vx、vy、wz
                    │
                    ▼
              x、y、yaw
                    │
                    ▼
        nav_msgs/msg/Odometry
                    │
                    ▼
               /wheel_odom

1.3 编译与运行

bash 复制代码 
cd ~/ros2workspace
colcon build --packages-select ss4d4wctrl_pkg --symlink-install
source install/setup.bash
ros2 run ss4d4wctrl_pkg ss4d4wctrl_pkg

查看里程计内容：

bash 复制代码 
ros2 topic echo /wheel_odom

查看发布频率：

bash 复制代码 
ros2 topic hz /wheel_odom

录制控制指令与轮式里程计：

bash 复制代码 
ros2 bag record -o calib_bag /PlatformControl /wheel_odom

2. 轮式里程计格式和发布实现

当前 /wheel_odom 使用 ROS2 标准 nav_msgs/msg/Odometry：

text 复制代码 
header.frame_id = "odom"
child_frame_id  = "base_link"

含义为：

  • pose：base_link 相对于局部累计坐标系 odom 的位姿；
  • twist：在 base_link 坐标系表达的车体速度；
  • 当前节点只发布 /wheel_odom，不在里程计发布函数中额外发布 odom -> base_link TF。

二维底盘使用的主要字段为：

text 复制代码 
pose.pose.position.x
pose.pose.position.y
pose.pose.orientation.z
pose.pose.orientation.w

twist.twist.linear.x
twist.twist.linear.y
twist.twist.angular.z

z、roll、pitch 以及对应速度均置零。

当前实现没有主动填写 pose.covariance 与 twist.covariance。如果后续把 /wheel_odom 输入 robot_localization，建议通过重复实车实验估计对应 covariance。

2.2 里程计共享数据格式

里程计线程把结果写入 sys_proc_t：

cpp 复制代码 
    // 轮式里程计 (odometry_proc 线程更新, 供节点发布话题)
    // pose 位于 odom 坐标系；twist 为 base_link 坐标系下的车体速度。
    double odom_x;    // X位置(m)
    double odom_y;    // Y位置(m)
    double odom_th;   // 航向角(rad)
    double odom_vx;   // 车体X方向线速度(m/s)
    double odom_vy;   // 车体Y方向线速度(m/s)
    double odom_wz;   // 绕Z轴角速度(rad/s)

其中：

\(x,y,\\theta)=(odom\\_x,odom\\_y,odom\\_th) \\

表示累计位姿；

\(v_x,v_y,\\omega_z)=(odom\\_vx,odom\\_vy,odom\\_wz) \\

表示 base_link 下的当前车体速度。

2.3 创建 /wheel_odom publisher

cpp 复制代码 
publisher_odom = this->create_publisher<nav_msgs::msg::Odometry>(
    "/wheel_odom", 10);

使用 20 ms 定时器以 50 Hz 发布：

cpp 复制代码 
timer_odom = this->create_wall_timer(
    20ms,
    std::bind(&ss4d4wCtrl::publish_odom_message, this));

2.4 ROS2 Odometry 消息组包

完整发布函数如下：

cpp 复制代码 
  void publish_odom_message()
  {
    // 先取本周期共享数据的局部副本，避免后续组包过程中重复读取。
    const double odom_x  = sys.odom_x;
    const double odom_y  = sys.odom_y;
    const double odom_th = sys.odom_th;
    const double odom_vx = sys.odom_vx;
    const double odom_vy = sys.odom_vy;
    const double odom_wz = sys.odom_wz;

    auto odom_msg = nav_msgs::msg::Odometry();

    // 标准里程计坐标系：pose 属于 odom，twist 属于 child_frame_id(base_link)。
    odom_msg.header.stamp = this->now();
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "base_link";

    // 位姿：发布 Ranger-style odometry_proc 积分后的累计位置。
    odom_msg.pose.pose.position.x = odom_x;
    odom_msg.pose.pose.position.y = odom_y;
    odom_msg.pose.pose.position.z = 0.0;

    // 姿态：Ranger-style odometry_proc 的 yaw -> quaternion。
    odom_msg.pose.pose.orientation.x = 0.0;
    odom_msg.pose.pose.orientation.y = 0.0;
    odom_msg.pose.pose.orientation.z = std::sin(odom_th / 2.0);
    odom_msg.pose.pose.orientation.w = std::cos(odom_th / 2.0);

    // 速度：直接发布里程计算法内部已经解算出的车体速度，不新增速度估计算法。
    // nav_msgs/Odometry 约定 twist 位于 child_frame_id，因此这里直接使用 base_link 下的 vx/vy/wz。
    odom_msg.twist.twist.linear.x = odom_vx;
    odom_msg.twist.twist.linear.y = odom_vy;
    odom_msg.twist.twist.linear.z = 0.0;
    odom_msg.twist.twist.angular.x = 0.0;
    odom_msg.twist.twist.angular.y = 0.0;
    odom_msg.twist.twist.angular.z = odom_wz;

    publisher_odom->publish(odom_msg);
  }

二维 yaw 到四元数采用：

\q_z=\\sin\\left(\\frac{\\theta}{2}\\right),\\qquad q_w=\\cos\\left(\\frac{\\theta}{2}\\right) \\

对应代码：

cpp 复制代码 
odom_msg.pose.pose.orientation.z = std::sin(odom_th / 2.0);
odom_msg.pose.pose.orientation.w = std::cos(odom_th / 2.0);

3. 具体轮式里程计算法

3.1 总体流程

轮式里程计核心运行在线程 odometry_proc() 中。线程每约 10 ms 执行一次：

cpp 复制代码 
while (1)
{
    usleep(10000);
    ...
}

每个周期依次执行：

  1. 读取四轮实际线速度；
  2. 读取四轮实际转向角；
  3. 转向角由 degree 转为 radian；
  4. 获取当前底盘 nav_mode；
  5. 使用单调时钟计算真实 dt；
  6. 根据 mode 选择运动学模型；
  7. 得到 base_link 下的 linear_x、linear_y、angular；
  8. 积分得到 odom 下的 x、y、th；
  9. 将结果写入 sys.odom_*；
  10. ROS2 节点以 50 Hz 组装并发布 /wheel_odom。

反馈读取代码：

cpp 复制代码 
fl_speed = wheel_fl_spd_m_act;
fr_speed = wheel_fr_spd_m_act;
rl_speed = wheel_bl_spd_m_act;
rr_speed = wheel_br_spd_m_act;

fl_steering = dir_fl_pos_m_act * PI / 180.0;
fr_steering = dir_fr_pos_m_act * PI / 180.0;
rl_steering = dir_bl_pos_m_act * PI / 180.0;
rr_steering = dir_br_pos_m_act * PI / 180.0;

3.2 时间步长与异常保护

使用 CLOCK_MONOTONIC 获得真实时间间隔：

cpp 复制代码 
clock_gettime(CLOCK_MONOTONIC, &current_time);
const double dt = (current_time.tv_sec - last_time.tv_sec) +
                  (current_time.tv_nsec - last_time.tv_nsec) / 1e9;
last_time = current_time;

正常运行时：

\dt\\approx0.01\\;s \\

对于明显异常的周期：

cpp 复制代码 
if (dt < 0.0001 || dt > 0.2)
{
    sys.odom_vx = 0.0;
    sys.odom_vy = 0.0;
    sys.odom_wz = 0.0;
    continue;
}

这样在系统调度暂停或线程异常阻塞后，不会拿上一周期速度乘一个异常大的 dt 继续积分。

3.3 mode 2.0：Dual Ackermann 双阿克曼模型

3.3.1 四轮速度融合

四个驱动轮实际速度记为：

\v_{FL},v_{FR},v_{RL},v_{RR} \\

整车等效线速度使用四轮算术平均：

\v=\\frac{v_{FL}+v_{FR}+v_{RL}+v_{RR}}{4} \\

对应代码：

cpp 复制代码 
double v = (static_cast<double>(fl_speed) + static_cast<double>(fr_speed) +
            static_cast<double>(rl_speed) + static_cast<double>(rr_speed)) / 4.0;

该值作为车辆中心等效线速度使用。

3.3.2 判断左右转向

双阿克曼前后轮反相，因此使用：

\\\delta_{hint} = \\frac{\\delta_{FL}+\\delta_{FR}-\\delta_{RL}-\\delta_{RR}}{4} \\

判断当前左右转方向：

cpp 复制代码 
const double turn_hint =
    (static_cast<double>(fl_steering) + static_cast<double>(fr_steering)
   - static_cast<double>(rl_steering) - static_cast<double>(rr_steering)) / 4.0;

左转时左侧为内轮，并把后轮角度反号后与前轮统一：

cpp 复制代码 
inner_angle = 0.5 * (
    static_cast<double>(fl_steering)
  - static_cast<double>(rl_steering));

右转时：

cpp 复制代码 
inner_angle = 0.5 * (
    static_cast<double>(fr_steering)
  - static_cast<double>(rr_steering));

3.3.3 内侧轮转角转换为车辆中心等效转角

设内轮转角为 \(\phi_i\)，车辆中心等效转角为 \(\phi\)，则：

\\\phi= \\operatorname{atan2} \\left( L\\sin\\phi_i, L\\cos\\phi_i+W\\sin\\phi_i \\right) \\

其中：

  • \(L\)：CNST_WHEEL_BASE；
  • \(W\)：STEERING_TRACK。

代码：

cpp 复制代码 
auto convert_inner_to_central = [](double angle) -> double {
    const double phi_i = std::fabs(angle);
    if (phi_i < 1e-12)
        return 0.0;

    const double numerator = CNST_WHEEL_BASE * std::sin(phi_i);
    const double denominator = CNST_WHEEL_BASE * std::cos(phi_i)
                             + STEERING_TRACK * std::sin(phi_i);
    double phi = std::atan2(numerator, denominator);
    return (angle >= 0.0) ? phi : -phi;
};

3.3.4 双阿克曼车体 twist

中心等效转角得到后：

\v_x=v\\cos\\phi \\

\v_y=0 \\

\\\omega_z=\\frac{2v\\sin\\phi}{L} \\

代码：

cpp 复制代码 
linear_x = v * std::cos(phi);
linear_y = 0.0;
angular  = 2.0 * v * std::sin(phi) / CNST_WHEEL_BASE;

这三个量直接作为 /wheel_odom.twist 的来源。

3.3.5 RK4 位姿积分

双阿克曼在 odom 中的连续运动模型为：

\\\dot{x}=v\\cos\\phi\\cos\\theta \\

\\\dot{y}=v\\cos\\phi\\sin\\theta \\

\\\dot{\\theta}=\\frac{2v\\sin\\phi}{L} \\

当前实现将一个真实 dt 再划分为 10 个子步，并采用四阶 Runge-Kutta：

cpp 复制代码 
integrate_dual_ackermann_rk4(v, phi, dt, x, y, th);

对应完整函数：

cpp 复制代码 
auto integrate_dual_ackermann_rk4 = [](double v, double phi, double dt,
                                        double &px, double &py, double &yaw) {
    const int sub_steps = 10;
    const double h = dt / static_cast<double>(sub_steps);
    const double body_vx = v * std::cos(phi);
    const double wz = 2.0 * v * std::sin(phi) / CNST_WHEEL_BASE;

    for (int i = 0; i < sub_steps; ++i)
    {
        auto deriv = [body_vx, wz](double theta,
                                   double &dx, double &dy, double &dtheta) {
            dx = body_vx * std::cos(theta);
            dy = body_vx * std::sin(theta);
            dtheta = wz;
        };

        double k1x, k1y, k1t;
        double k2x, k2y, k2t;
        double k3x, k3y, k3t;
        double k4x, k4y, k4t;

        deriv(yaw,             k1x, k1y, k1t);
        deriv(yaw + 0.5*h*k1t, k2x, k2y, k2t);
        deriv(yaw + 0.5*h*k2t, k3x, k3y, k3t);
        deriv(yaw + h*k3t,     k4x, k4y, k4t);

        px  += h * (k1x + 2.0*k2x + 2.0*k3x + k4x) / 6.0;
        py  += h * (k1y + 2.0*k2y + 2.0*k3y + k4y) / 6.0;
        yaw += h * (k1t + 2.0*k2t + 2.0*k3t + k4t) / 6.0;
    }
};

3.4 mode 1.0 / 7.0：Parallel 平移模型

3.4.1 等效线速度

同样使用：

\v=\\frac{v_{FL}+v_{FR}+v_{RL}+v_{RR}}{4} \\

3.4.2 四轮实际转向角圆均值

转向角属于周期变量，因此使用圆均值，而不是普通算术平均：

\\\phi= \\operatorname{atan2} \\left( \\sum_i\\sin\\delta_i, \\sum_i\\cos\\delta_i \\right) \\

代码：

cpp 复制代码 
auto circular_mean4 = [](double a0, double a1, double a2, double a3) -> double {
    const double s = std::sin(a0) + std::sin(a1) +
                     std::sin(a2) + std::sin(a3);
    const double c = std::cos(a0) + std::cos(a1) +
                     std::cos(a2) + std::cos(a3);
    if (std::fabs(s) < 1e-12 && std::fabs(c) < 1e-12)
        return 0.0;
    return std::atan2(s, c);
};

\v_x=v\\cos\\phi \\

\v_y=v\\sin\\phi \\

\\\omega_z=0 \\

对应：

cpp 复制代码 
linear_x = v * std::cos(phi);
linear_y = v * std::sin(phi);
angular  = 0.0;

当前 yaw 为 \(\theta\)，车体系速度通过二维旋转变换到 odom：

\\\dot{x}=v_x\\cos\\theta-v_y\\sin\\theta \\

\\\dot{y}=v_x\\sin\\theta+v_y\\cos\\theta \\

代码：

cpp 复制代码 
x += (linear_x * std::cos(th) - linear_y * std::sin(th)) * dt;
y += (linear_x * std::sin(th) + linear_y * std::cos(th)) * dt;

该分支不更新 yaw。

3.5 mode 3.0：Spinning 原地自旋模型

自旋模式下：

\v_x=0,\\qquad v_y=0 \\

需要利用四个轮子的实际速度与转向角恢复车体角速度 \(\omega_z\)。

3.5.1 四轮位置

定义：

\a=\\frac{L}{2},\\qquad b=\\frac{W}{2} \\

轮子相对底盘几何中心的位置为：

\FL=(a,b),\\quad FR=(a,-b),\\quad RL=(-a,b),\\quad RR=(-a,-b) \\

纯旋转刚体第 \(i\) 个轮子中心的速度满足：

\\\mathbf{v}_i = \\omega_z \\begin{bmatrix} -y_i\\\\ x_i \\end{bmatrix} \\

因此：

\\\omega_i= \\frac{-y_i v_{ix}+x_i v_{iy}}{x_i\^2+y_i\^2} \\

3.5.2 将每个轮子的滚动速度恢复为二维速度向量

对于第 \(i\) 个轮：

\v_{ix}=v_i\\cos\\delta_i \\

\v_{iy}=v_i\\sin\\delta_i \\

代码：

cpp 复制代码 
const double vfl_x = fl_speed * std::cos(fl_steering);
const double vfl_y = fl_speed * std::sin(fl_steering);
const double vfr_x = fr_speed * std::cos(fr_steering);
const double vfr_y = fr_speed * std::sin(fr_steering);
const double vrl_x = rl_speed * std::cos(rl_steering);
const double vrl_y = rl_speed * std::sin(rl_steering);
const double vrr_x = rr_speed * std::cos(rr_steering);
const double vrr_y = rr_speed * std::sin(rr_steering);

3.5.3 四轮分别估计角速度

令：

\r\^2=a\^2+b\^2 \\

对应四轮：

cpp 复制代码 
const double w_fl = (-b * vfl_x + a * vfl_y) / r2;
const double w_fr = ( b * vfr_x + a * vfr_y) / r2;
const double w_rl = (-b * vrl_x - a * vrl_y) / r2;
const double w_rr = ( b * vrr_x - a * vrr_y) / r2;

最后取四轮平均：

\\\omega_z= \\frac{\\omega_{FL}+\\omega_{FR}+\\omega_{RL}+\\omega_{RR}}{4} \\

cpp 复制代码 
angular = (w_fl + w_fr + w_rl + w_rr) / 4.0;

然后只积分 yaw：

cpp 复制代码 
th += angular * dt;

3.6 mode 4.0 / 5.0 与其它非运动状态

急停、只转向不走以及其它非有效运动模式均不应产生车体里程：

cpp 复制代码 
linear_x = 0.0;
linear_y = 0.0;
angular  = 0.0;

尤其 mode 5.0 中转向执行器虽然可能在运动，但车辆本体没有驱动位移，因此不能把转向机构自身角度变化错误积分为底盘运动。

3.7 yaw 归一化

每个周期把航向角限制在：

\\[-\\pi,\\pi \]

cpp 复制代码 
th = wrap_to_pi(th);

对应函数：

cpp 复制代码 
auto wrap_to_pi = [PI](double a) -> double {
    while (a > PI)  a -= 2.0 * PI;
    while (a < -PI) a += 2.0 * PI;
    return a;
};

3.8 将结果共享给 ROS2 发布线程

cpp 复制代码 
sys.odom_x  = x;
sys.odom_y  = y;
sys.odom_th = th;
sys.odom_vx = linear_x;
sys.odom_vy = linear_y;
sys.odom_wz = angular;

因此当前 /wheel_odom.pose 与 /wheel_odom.twist 来自同一套运动学计算和同一组四轮实际反馈。

4. 完整轮式里程计相关源代码

本节粘贴当前工程中完整的轮式里程计相关代码。CAN 驱动、HMI、灯光、急停、伺服器状态机等与轮式里程计算法本身无关的底盘代码不重复粘贴。

4.1 sys_dev_ctrl.cpp：里程计变量和底盘参数

cpp 复制代码 
//************************  ODOMETRY INFO Varibles. ***********************************
#if 1

static pthread_mutex_t mutex_odometry_lock;
static pthread_t id_thread_odometry = 0;

struct timespec odometry_update_time;
struct timespec last_time, current_time;

float fl_speed = 0;
float fr_speed = 0;
float rl_speed = 0;
float rr_speed = 0;
float fl_steering = 0;
float fr_steering = 0;
float rl_steering = 0;
float rr_steering = 0;

float front_steering = 0;
float rear_steering = 0;
double front_linear_speed = 0;
double rear_linear_speed = 0;

double linear_x = 0;
double linear_y = 0;
double angular = 0;

double x = 0;
double y = 0;
double th = 0;

const float CNST_WHEEL_SEPERATION = 0.480f;   // 左右车轮间距 左前轮与右前轮间的距离
const float CNST_WHEEL_BASE = 0.685f; // 前后车轮间距 左前轮与左后轮间的距离
const float CNST_WHEEL_RADIUS = 0.110f; // 车轮半径
const float CNST_WHEEL_STEERING_Y_OFFSET = 0.0f;
const float STEERING_TRACK = CNST_WHEEL_SEPERATION;


#endif

4.2 sys_dev_ctrl.hpp：里程计共享数据成员

cpp 复制代码 
    // 轮式里程计 (odometry_proc 线程更新, 供节点发布话题)
    // pose 位于 odom 坐标系；twist 为 base_link 坐标系下的车体速度。
    double odom_x;    // X位置(m)
    double odom_y;    // Y位置(m)
    double odom_th;   // 航向角(rad)
    double odom_vx;   // 车体X方向线速度(m/s)
    double odom_vy;   // 车体Y方向线速度(m/s)
    double odom_wz;   // 绕Z轴角速度(rad/s)

4.3 sys_dev_ctrl.cpp：完整里程计计算线程

cpp 复制代码 
void *odometry_proc(void * mode)
{
    const double PI = 3.14159265358979323846;
    const double SPEED_EPS = 1e-4;
    const double STEER_EPS = 1e-4;

    // 当前轮式里程计首先根据底盘运动模式重建聚合后的
    // linear / angular / steering 运动状态，再分别使用
    // Dual-Ackermann / Parallel / Spinning 三种模型积分。
    // 聚合运动状态由四轮实际速度和四轮实际转向角共同计算。

    auto wrap_to_pi = [PI](double a) -> double {
        while (a > PI)  a -= 2.0 * PI;
        while (a < -PI) a += 2.0 * PI;
        return a;
    };

    auto circular_mean4 = [](double a0, double a1, double a2, double a3) -> double {
        const double s = std::sin(a0) + std::sin(a1) + std::sin(a2) + std::sin(a3);
        const double c = std::cos(a0) + std::cos(a1) + std::cos(a2) + std::cos(a3);
        if (std::fabs(s) < 1e-12 && std::fabs(c) < 1e-12)
            return 0.0;
        return std::atan2(s, c);
    };

    // 与 Ranger ROS2 humble 的 ConvertInnerAngleToCentral() 同形：
    // 输入双阿克曼内侧轮转角，转换为车轴中心等效转角。
    auto convert_inner_to_central = [](double angle) -> double {
        const double phi_i = std::fabs(angle);
        if (phi_i < 1e-12)
            return 0.0;

        const double numerator = CNST_WHEEL_BASE * std::sin(phi_i);
        const double denominator = CNST_WHEEL_BASE * std::cos(phi_i)
                                 + STEERING_TRACK * std::sin(phi_i);
        double phi = std::atan2(numerator, denominator);
        return (angle >= 0.0) ? phi : -phi;
    };

    // Ranger 使用 runge_kutta4 并以 dt/10 为子步长积分 Dual-Ackermann。
    // 这里保留同样的积分思想，不引入额外 Boost 依赖。
    auto integrate_dual_ackermann_rk4 = [](double v, double phi, double dt,
                                            double &px, double &py, double &yaw) {
        const int sub_steps = 10;
        const double h = dt / static_cast<double>(sub_steps);
        const double body_vx = v * std::cos(phi);
        const double wz = 2.0 * v * std::sin(phi) / CNST_WHEEL_BASE;

        for (int i = 0; i < sub_steps; ++i)
        {
            auto deriv = [body_vx, wz](double theta,
                                       double &dx, double &dy, double &dtheta) {
                dx = body_vx * std::cos(theta);
                dy = body_vx * std::sin(theta);
                dtheta = wz;
            };

            double k1x, k1y, k1t;
            double k2x, k2y, k2t;
            double k3x, k3y, k3t;
            double k4x, k4y, k4t;

            deriv(yaw,                 k1x, k1y, k1t);
            deriv(yaw + 0.5*h*k1t,     k2x, k2y, k2t);
            deriv(yaw + 0.5*h*k2t,     k3x, k3y, k3t);
            deriv(yaw + h*k3t,         k4x, k4y, k4t);

            px  += h * (k1x + 2.0*k2x + 2.0*k3x + k4x) / 6.0;
            py  += h * (k1y + 2.0*k2y + 2.0*k3y + k4y) / 6.0;
            yaw += h * (k1t + 2.0*k2t + 2.0*k3t + k4t) / 6.0;
        }
    };

    printf("odometry_proc (Ranger-style mode kinematics)!!\r\n");

    // 在线程内部初始化时间基准，避免 pthread_create 与 last_time 初始化的竞态。
    clock_gettime(CLOCK_MONOTONIC, &last_time);

    while (1)
    {
        usleep(10000);

        // 读取四轮实际反馈。现有底盘参数、CAN 换算和传感器方向全部保持不变。
        fl_speed = wheel_fl_spd_m_act;
        fr_speed = wheel_fr_spd_m_act;
        rl_speed = wheel_bl_spd_m_act;
        rr_speed = wheel_br_spd_m_act;

        fl_steering = dir_fl_pos_m_act * PI / 180.0;
        fr_steering = dir_fr_pos_m_act * PI / 180.0;
        rl_steering = dir_bl_pos_m_act * PI / 180.0;
        rr_steering = dir_br_pos_m_act * PI / 180.0;

        const float nav_mode_val = *(float*)mode;

        clock_gettime(CLOCK_MONOTONIC, &current_time);
        const double dt = (current_time.tv_sec - last_time.tv_sec) +
                          (current_time.tv_nsec - last_time.tv_nsec) / 1e9;
        last_time = current_time;

        // 正常 100 Hz 线程 dt 约 0.01 s；异常长时间调度暂停时不把上一周期速度外推。
        if (dt < 0.0001 || dt > 0.2)
        {
            sys.odom_vx = 0.0;
            sys.odom_vy = 0.0;
            sys.odom_wz = 0.0;
            continue;
        }

        linear_x = 0.0;
        linear_y = 0.0;
        angular  = 0.0;

        // ------------------------------------------------------------------
        // mode 2.0: 双阿克曼（对应 Ranger MOTION_MODE_DUAL_ACKERMAN）
        // ------------------------------------------------------------------
        if ((nav_mode_val > 1.9f) && (nav_mode_val < 2.1f))
        {
            // Ranger 的 odom 输入是底盘聚合 linear_velocity。
            // 本平台从四轮实际滚动线速度恢复：四轮同号时算术平均能显著降低单轮噪声。
            double v = (static_cast<double>(fl_speed) + static_cast<double>(fr_speed) +
                        static_cast<double>(rl_speed) + static_cast<double>(rr_speed)) / 4.0;
            if (std::fabs(v) < SPEED_EPS)
                v = 0.0;

            // 双阿克曼中前后轮反相：把后轮角度取反后，与前轮统一为"前轴等效转角"。
            // 通过平均转向符号判断左右转，再取对应内侧轮，最后前/后内侧轮平均抑制编码器噪声。
            const double turn_hint = (static_cast<double>(fl_steering) + static_cast<double>(fr_steering)
                                    - static_cast<double>(rl_steering) - static_cast<double>(rr_steering)) / 4.0;

            double inner_angle = 0.0;
            if (turn_hint > STEER_EPS)
            {
                // 左转：左侧为内轮；后左轮角度反号后与前左轮同号。
                inner_angle = 0.5 * (static_cast<double>(fl_steering)
                                   - static_cast<double>(rl_steering));
            }
            else if (turn_hint < -STEER_EPS)
            {
                // 右转：右侧为内轮。
                inner_angle = 0.5 * (static_cast<double>(fr_steering)
                                   - static_cast<double>(rr_steering));
            }

            const double phi = convert_inner_to_central(inner_angle);

            // 与 Ranger DualAckermanModel 一致：
            // xdot = v*cos(phi)*cos(theta)
            // ydot = v*cos(phi)*sin(theta)
            // thetadot = 2*v*sin(phi)/L
            linear_x = v * std::cos(phi);  // base_link 下实际用于积分的纵向速度
            linear_y = 0.0;
            angular  = 2.0 * v * std::sin(phi) / CNST_WHEEL_BASE;

            integrate_dual_ackermann_rk4(v, phi, dt, x, y, th);
        }
        // ------------------------------------------------------------------
        // mode 1.0 / 7.0: 同相平移/边转边平移（Ranger ParallelModel）
        // mode 5.0 是"只转不走"，单独在静止分支处理。
        // ------------------------------------------------------------------
        else if (((nav_mode_val > 0.9f) && (nav_mode_val < 1.1f)) ||
                 ((nav_mode_val > 6.9f) && (nav_mode_val < 7.1f)))
        {
            double v = (static_cast<double>(fl_speed) + static_cast<double>(fr_speed) +
                        static_cast<double>(rl_speed) + static_cast<double>(rr_speed)) / 4.0;
            if (std::fabs(v) < SPEED_EPS)
                v = 0.0;

            // 四轮同相时，实际转向角用圆均值，避免简单平均在角度边界附近失效。
            const double phi = circular_mean4(fl_steering, fr_steering,
                                              rl_steering, rr_steering);

            // Ranger ParallelModel：theta 不变，速度方向为 theta + phi。
            linear_x = v * std::cos(phi);
            linear_y = v * std::sin(phi);
            angular  = 0.0;

            x += (linear_x * std::cos(th) - linear_y * std::sin(th)) * dt;
            y += (linear_x * std::sin(th) + linear_y * std::cos(th)) * dt;
        }
        // ------------------------------------------------------------------
        // mode 3.0: 原地自旋（对应 Ranger SpinningModel）
        // ------------------------------------------------------------------
        else if ((nav_mode_val > 2.9f) && (nav_mode_val < 3.1f))
        {
            linear_x = 0.0;
            linear_y = 0.0;

            // Ranger 直接使用底盘 motion_state.angular_velocity。
            // 本平台没有该聚合量，因此利用四个轮子的"实际速度向量在绕中心切向上的投影"
            // 重建 angular_velocity。四轮共同估计并取平均，
            // 以降低单轮速度反馈波动对整车角速度估计的影响。
            const double a = CNST_WHEEL_BASE / 2.0;
            const double b = STEERING_TRACK / 2.0;
            const double r2 = a*a + b*b;

            const double vfl_x = fl_speed * std::cos(fl_steering);
            const double vfl_y = fl_speed * std::sin(fl_steering);
            const double vfr_x = fr_speed * std::cos(fr_steering);
            const double vfr_y = fr_speed * std::sin(fr_steering);
            const double vrl_x = rl_speed * std::cos(rl_steering);
            const double vrl_y = rl_speed * std::sin(rl_steering);
            const double vrr_x = rr_speed * std::cos(rr_steering);
            const double vrr_y = rr_speed * std::sin(rr_steering);

            if (r2 > 1e-12)
            {
                const double w_fl = (-b * vfl_x + a * vfl_y) / r2;
                const double w_fr = ( b * vfr_x + a * vfr_y) / r2;
                const double w_rl = (-b * vrl_x - a * vrl_y) / r2;
                const double w_rr = ( b * vrr_x - a * vrr_y) / r2;
                angular = (w_fl + w_fr + w_rl + w_rr) / 4.0;
            }

            if (std::fabs(angular) < SPEED_EPS)
                angular = 0.0;

            // Ranger SpinningModel：平移为 0，只积分 yaw。
            th += angular * dt;
        }
        // ------------------------------------------------------------------
        // mode 0 / 4 / 5 / 6 / 其它：不产生轮式里程计位移
        // 4.0 急停；5.0 只转向不走；6.0 原系统特殊暂停/清零逻辑。
        // ------------------------------------------------------------------
        else
        {
            linear_x = 0.0;
            linear_y = 0.0;
            angular  = 0.0;
        }

        th = wrap_to_pi(th);

        // 共享给 ROS2 /wheel_odom 发布线程。
        // pose 在 odom 坐标系；twist 为 base_link 坐标系速度。
        sys.odom_x  = x;
        sys.odom_y  = y;
        sys.odom_th = th;
        sys.odom_vx = linear_x;
        sys.odom_vy = linear_y;
        sys.odom_wz = angular;
    }

    return 0;
}

int SYS_DEV_CTRL_PROC::init_odometry_module()
{
    pthread_mutex_init(&mutex_odometry_lock, NULL);

    int ret = pthread_create(&id_thread_odometry, NULL, odometry_proc, &nav_mode);
    if (0 > ret)
    {
        return -1;
    }
    clock_gettime(CLOCK_MONOTONIC, &last_time);

    printf("load odometry module successfully.\r\n");
    return 0;
}

4.4 ss4d4wctrl.cpp：ROS2 发布所需声明与初始化

需要的头文件和命名空间：

cpp 复制代码 
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include <cmath>

using namespace std::chrono_literals;

底盘共享状态由控制层提供：

cpp 复制代码 
extern sys_proc_t sys;

在 ss4d4wCtrl 类中声明：

cpp 复制代码 
rclcpp::TimerBase::SharedPtr timer_odom;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr publisher_odom;

构造函数中创建 publisher 和 50 Hz 定时器：

cpp 复制代码 
// ss4d4wctrl 构造函数中的 /wheel_odom 发布器与 50 Hz 定时器
publisher_odom = this->create_publisher<nav_msgs::msg::Odometry>("/wheel_odom", 10);

timer_odom = this->create_wall_timer(
    20ms, std::bind(&ss4d4wCtrl::publish_odom_message, this));

4.5 ss4d4wctrl.cpp：完整 /wheel_odom 发布函数

cpp 复制代码 
  void publish_odom_message()
  {
    // 先取本周期共享数据的局部副本，避免后续组包过程中重复读取。
    const double odom_x  = sys.odom_x;
    const double odom_y  = sys.odom_y;
    const double odom_th = sys.odom_th;
    const double odom_vx = sys.odom_vx;
    const double odom_vy = sys.odom_vy;
    const double odom_wz = sys.odom_wz;

    auto odom_msg = nav_msgs::msg::Odometry();

    // 标准里程计坐标系：pose 属于 odom，twist 属于 child_frame_id(base_link)。
    odom_msg.header.stamp = this->now();
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "base_link";

    // 位姿：发布 Ranger-style odometry_proc 积分后的累计位置。
    odom_msg.pose.pose.position.x = odom_x;
    odom_msg.pose.pose.position.y = odom_y;
    odom_msg.pose.pose.position.z = 0.0;

    // 姿态：Ranger-style odometry_proc 的 yaw -> quaternion。
    odom_msg.pose.pose.orientation.x = 0.0;
    odom_msg.pose.pose.orientation.y = 0.0;
    odom_msg.pose.pose.orientation.z = std::sin(odom_th / 2.0);
    odom_msg.pose.pose.orientation.w = std::cos(odom_th / 2.0);

    // 速度：直接发布里程计算法内部已经解算出的车体速度，不新增速度估计算法。
    // nav_msgs/Odometry 约定 twist 位于 child_frame_id，因此这里直接使用 base_link 下的 vx/vy/wz。
    odom_msg.twist.twist.linear.x = odom_vx;
    odom_msg.twist.twist.linear.y = odom_vy;
    odom_msg.twist.twist.linear.z = 0.0;
    odom_msg.twist.twist.angular.x = 0.0;
    odom_msg.twist.twist.angular.y = 0.0;
    odom_msg.twist.twist.angular.z = odom_wz;

    publisher_odom->publish(odom_msg);
  }

结语

当前四轮独转底盘采用模式化车辆运动学计算轮式里程计：

  • 双阿克曼模式根据四轮实际轮速和内侧轮转角恢复车辆中心速度与角速度，并使用 RK4 积分；
  • 平移模式通过四轮速度平均和转向角圆均值恢复二维平移速度；
  • 自旋模式将四轮实际速度沿各轮转向方向展开为二维速度向量，再根据四轮相对车辆中心的位置分别反算角速度并取平均；
  • 急停和只转向不走状态不累计车体里程。

最终统一得到：

\(v_x,v_y,\\omega_z) \\

以及：

\(x,y,\\theta) \\

并通过 ROS2 标准 nav_msgs/msg/Odometry 发布到：

text 复制代码 
/wheel_odom

常用检查命令：

bash 复制代码 
ros2 topic hz /wheel_odom
ros2 topic echo /wheel_odom

实车测试时可同步记录：

bash 复制代码 
ros2 bag record -o calib_bag /PlatformControl /wheel_odom

如果需要评价轮式里程计的绝对物理精度，应同时使用已知真实行驶距离、真实旋转角或独立定位系统作为 ground truth。控制指令可以用于分析执行响应，但不应单独作为绝对里程真值。

相关推荐
zylyehuo2 个月前
Ranger Mini V3 底盘运动性能测试与录包步骤
car·ros2
spencer_tseng3 个月前
Vehicle outbound
car
SAP学习成长之路2 年前
SAP 零售方案 CAR 系统的介绍与研究
大数据·开发语言·sap·abap·零售·car·bapi
林多2 年前
【Android】CarWatchDog I/O监控服务
android·watchdog·监控·car·disablepackage
笨笨D幸福3 年前
IOI车机系统刷机和改包笔记2 - 改包脚本
笔记·car·雪佛兰
TechMerger3 年前
万字解读 Android 车机核心 ：CarService 的构成和链路～
android·kotlin·car·automotive·carservice
spencer_tseng3 年前
car tire
car·tire
热门推荐
01国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）02GitHub 镜像站点03ZCode 周末送额度活动开启：3 亿 Token 免费领取04历年考研数学一、数学二、数学三真题试卷及答案PDF05蜘蛛磁力 搜索引擎大全，如何使用蜘蛛磁力查找磁力链接062026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？07微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新08VSCode SSH 连接远程服务器后，Codex 插件登录失败09AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路10Agnes AI 免费 API 接入指南：文本、生图、生视频，一套接口全免费