机器人--里程计

教程

什么是里程计

里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法。该方法被用在许多机器人系统来估计机器人相对于初始位置移动的距离 。

注意：里程计是一套算法，不是物理硬件。

作用

机器人相对于初始位置移动的位姿信息。

在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);
  }