SLAM: 如何生成odom数据

方法一：使用轮式里程计

#!/usr/bin/env python3
# wheel_odometry.pyimport rospy
import math
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, Quaternion, Point, Pose, Vector3
from std_msgs.msg import Float32, Int32
import tf
from tf.broadcaster import TransformBroadcasterclass WheelOdometry:def __init__(self):rospy.init_node('wheel_odometry')# 机器人参数self.wheel_radius = 0.05      # 轮子半径 (米)self.wheel_base = 0.30        # 轮距 (米)self.encoder_ticks_per_rev = 360  # 编码器每转脉冲数# 状态变量self.x = 0.0                  # X 位置 (米)self.y = 0.0                  # Y 位置 (米)self.yaw = 0.0                # 偏航角 (弧度)self.last_left_ticks = 0self.last_right_ticks = 0self.last_time = rospy.Time.now()# 发布者self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)self.tf_broadcaster = TransformBroadcaster()# 订阅者 (编码器数据)rospy.Subscriber('/left_wheel_ticks', Int32, self.left_encoder_cb)rospy.Subscriber('/right_wheel_ticks', Int32, self.right_encoder_cb)self.rate = rospy.Rate(30)  # 30Hzdef left_encoder_cb(self, msg):self.process_encoders(msg.data, self.last_right_ticks)def right_encoder_cb(self, msg):self.process_encoders(self.last_left_ticks, msg.data)def process_encoders(self, left_ticks, right_ticks):"""处理编码器数据并计算里程计"""current_time = rospy.Time.now()dt = (current_time - self.last_time).to_sec()if dt <= 0:return# 计算轮子移动距离left_distance = 2 * math.pi * self.wheel_radius * (left_ticks - self.last_left_ticks) / self.encoder_ticks_per_revright_distance = 2 * math.pi * self.wheel_radius * (right_ticks - self.last_right_ticks) / self.encoder_ticks_per_rev# 计算线速度和角速度linear_velocity = (left_distance + right_distance) / (2 * dt)angular_velocity = (right_distance - left_distance) / (self.wheel_base * dt)# 更新位置delta_distance = (left_distance + right_distance) / 2delta_yaw = (right_distance - left_distance) / self.wheel_baseself.x += delta_distance * math.cos(self.yaw + delta_yaw / 2)self.y += delta_distance * math.sin(self.yaw + delta_yaw / 2)self.yaw += delta_yaw# 发布里程计self.publish_odometry(linear_velocity, angular_velocity, current_time)# 更新状态self.last_left_ticks = left_ticksself.last_right_ticks = right_ticksself.last_time = current_timedef publish_odometry(self, linear_vel, angular_vel, current_time):"""发布里程计消息和 TF 变换"""# 创建 Odometry 消息odom = Odometry()odom.header.stamp = current_timeodom.header.frame_id = "odom"odom.child_frame_id = "base_link"# 设置位置odom.pose.pose.position = Point(self.x, self.y, 0)# 设置方向 (四元数)quaternion = Quaternion()q = tf.transformations.quaternion_from_euler(0, 0, self.yaw)odom.pose.pose.orientation.x = q[0]odom.pose.pose.orientation.y = q[1]odom.pose.pose.orientation.z = q[2]odom.pose.pose.orientation.w = q[3]# 设置速度odom.twist.twist.linear.x = linear_velodom.twist.twist.angular.z = angular_vel# 发布里程计self.odom_pub.publish(odom)# 发布 TF 变换self.tf_broadcaster.sendTransform((self.x, self.y, 0),q,current_time,"base_link","odom")def run(self):while not rospy.is_shutdown():self.rate.sleep()if __name__ == '__main__':try:odom = WheelOdometry()odom.run()except rospy.ROSInterruptException:pass

方法二：IMU + 轮式里程计融合

#!/usr/bin/env python3
# imu_odometry.pyimport rospy
import math
import numpy as np
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, Quaternion, Point, Pose
from sensor_msgs.msg import Imu
from std_msgs.msg import Int32
import tf
from tf.broadcaster import TransformBroadcasterclass IMUOdometry:def __init__(self):rospy.init_node('imu_odometry')# 卡尔曼滤波器参数self.x = np.array([0.0, 0.0, 0.0]).T  # [x, y, yaw]self.P = np.diag([0.1, 0.1, 0.1])     # 协方差矩阵# 系统噪声self.Q = np.diag([0.1, 0.1, 0.05])    # 过程噪声# 订阅者rospy.Subscriber('/imu/data', Imu, self.imu_callback)rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback)# 发布者self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)self.tf_broadcaster = TransformBroadcaster()self.last_time = rospy.Time.now()self.last_cmd_vel = Twist()def imu_callback(self, msg):"""处理 IMU 数据"""current_time = rospy.Time.now()dt = (current_time - self.last_time).to_sec()if dt <= 0:return# 从 IMU 获取角速度angular_vel_z = msg.angular_velocity.z# 预测步骤self.predict_step(self.last_cmd_vel, dt)# 更新步骤 (使用 IMU 的角速度)self.update_imu(angular_vel_z, dt)# 发布里程计self.publish_odometry(current_time)self.last_time = current_timedef cmd_vel_callback(self, msg):"""记录控制命令"""self.last_cmd_vel = msgdef predict_step(self, cmd_vel, dt):"""预测步骤"""# 状态转移矩阵F = np.array([[1, 0, -dt * cmd_vel.linear.x * math.sin(self.x[2])],[0, 1, dt * cmd_vel.linear.x * math.cos(self.x[2])],[0, 0, 1]])# 预测状态self.x[0] += dt * cmd_vel.linear.x * math.cos(self.x[2])self.x[1] += dt * cmd_vel.linear.x * math.sin(self.x[2])self.x[2] += dt * cmd_vel.angular.z# 预测协方差self.P = F @ self.P @ F.T + self.Qdef update_imu(self, angular_vel_z, dt):"""使用 IMU 数据更新"""# 简单的互补滤波imu_yaw_rate = angular_vel_zpredicted_yaw_rate = (self.x[2] - self.last_yaw) / dt if hasattr(self, 'last_yaw') else 0# 融合fused_yaw_rate = 0.8 * predicted_yaw_rate + 0.2 * imu_yaw_rateself.x[2] += fused_yaw_rate * dtself.last_yaw = self.x[2]def publish_odometry(self, current_time):"""发布里程计"""odom = Odometry()odom.header.stamp = current_timeodom.header.frame_id = "odom"odom.child_frame_id = "base_link"# 位置odom.pose.pose.position = Point(self.x[0], self.x[1], 0)# 方向q = tf.transformations.quaternion_from_euler(0, 0, self.x[2])odom.pose.pose.orientation = Quaternion(*q)# 速度 (从控制命令获取)odom.twist.twist = self.last_cmd_velself.odom_pub.publish(odom)# 发布 TFself.tf_broadcaster.sendTransform((self.x[0], self.x[1], 0),q,current_time,"base_link","odom")if __name__ == '__main__':try:odom = IMUOdometry()rospy.spin()except rospy.ROSInterruptException:pass

