机器人 / 系统课时
从编码器反馈推算底盘里程计
从编码器反馈推算底盘里程计
把左右轮编码器的累计转角转换成机器人在 odom 坐标系下的位置和朝向,再通过 ROS 2 的 nav_msgs/msg/Odometry 发布车体速度与协方差。用 Jazzy 的两帧关节反馈验证前进距离。
学习目标
- 根据轮半径、轮距和编码器角度增量计算平面位姿。
- 使用
rclpy从/joint_states生成包含速度与协方差的里程计消息。 - 通过注入已知反馈检查时间戳、坐标系及计算结果。
从轮角增量还原车体运动
假设双轮差速底盘的轮半径 r=0.05 m、左右轮中心距 b=0.30 m。JointState.position 中的轮关节位置单位为弧度,不能把电机轴转角或编码器脉冲数直接当作车轮弧度;应先结合减速比与每转计数换算,并对跨越角度周期的反馈做连续展开。
设一段时间内左右轮角增量分别为 和 ,则前进距离及航向变化为:
以区间中点朝向 近似积分:x 增加 Δs·cos(中点朝向),y 增加 Δs·sin(中点朝向)。再将 Δs/Δt 作为 base_link 的前向速度,将 Δθ/Δt 作为绕 Z 轴角速度。例如两轮都转 1 rad,耗时 1 s,机器人应前进 0.05 m、不转向,前向速度为 0.05 m/s。

用 rclpy 发布里程计
适用环境为 Ubuntu 24.04 + ROS 2 Jazzy,系统可导入 rclpy、sensor_msgs、nav_msgs。将下面代码保存为 wheel_odom.py。订阅者按关节名称查找左右轮,避免依赖 JointState.name 的排列顺序;首帧作为零位基准,之后用消息时间戳求速度。
import math
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from nav_msgs.msg import Odometry
from sensor_msgs.msg import JointState
class WheelOdom(Node):
def __init__(self):
super().__init__('wheel_odom')
self.declare_parameter('wheel_radius', 0.05)
self.declare_parameter('wheel_separation', 0.30)
self.radius = float(self.get_parameter('wheel_radius').value)
self.separation = float(self.get_parameter('wheel_separation').value)
if self.radius <= 0 or self.separation <= 0:
raise ValueError('wheel geometry must be positive')
self.x = self.y = self.yaw = 0.0
self.previous = None
self.publisher = self.create_publisher(Odometry, '/wheel_odom', 10)
self.create_subscription(JointState, '/joint_states', self.update, 10)
self.get_logger().info('waiting for /joint_states')
def update(self, msg):
try:
left = msg.position[msg.name.index('left_wheel_joint')]
right = msg.position[msg.name.index('right_wheel_joint')]
except (ValueError, IndexError):
return # 缺少指定关节或对应角度时丢弃
stamp_ns = msg.header.stamp.sec * 1_000_000_000 + msg.header.stamp.nanosec
if self.previous is None:
self.previous = (left, right, stamp_ns)
return # 首帧只建立积分基准
old_left, old_right, old_ns = self.previous
self.previous = (left, right, stamp_ns)
dt = (stamp_ns - old_ns) / 1e9
if dt <= 0:
return # 不使用重复或倒退的时间戳
dl = (left - old_left) * self.radius
dr = (right - old_right) * self.radius
ds = (dl + dr) / 2
dyaw = (dr - dl) / self.separation
mid_yaw = self.yaw + dyaw / 2
self.x += ds * math.cos(mid_yaw)
self.y += ds * math.sin(mid_yaw)
self.yaw = math.atan2(math.sin(self.yaw + dyaw),
math.cos(self.yaw + dyaw))
odom = Odometry()
odom.header.stamp = msg.header.stamp
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.orientation.z = math.sin(self.yaw / 2)
odom.pose.pose.orientation.w = math.cos(self.yaw / 2)
odom.twist.twist.linear.x = ds / dt
odom.twist.twist.angular.z = dyaw / dt
# 6×6 协方差按 x、y、z、roll、pitch、yaw 排列;数值仅供演示
for k, variance in enumerate((0.0025, 0.0025, 1000.0, 1000.0, 1000.0, 0.04)):
odom.pose.covariance[k * 6 + k] = variance
for k, variance in enumerate((0.01, 0.01, 1000.0, 1000.0, 1000.0, 0.09)):
odom.twist.covariance[k * 6 + k] = variance
self.publisher.publish(odom)
def main():
rclpy.init()
node = WheelOdom()
try:
rclpy.spin(node)
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
node.destroy_node()
rclpy.try_shutdown()
if __name__ == '__main__':
main()预期结果:节点保持运行并监听 /joint_states;收到第一帧时不发布,收到第二帧且时间递增时向 /wheel_odom 发布 Odometry。pose 位姿属于 odom,twist 速度属于 base_link,而且此示例不会额外广播 /tf。
用两帧确定性数据验收
在终端 A 启动脚本:
source /opt/ros/jazzy/setup.bash
python3 wheel_odom.py预期输出包含 waiting for /joint_states;日志时间戳 <TIME> 随运行环境变化。保持此终端运行,在终端 B 先启动消息观察:
source /opt/ros/jazzy/setup.bash
ros2 topic echo /wheel_odom --once预期结果:终端 B 等待下一条里程计消息。最后在终端 C 依次发送两个关节角度样本,保持 name 与代码一致:
source /opt/ros/jazzy/setup.bash
ros2 topic pub --once /joint_states sensor_msgs/msg/JointState "{header: {stamp: {sec: 10, nanosec: 0}}, name: ['left_wheel_joint', 'right_wheel_joint'], position: [0.0, 0.0]}"
sleep 1
ros2 topic pub --once /joint_states sensor_msgs/msg/JointState "{header: {stamp: {sec: 11, nanosec: 0}}, name: ['left_wheel_joint', 'right_wheel_joint'], position: [1.0, 1.0]}"预期结果:B 终端接收到一条消息,header.stamp.sec=11、pose.pose.position.x≈0.05、pose.pose.position.y≈0、pose.pose.orientation.w≈1、twist.twist.linear.x≈0.05、twist.twist.angular.z≈0;两组 covariance 各有 36 项,示例只设置对角线。
排除错误数据与重复发布
- 一直没有输出:先确认
JointState.name中存在两个指定轮关节、position数量匹配,且连续两帧header.stamp严格递增。仅发送一帧无法计算增量。 - 直行却旋转:检查左右轮关节方向是否同向定义,并实测轮距;两轮相同的正角度增量应给出
Δθ=0。若轮反馈周期性跳变,应先展开累计角度。 - 协方差设置不合理:6×6 矩阵的对角线是方差,例如
0.0025 m²代表位置标准差0.05 m;非对角线是交叉协方差。代码中的固定数值仅供字段验收;实际应从直行、转弯试验标定轮噪声,并随里程积分传播位姿协方差。
轮式里程计从初始位姿累计相对运动,不能消除地面打滑造成的漂移;要提供长期稳定的全局位置,还需融合 IMU、激光或其他定位观测。