机器人 / 系统课时

从编码器反馈推算底盘里程计

从编码器反馈推算底盘里程计

把左右轮编码器的累计转角转换成机器人在 odom 坐标系下的位置和朝向,再通过 ROS 2 的 nav_msgs/msg/Odometry 发布车体速度与协方差。用 Jazzy 的两帧关节反馈验证前进距离。

学习目标

  • 根据轮半径、轮距和编码器角度增量计算平面位姿。
  • 使用 rclpy 从 /joint_states 生成包含速度与协方差的里程计消息。
  • 通过注入已知反馈检查时间戳、坐标系及计算结果。

从轮角增量还原车体运动

假设双轮差速底盘的轮半径 r=0.05 m、左右轮中心距 b=0.30 m。JointState.position 中的轮关节位置单位为弧度,不能把电机轴转角或编码器脉冲数直接当作车轮弧度;应先结合减速比与每转计数换算,并对跨越角度周期的反馈做连续展开。

设一段时间内左右轮角增量分别为 ΔϕL\Delta\phi_L 和 ΔϕR\Delta\phi_R,则前进距离及航向变化为:

Δs=r2(ΔϕR+ΔϕL),Δθ=rb(ΔϕR−ΔϕL)\Delta s = \frac{r}{2}(\Delta\phi_R+\Delta\phi_L),\qquad \Delta\theta = \frac{r}{b}(\Delta\phi_R-\Delta\phi_L)

以区间中点朝向 θ+Δθ/2\theta+\Delta\theta/2 近似积分: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 的排列顺序;首帧作为零位基准,之后用消息时间戳求速度。

python
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 启动脚本:

bash
source /opt/ros/jazzy/setup.bash
python3 wheel_odom.py

预期输出包含 waiting for /joint_states;日志时间戳 <TIME> 随运行环境变化。保持此终端运行,在终端 B 先启动消息观察:

bash
source /opt/ros/jazzy/setup.bash
ros2 topic echo /wheel_odom --once

预期结果:终端 B 等待下一条里程计消息。最后在终端 C 依次发送两个关节角度样本,保持 name 与代码一致:

bash
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、激光或其他定位观测。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

从编码器反馈推算底盘里程计

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通