方法三：卡尔曼滤波融合

安装 EKF 包

sudo apt install ros-noetic-robot-pose-ekf

运行Launch 文件

<!-- launch/odom_ekf.launch -->
<launch><!-- 轮式里程计节点 --><node pkg="my_robot" type="wheel_odometry.py" name="wheel_odom" output="screen"><param name="wheel_radius" value="0.05"/><param name="wheel_base" value="0.30"/></node><!-- IMU 节点 (如果有的话) --><node pkg="my_robot" type="imu_driver.py" name="imu_driver" output="screen"/><!-- EKF 融合 --><node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf"><param name="output_frame" value="odom"/><param name="base_footprint_frame" value="base_link"/><param name="freq" value="30.0"/><param name="sensor_timeout" value="1.0"/><param name="odom_used" value="true"/><param name="imu_used" value="true"/><param name="vo_used" value="false"/><remap from="odom" to="/wheel_odom/odom" /><remap from="imu_data" to="/imu/data" /></node><!-- 发布静态 TF --><node pkg="tf" type="static_transform_publisher" name="base_to_imu"args="0 0 0 0 0 0 base_link imu_link 100" />
</launch>

完整的 SLAM 集成示例

运行SLAM Launch 文件

<!-- launch/slam.launch -->
<launch><!-- 里程计节点 --><node pkg="my_robot" type="wheel_odometry.py" name="odometry" output="screen"/><!-- 激光雷达 --><node pkg="rplidar_ros" type="rplidarNode" name="rplidar"><param name="serial_port" value="/dev/ttyUSB0"/><param name="frame_id" value="laser_link"/></node><!-- Gmapping SLAM --><node pkg="gmapping" type="slam_gmapping" name="slam_gmapping"><param name="base_frame" value="base_link"/><param name="odom_frame" value="odom"/><param name="map_frame" value="map"/><param name="map_update_interval" value="1.0"/><param name="maxUrange" value="10.0"/><param name="sigma" value="0.05"/><param name="kernelSize" value="1"/><param name="lstep" value="0.05"/><param name="astep" value="0.05"/><param name="iterations" value="5"/><param name="lsigma" value="0.075"/><param name="ogain" value="3.0"/><param name="lskip" value="0"/><param name="minimumScore" value="50"/><param name="srr" value="0.1"/><param name="srt" value="0.2"/><param name="str" value="0.1"/><param name="stt" value="0.2"/><param name="linearUpdate" value="0.2"/><param name="angularUpdate" value="0.1"/><param name="temporalUpdate" value="0.5"/><param name="resampleThreshold" value="0.5"/><param name="particles" value="30"/><param name="xmin" value="-10.0"/><param name="ymin" value="-10.0"/><param name="xmax" value="10.0"/><param name="ymax" value="10.0"/><param name="delta" value="0.05"/><param name="llsamplerange" value="0.01"/><param name="llsamplestep" value="0.01"/><param name="lasamplerange" value="0.005"/><param name="lasamplestep" value="0.005"/></node><!-- 静态 TF --><node pkg="tf" type="static_transform_publisher" name="base_to_laser"args="0.2 0 0.1 0 0 0 base_link laser_link 100" /><!-- RViz --><node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_robot)/config/slam.rviz"/>
</launch>

验证和调试

# 查看里程计话题
rostopic echo /odom# 检查 TF 树
rosrun rqt_tf_tree rqt_tf_tree# 可视化里程计路径
rosrun rviz rviz -d $(find my_robot)/config/odom_debug.rviz

里程计调试配置

# config/odom_debug.rviz
Visualization Manager:Displays:- Class: rviz/OdometryName: OdometryTopic: /odomKeep: 100Position Tolerance: 0.1Angle Tolerance: 0.1Color: 255; 0; 0- Class: rviz/TFName: TF- Class: rviz/RobotModelName: Robot Model

总结

在 SLAM 场景中生成 odom 数据的方法：

1. 轮式里程计：最常用，基于编码器数据

2. IMU 融合：提高角度估计精度

3. EKF 滤波：多传感器融合，最稳定

4. 速度积分：简单但容易漂移

推荐方案：使用轮式里程计 + IMU 融合的 EKF 方法，这样既能保证短期精度，又能减少长期漂移。