教程
什么是里程计
里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法。该方法被用在许多机器人系统来估计机器人相对于初始位置移动的距离。
注意：里程计是一套算法，不是物理硬件。
作用
机器人相对于初始位置移动的位姿信息。
在ros中，通过发布里程计话题odom，其他模块可以订阅这个话题的数据，获取机器人实时的位姿信息。
里程计和TF
既然里程计话题发布机器人的位姿信息，TF也是发布机器人位姿信息，为什么不发布里程计话题就可以了，还要计算出里程之后再发布tf？
| 特性 | 里程计话题 | TF（Transform） |
|---|---|---|
| 数据内容 | 包含位姿（pose）和速度（twist），通常带协方差 |
仅包含坐标系间的刚性变换（平移+旋转） |
| 主要用途 | 提供里程计的完整运动状态（供算法使用） | 提供坐标系间的空间关系（供可视化、传感器融合等使用） |
| 消费者 | 导航算法（如EKF、路径规划） | RViz、传感器数据处理节点、多坐标系协作模块 |
| 更新频率 | 通常较低（10-50Hz，取决于传感器） | 通常较高（与TF广播频率一致，可能100Hz+） |
| 话题名称 | 不固定，不同的机器人可以使用不同的里程计话题名称，创建话题时需要质地的那个话题名称。 订阅的时候也通过话题名称订阅。 |
固定话题名称，/tf和/tf_static。所有的TF变化都通过这两个话题发布，再通过looup_transform订阅指定两个坐标系的TF。 |
| RVIZ插件 | Odometry插件 | TF插件 |
RVIZ对TF的依赖要更强些。
里程计在ros中的作用
ros2中的里程计odometry：
# 包含父ID
std_msgs/Header header
# 子ID，即姿势所在的坐标系
string child_frame_id
# 通常相对于父坐标的估计姿态.
geometry_msgs/PoseWithCovariance pose
# 在子坐标系中的线速度和角速度.
geometry_msgs/TwistWithCovariance twist
ros2中的里程计是需要通过传感器比如编码器，IMU等获取机器人的线速度，角速度，加速度等信息来完成填充odometry接口中的数据，之后再以话题的方式发布出去。
可见ros2中的里程计不仅可以发布机器人的当前位姿，还包括机器人的线速度和角速度等信息。
里程计的分类
轮式里程计
轮式里程计是通过电机编码器反馈电机转速，再利用电机转速和减速比等物理参数计算出小车轮子的转速，再通过运动学正解计算小车整体的线速度和角速度，也就是里程计需要发布的参数的。
角度累计
不断累计机器人绕Y轴转动的角度yam，其实根据三角函数计算式可知，超过180度就是计算-180度内的角度，超过-180度就是计算180度内的角度，所以，机器人转一圈的范围也就是0~180或者0~-180之间，为了防止角度溢出，我们只需要控制角度累计的角度再-180~180之间即可。
两轮差速位置累计
delta_th = vth * dt;
odom_th_ += delta_th; // 角度累计--姿态累计
delta_x = vx * cos(odom_th_) * dt; // 位姿累计
delta_y = vx * sin(odom_th_) * dt;
适用对象： 专为 两轮差分驱动机器人 设计（如TurtleBot），假设机器人 只有X方向线速度（vx）和Z轴角速度（vth），Y方向速度始终为0。
对于差分驱动机器人：
Y方向速度恒为0（不能横向移动），因此
Y * sin(pZ)和Y * cos(pZ)项为0。若
dt很小，角度变化delta_th对位移的影响可忽略，此时方法1是方法2的近似。
通用2D位置累计
pZ += Z * Sampling_Time; // 角度累计
pX += (X * cos(pZ) - Y * sin(pZ)) * Sampling_Time; // 位置累计
pY += (X * sin(pZ) + Y * cos(pZ)) * Sampling_Time;适用对象： 适用于 任何2D平面运动的机器人（如全向轮、麦克纳姆轮机器人），支持 X/Y方向线速度（X, Y）和Z轴角速度（Z）。
哪种更准确？
如果机器人 严格两轮差分驱动，且
dt足够小： 两种方法等效（因为Y=0，方法2退化为方法1）。如果机器人 有Y方向速度 或
dt较大： 必须用方法2，否则会忽略旋转对位移的耦合影响。如果机器人 快速旋转（
vth较大）： 方法2更准确，方法1会引入误差。
里程累计
里程累计包括位置，即X轴位置和Y轴位置累计和角度累计，三者共同表示机器人的位姿。
注意：一定要线角度累计，再利用累计的角度计算位置累计。
惯性里程计
视觉里程计
激光里程计
ros中的实现--odometry
odometry:
# 包含父ID
std_msgs/Header header
# 子ID，即姿势所在的坐标系
string child_frame_id
# 通常相对于父坐标的估计姿态.
geometry_msgs/PoseWithCovariance pose
# 在子坐标系中的线速度和角速度.
geometry_msgs/TwistWithCovariance twist
odom.child_frame_id = robot_frame_id; #里程计TF子坐标
odom.twist.twist.linear.x = X; # X方向速度
odom.twist.twist.linear.y = Y; # Y方向速度
odom.twist.twist.angular.z = Z; # 绕Z轴角速度
角速度可以通过IMU获取，或者通过编码器和运动学正解获取机器人整体角速度和线速度，那么XY方向的线速度怎么获取呢？
在轮式里程计中，计算出机器人整体的角速度和线速度之后，可以利用角速度计算出这段时间内的总的旋转角度，再通过总的旋转角度计算出线速度再XY轴上各自的分量，各自分速度再和时间积分就是位置里程。
delta_th = vth * dt;
odom_th_ += delta_th; // 角度累计--姿态累计
delta_x = vx * cos(odom_th_) * dt; // 位姿累计
delta_y = vx * sin(odom_th_) * dt;里程计和TF广播
里程计是如何完成TF广播的？
里程计代表机器人的位置相对于初始位置的累加变化，即机器人当前位姿。
TF(动态)广播器记录和发布的是两个坐标系之间的实时平移和旋转的坐标系变换关系。
tf2::Quaternion q;
q.setRPY(0, 0, odom_th_); // odom_th_是yam旋转角累计
// 发布里程计话题，用于算法
odom_publisher_->publish(odom_msg);
// 之后发布TF，用于RVIZ坐标变换
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "odom";
t.child_frame_id = "base_footprint";
t.transform.translation.x = odom_x_;
t.transform.translation.y = odom_y_;
t.transform.translation.z = 0.0;
t.transform.rotation.x = q[0];
t.transform.rotation.y = q[1];
t.transform.rotation.z = q[2];
t.transform.rotation.w = q[3];
if (pub_odom_) {
// 广播里程计TF
tf_broadcaster_->sendTransform(t);
}里程计话题发布
1，创建话题
2，创建话题消息
消息类型：
nav_msgs/msg/Odometry
# 消息中的 pose（位姿） 应该由 header.frame_id 给出的坐标系指定。
# 消息中的 twist（速度信息） 应该由 child_frame_id 给定的坐标系来指定。
# 包含位姿的父坐标系的 frame_id。
- 这是一个标准的 ROS 2 消息头（Header），包含时间戳和父坐标系 ID。
- `header.frame_id` 表示 **位姿（pose）所在的参考系**，也就是“父坐标系”。
std_msgs/Header header
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
string child_frame_id
# 估计的位姿，通常是相对于一个固定的全局世界坐标系。
geometry_msgs/PoseWithCovariance pose
Pose pose
Point position
float64 x
float64 y
float64 z
Quaternion orientation
float64 x 0
float64 y 0
float64 z 0
float64 w 1
float64[36] covariance
# 估计的线速度和角速度是相对于子坐标系的。
geometry_msgs/TwistWithCovariance twist
Twist twist
Vector3 linear
float64 x
float64 y
float64 z
Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
1. std_msgs/Header header
作用：消息的头部信息，包含时间戳和参考坐标系。
stamp(builtin_interfaces/Time)：sec：时间戳的秒部分（Unix时间）。nanosec：时间戳的纳秒部分（0~999,999,999）。用途：标识里程计数据的时间点，用于与其他传感器数据同步。
frame_id(string)：值示例：
"odom"。用途：既是里程计的父坐标系，也是pose的参考坐标系，也就是pose的父坐标系，也就是pose是child_frame_id这个坐标系相对于frame_id这个坐标系的pose。
2. string child_frame_id
作用：指定机器人本体的坐标系。
值示例：
"base_footprint"或"base_link"。用途：里程计子坐标系，也就是机器人本体坐标系，也是pose和速度信息所在坐标系。
3. geometry_msgs/PoseWithCovariance pose
作用：机器人相对于全局坐标系的位姿（位置+朝向）及其不确定性。
pose(geometry_msgs/Pose)：position(Point)：x,y,z：3D位置坐标（单位：米）。注意：对于地面机器人，
z通常为0（除非有高度变化）。
orientation(Quaternion)：x,y,z,w：四元数表示朝向（需归一化）。默认值：
(0, 0, 0, 1)表示无旋转（朝向与父坐标系一致）。
covariance(float64[36])：位姿的6x6协方差矩阵（按行优先排列），表示不确定性：
[x_var, xy, xz, x_roll, x_pitch, x_yaw,
xy, y_var, yz, y_roll, y_pitch, y_yaw,
...]4. geometry_msgs/TwistWithCovariance twist
作用：机器人本体的线速度和角速度及其不确定性。
twist(geometry_msgs/Twist)：linear(Vector3)：x：前进速度（单位：米/秒，正值为前进）。y：横向速度（正值为左移，差分驱动机器人通常为0）。z：垂直速度（地面机器人通常为0）。
angular(Vector3)：x：横滚角速度（绕X轴旋转，地面机器人通常为0）。y：俯仰角速度（绕Y轴旋转，地面机器人通常为0）。z：偏航角速度（绕Z轴旋转，单位：弧度/秒，正值为逆时针）。
covariance(float64[36])：速度的6x6协方差矩阵（排列方式同
pose.covariance）。用途：描述速度测量的噪声特性（如IMU误差）。
协方差的选择
const double odom_pose_covariance[36] = {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 };
const double odom_pose_covariance2[36] = {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 };
const double odom_twist_covariance[36] = {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 };
const double odom_twist_covariance2[36] = {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} ;
什么是协方差矩阵？
在概率机器人学中，协方差矩阵用来表示某个状态变量的不确定性。数值越大，说明该维度上的测量或估计越不可靠。
位姿协方差结构：
[ x_x, x_y, x_z, x_roll, x_pitch, x_yaw, // x可靠性
y_x, y_y, y_z, y_roll, y_pitch, y_yaw, // y可靠性
z_x, z_y, z_z, z_roll, z_pitch, z_yaw, // z可靠性
roll_x, roll_y, roll_z, roll_roll, roll_pitch, roll_yaw, // roll可靠性
pitch_x, pitch_y, pitch_z, pitch_roll, pitch_pitch, pitch_yaw, // pitch可靠性
yaw_x, yaw_y, yaw_z, yaw_roll, yaw_pitch, yaw_yaw ] // yaw可靠性{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}| 维度 | 值 | 含义说明 |
|---|---|---|
x, y |
1e-3 |
X、Y 位置估计较准确 |
z |
1e6 |
Z 轴方向不关心或不可靠（如地面机器人） |
roll, pitch |
1e6 |
不依赖或无法测量 |
yaw |
1e3 |
偏航角有一定误差 |
x和y方向：高精度（方差=0.001，置信度高）。z方向：极低精度（方差=1e6，表示忽略此方向，适用于地面机器人）。旋转（
roll、pitch）：极低精度（方差=1e6，通常地面机器人不关心俯仰/横滚）。yaw方向：中等精度（方差=1000，适用于航向角估计）。
这是一个典型的 2D 地面机器人位姿协方差矩阵
{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}移动的x和旋转的yaw方向：极高精度（方差=1e-9，几乎完全信任）。
位姿协方差结构类似，但表示速度的不确定性（数据可靠性）。
-- 机器人禁止使用高可靠性协方差；运动姿态使用低可靠性协方差。