Real Robot - 真实机器人¶
在仿真环境中测试完算法后，下一步就是将其部署到真实机器人上。本章将介绍如何在真实机器人上使用 ROS。
从仿真到真实¶
┌─────────────────────────────────────────────────────────────┐
│ 从仿真到真实的迁移过程 │
├─────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │
│ │ 仿真 │ │ 测试 │ │ 部署 │ │
│ │ (Gazebo) │───▶│ (实验室) │───▶│ (真实) │ │
│ └─────────────┘ └─────────────┘ └─────────────┘ │
│ │
│ 挑战： │
│ - 传感器噪声和延迟 │
│ - 执行器响应时间 │
│ - 环境不确定性 │
│ - 安全性考虑 │
│ │
│ 解决方案： │
│ - 逐步测试，从小范围开始 │
│ - 增加安全机制 │
│ - 监控和日志记录 │
│ - 远程控制和紧急停止 │
│ │
└─────────────────────────────────────────────────────────────┘
硬件接口¶
1. 电机控制¶
#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
class MotorController:
def __init__(self):
# 初始化电机驱动
self.left_motor = Motor(port='/dev/ttyUSB0')
self.right_motor = Motor(port='/dev/ttyUSB1')
# 订阅速度命令
rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback)
def cmd_vel_callback(self, msg):
"""处理速度命令"""
# 差速驱动运动学
linear_vel = msg.linear.x
angular_vel = msg.angular.z
# 计算左右轮速度
wheel_separation = 0.3 # 轮距（米）
left_vel = linear_vel - (angular_vel * wheel_separation / 2)
right_vel = linear_vel + (angular_vel * wheel_separation / 2)
# 设置电机速度
self.left_motor.set_velocity(left_vel)
self.right_motor.set_velocity(right_vel)
2. 传感器驱动¶
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import LaserScan
class LidarDriver:
def __init__(self):
# 初始化激光雷达
self.lidar = Lidar(port='/dev/ttyUSB2')
# 发布激光数据
self.scan_pub = rospy.Publisher('/scan', LaserScan, queue_size=10)
# 定时读取数据
rospy.Timer(rospy.Duration(0.1), self.read_scan)
def read_scan(self, event):
"""读取激光数据"""
# 从硬件读取数据
ranges = self.lidar.get_ranges()
# 创建 LaserScan 消息
scan = LaserScan()
scan.header.stamp = rospy.Time.now()
scan.header.frame_id = 'laser'
scan.angle_min = -3.14
scan.angle_max = 3.14
scan.angle_increment = 0.01
scan.range_min = 0.1
scan.range_max = 10.0
scan.ranges = ranges
# 发布数据
self.scan_pub.publish(scan)
3. 里程计¶
#!/usr/bin/env python3
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
import tf
class OdometryPublisher:
def __init__(self):
# 初始化编码器
self.left_encoder = Encoder(port='/dev/ttyUSB3')
self.right_encoder = Encoder(port='/dev/ttyUSB4')
# 发布里程计
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
# 发布 TF
self.tf_broadcaster = tf.TransformBroadcaster()
# 机器人参数
self.wheel_radius = 0.05 # 轮子半径（米）
self.wheel_separation = 0.3 # 轮距（米）
# 初始位置
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# 上一次编码器值
self.last_left_enc = 0
self.last_right_enc = 0
# 定时发布
rospy.Timer(rospy.Duration(0.05), self.publish_odom)
def publish_odom(self, event):
"""发布里程计"""
# 读取编码器
left_enc = self.left_encoder.get_value()
right_enc = self.right_encoder.get_value()
# 计算位移
delta_left = (left_enc - self.last_left_enc) * self.wheel_radius
delta_right = (right_enc - self.last_right_enc) * self.wheel_radius
# 更新上一次编码器值
self.last_left_enc = left_enc
self.last_right_enc = right_enc
# 计算机器人位移
delta_s = (delta_left + delta_right) / 2
delta_theta = (delta_right - delta_left) / self.wheel_separation
# 更新位置
self.x += delta_s * math.cos(self.theta + delta_theta / 2)
self.y += delta_s * math.sin(self.theta + delta_theta / 2)
self.theta += delta_theta
# 创建里程计消息
odom = Odometry()
odom.header.stamp = rospy.Time.now()
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.position.z = 0.0
# 设置方向
q = tf.transformations.quaternion_from_euler(0, 0, self.theta)
odom.pose.pose.orientation = Quaternion(*q)
# 发布里程计
self.odom_pub.publish(odom)
# 发布 TF
self.tf_broadcaster.sendTransform(
(self.x, self.y, 0),
q,
rospy.Time.now(),
'base_link',
'odom'
)
真实机器人调试¶
1. 传感器校准¶
# 检查传感器连接
ls /dev/ttyUSB*
# 测试激光雷达
rostopic echo /scan
# 测试相机
rosrun image_view image_view image:=/camera/image_raw
# 测试编码器
rostopic echo /odom
2. 参数调优¶
# 查看当前参数
rosparam list
# 修改参数
rosparam set /motor_controller/max_speed 0.5
# 保存参数
rosparam dump params.yaml
3. 可视化¶
在 RViz 中添加： - 激光扫描数据 - 相机图像 - 里程计轨迹 - TF 坐标系
安全注意事项¶
1. 紧急停止¶
#!/usr/bin/env python3
import rospy
from std_msgs.msg import Bool
class EmergencyStop:
def __init__(self):
# 订阅紧急停止信号
rospy.Subscriber('/emergency_stop', Bool, self.emergency_stop_callback)
# 发布停止命令
self.stop_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# 紧急停止状态
self.is_stopped = False
def emergency_stop_callback(self, msg):
"""处理紧急停止信号"""
if msg.data:
# 发送停止命令
stop_cmd = Twist()
self.stop_pub.publish(stop_cmd)
self.is_stopped = True
rospy.logwarn("Emergency stop activated!")
2. 碰撞检测¶
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import LaserScan
class CollisionDetector:
def __init__(self):
# 订阅激光数据
rospy.Subscriber('/scan', LaserScan, self.scan_callback)
# 发布停止命令
self.stop_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# 安全距离
self.safe_distance = 0.3 # 米
def scan_callback(self, msg):
"""检测碰撞风险"""
# 检查前方障碍物
front_ranges = msg.ranges[len(msg.ranges)//4:3*len(msg.ranges)//4]
min_distance = min(front_ranges)
if min_distance < self.safe_distance:
# 发送停止命令
stop_cmd = Twist()
self.stop_pub.publish(stop_cmd)
rospy.logwarn(f"Collision risk detected! Distance: {min_distance:.2f}m")
3. 电池监控¶
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import BatteryState
class BatteryMonitor:
def __init__(self):
# 订阅电池状态
rospy.Subscriber('/battery', BatteryState, self.battery_callback)
# 电池阈值
self.low_battery_threshold = 0.2 # 20%
self.critical_battery_threshold = 0.1 # 10%
def battery_callback(self, msg):
"""监控电池状态"""
battery_level = msg.percentage
if battery_level < self.critical_battery_threshold:
rospy.logerr(f"Critical battery level: {battery_level*100:.1f}%")
# 执行紧急停止
self.emergency_stop()
elif battery_level < self.low_battery_threshold:
rospy.logwarn(f"Low battery level: {battery_level*100:.1f}%")
# 返回充电站
self.return_to_charging_station()
真实机器人测试流程¶
步骤 1：硬件检查¶
# 检查所有连接
rostopic list
# 检查传感器数据
rostopic echo /scan
rostopic echo /odom
rostopic echo /camera/image_raw
# 检查电机响应
rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.1
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"
步骤 2：低速测试¶
# 设置低速限制
rosparam set /motor_controller/max_speed 0.1
# 测试基本移动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
步骤 3：导航测试¶
# 启动导航栈
roslaunch my_robot navigation.launch
# 设置初始位置
# 在 RViz 中使用 "2D Pose Estimate"
# 设置目标位置
# 在 RViz 中使用 "2D Nav Goal"
步骤 4：完整测试¶
常见问题¶
1. 传感器数据不准确¶
原因：传感器噪声或校准错误
解决方案： - 校准传感器 - 使用滤波算法（卡尔曼滤波） - 增加传感器冗余
2. 电机响应慢¶
原因：通信延迟或驱动程序问题
解决方案： - 优化通信协议 - 使用实时操作系统 - 增加控制频率
3. 导航不准确¶
原因：里程计漂移或地图不准确
解决方案： - 使用更好的里程计（视觉里程计、IMU） - 重新构建地图 - 使用闭环检测
实验¶
Lab 4-2: 真实机器人使用¶
- 连接真实机器人
- 测试传感器
- 测试电机控制
- 运行简单导航任务
Lab 5: 真实机器人综合项目¶
- 设计任务
- 实现算法
- 测试和调试
- 部署和演示
