一篇文章搞懂移动机器人底盘里程计更新代码

目录摘要一、轮式里程计到底是什么1.1 轮式里程计的基本概念1.2 轮式里程计不是硬件但依赖硬件二、这段代码整体在做什么2.1 完整代码2.2 代码的核心功能三、Pose2D 结构体到底是什么3.1 struct Pose2D 的含义3.2 为什么不用三个普通变量3.3 Pose2D pose{0.0, 0.0, 0.0}; 是什么意思四、为什么函数参数要写成 Pose2D pose4.1 Pose2D pose 的含义4.2 如果不用引用会发生什么4.3 为什么这里必须用引用4.4 引用和指针有什么区别五、循环积分、输出结果与工程注意事项5.1 for 循环在做什么5.2 这段代码属于简单欧拉积分5.3 工程中需要注意的点1theta 最好做角度归一化2v 和 w 的来源要明确3编码器里程计会累积误差4cos() 和 sin() 使用的是弧度六、总结摘要在移动机器人底盘控制中机器人不仅要知道自己应该怎么运动还要不断估计“自己现在在哪里”。这就是里程计更新的核心作用。本文承接上一章里程计相关内容围绕一段简单的C 二维机器人里程计代码展开详细讲解struct Pose2D、Pose2D引用传参、Pose2D pose{0.0, 0.0, 0.0}初始化方式、cos和sin在位姿更新中的作用以及循环积分如何一步步得到机器人的最终位置和航向角。通过本文可以帮助初学者真正理解机器人运动学代码背后的含义。上一章节移动机器人底盘运动学模型全解析链接如下移动机器人底盘运动学模型全解析-CSDN博客https://blog.csdn.net/m0_58954356/article/details/161490524?spm1001.2014.3001.5502一、轮式里程计到底是什么1.1 轮式里程计的基本概念轮式里程计英文通常称为Wheel Odometry。它本质上并不是某一个具体的硬件传感器而是一种基于轮子运动信息来估计机器人位姿的算法或方法。简单来说轮式里程计的作用就是根据机器人轮子的转动情况推算机器人在二维平面中的当前位置和朝向。在移动机器人中机器人位姿通常可以表示为x // 机器人在地图坐标系中的 x 坐标 y // 机器人在地图坐标系中的 y 坐标 theta // 机器人当前航向角也就是说轮式里程计要解决的问题是机器人从哪里出发机器人走了多远机器人现在在哪里机器人当前朝向是多少需要注意的是轮式里程计并不是直接测量机器人在地图中的位置而是通过轮子的转动信息进行间接推算。1.2 轮式里程计不是硬件但依赖硬件严格来说编码器是硬件传感器轮式里程计是算法 / 估计方法在实际机器人系统中轮式里程计通常依赖电机编码器或轮编码器提供基础数据。编码器可以检测轮子或电机的转动情况例如轮子转了多少圈 轮子当前转速是多少 电机输出了多少脉冲然后程序再根据这些信息结合轮子半径、轮距、底盘运动学模型等参数进一步计算轮子走过的距离 机器人前进的距离 机器人转过的角度 机器人当前的 x、y、theta因此轮式里程计可以理解为轮式里程计 硬件采集轮子转动信息 算法计算机器人位姿其中硬件负责采集轮子运动数据算法负责根据这些数据推算机器人位置和姿态。二、这段代码整体在做什么2.1 完整代码#include iostream #include cmath using namespace std; struct Pose2D { double x; double y; double theta; }; void update_odometry(Pose2D pose, double v, double w, double dt) { pose.x v * dt * cos(pose.theta); pose.y v * dt * sin(pose.theta); pose.theta w * dt; } int main() { Pose2D pose{0.0, 0.0, 0.0}; double v 0.5; double w 0.2; double dt 0.02; for (int i 0; i 100; i) { update_odometry(pose, v, w, dt); } cout x pose.x endl; cout y pose.y endl; cout theta pose.theta endl; return 0; }2.2 代码的核心功能这段代码模拟的是一个移动机器人在二维平面上的运动过程。机器人有三个状态量x // 机器人在地图中的 x 坐标 y // 机器人在地图中的 y 坐标 theta // 机器人当前朝向角然后给定机器人运动指令double v 0.5; // 线速度 double w 0.2; // 角速度 double dt 0.02; // 时间间隔含义是机器人每秒向前运动 0.5 m 机器人每秒旋转 0.2 rad 每隔 0.02 秒更新一次位置循环 100 次for (int i 0; i 100; i)总运动时间为100 * 0.02 2 s所以这段代码本质上是在计算机器人以 0.5 m/s 的线速度和 0.2 rad/s 的角速度运动 2 秒后最终的位置和朝向是多少。三、Pose2D结构体到底是什么3.1struct Pose2D的含义代码中定义了一个结构体struct Pose2D { double x; double y; double theta; };Pose2D可以理解为一个自定义数据类型。它把机器人二维平面中的位姿信息封装到了一起。Pose2D x 坐标 y 坐标 航向角 theta其中x 表示机器人在世界坐标系下的横向位置 y 表示机器人在世界坐标系下的纵向位置 theta 表示机器人当前朝向角在移动机器人中二维位姿通常写成pose (x, y, theta)也就是位置 姿态3.2 为什么不用三个普通变量当然也可以直接写成double x; double y; double theta;但是这样写有一个问题这三个变量本来是属于同一个机器人状态的如果分散写后续函数传参会比较乱。比如更新里程计时需要同时修改x y theta如果不用结构体函数可能要写成void update_odometry(double x, double y, double theta, double v, double w, double dt)这样参数会越来越多可读性也会变差。使用结构体后可以把它们统一封装Pose2D pose;这样代码更加清晰pose.x pose.y pose.theta看到pose就知道它表示机器人的当前位姿。3.3Pose2D pose{0.0, 0.0, 0.0};是什么意思这一句非常重要Pose2D pose{0.0, 0.0, 0.0};它表示创建一个Pose2D类型的变量变量名叫pose。因为Pose2D结构体中有三个成员double x; double y; double theta;所以花括号中的三个数会依次赋值给这三个成员pose.x 0.0; pose.y 0.0; pose.theta 0.0;因此Pose2D pose{0.0, 0.0, 0.0};等价于Pose2D pose; pose.x 0.0; pose.y 0.0; pose.theta 0.0;这叫做列表初始化也可以叫做花括号初始化。它的含义就是机器人初始位置在原点初始朝向角为 0。四、为什么函数参数要写成Pose2D pose4.1Pose2D pose的含义函数定义如下void update_odometry(Pose2D pose, double v, double w, double dt)这里最关键的是Pose2D pose其中表示引用。所以Pose2D pose表示pose 是一个 Pose2D 类型变量的引用。通俗理解就是函数里面的 pose是 main 函数里面 pose 的另一个名字。也就是说函数内部修改pose.x、pose.y、pose.theta实际上修改的就是main函数中的那个pose。4.2 如果不用引用会发生什么如果函数写成这样void update_odometry(Pose2D pose, double v, double w, double dt) { pose.x v * dt * cos(pose.theta); pose.y v * dt * sin(pose.theta); pose.theta w * dt; }注意这里没有Pose2D pose这叫做值传递。值传递会把外面的pose复制一份传进函数。也就是说main 函数中的 pose和update_odometry 函数中的 pose不是同一个变量。函数内部虽然修改了pose.x pose.y pose.theta但是修改的是副本。函数执行完以后副本被销毁外面的pose不会发生变化。所以如果不用引用最后输出的结果很可能还是x 0 y 0 theta 0这显然不符合里程计更新的目的。4.3 为什么这里必须用引用因为update_odometry这个函数的目的就是更新机器人的状态。也就是说它要把旧的 pose更新成新的 pose所以必须让函数能够修改外部的pose。因此这里写成void update_odometry(Pose2D pose, double v, double w, double dt)这样函数内部的修改才会真正保留下来。可以简单记住一句话函数想修改外部变量就可以使用引用传参。4.4 引用和指针有什么区别这段代码也可以用指针写void update_odometry(Pose2D* pose, double v, double w, double dt) { pose-x v * dt * cos(pose-theta); pose-y v * dt * sin(pose-theta); pose-theta w * dt; }调用时要写成update_odometry(pose, v, w, dt);但是相比指针引用写法更自然pose.x pose.y pose.theta而指针要写成pose-x pose-y pose-theta所以在 C 中如果函数需要修改外部对象引用是一种很常见、很清晰的写法。五、循环积分、输出结果与工程注意事项5.1for循环在做什么主函数中有这样一段代码for (int i 0; i 100; i) { update_odometry(pose, v, w, dt); }每执行一次循环就更新一次机器人位姿。每次更新的时间间隔是dt 0.02 s循环 100 次所以总时间是T 100 * 0.02 2 s也就是说这段程序模拟机器人连续运动 2 秒。每一次循环都在做根据当前 pose、线速度 v、角速度 w 和时间间隔 dt计算下一时刻的 pose。5.2 这段代码属于简单欧拉积分这段更新方式属于比较基础的离散积分方法也可以理解为欧拉积分当前位置 上一时刻位置 当前速度 * 时间间隔对应代码pose.x v * dt * cos(pose.theta); pose.y v * dt * sin(pose.theta); pose.theta w * dt;这种方法简单、直观非常适合入门学习机器人里程计。但是它也有一个问题dt 越大误差可能越明显。所以在真实机器人控制中一般会让dt尽量小并且保持稳定。例如常见控制周期可能是10 ms 0.01s20 ms 0.02s50 ms 0.05s5.3 工程中需要注意的点实际机器人里程计更新时不能只看这三行代码还要注意以下问题。1theta最好做角度归一化如果机器人一直旋转theta会越来越大。例如theta 7.0 rad theta 20.0 rad theta 100.0 rad虽然数学上仍然能计算但是工程中不方便观察和控制。常见做法是把theta限制在[-pi, pi]或者[0, 2*pi]2v和w的来源要明确在真实机器人中v和w可能来自编码器IMU底盘控制器反馈速度指令传感器融合结果如果速度来源不准确里程计结果也会有误差。3编码器里程计会累积误差机器人在真实地面上运动时可能存在轮胎打滑地面不平电机响应滞后轮子半径误差左右轮间距误差这些都会导致里程计逐渐漂移。所以实际工程中常常会结合IMU激光雷达视觉GPS / RTKSLAM对里程计进行修正。4cos()和sin()使用的是弧度这是初学者非常容易犯错的地方。如果写theta 90;这不是 90 度而是 90 弧度。如果要把角度转换为弧度需要写theta_rad theta_deg * 3.1415926 / 180.0;六、总结本文这段代码虽然不长但是包含了移动机器人控制中非常重要的几个知识点struct Pose2D用于封装二维位姿包括x、y和theta。Pose2D pose{0.0, 0.0, 0.0};表示创建一个机器人初始位姿初始位置在原点初始朝向为 0。Pose2D pose表示引用传参函数内部修改pose会直接修改外部的pose。pose.x v * dt * cos(pose.theta); pose.y v * dt * sin(pose.theta); pose.theta w * dt;表示根据机器人二维运动学模型更新位姿。这段代码可以看作机器人里程计的最基础版本。理解它以后后续学习差速底盘、阿克曼底盘、ROS 里程计发布、路径跟踪控制、导航定位都会轻松很多。