当前位置：首页 > news >正文

这里有个坑：Gazebo图像默认带噪声，得先做高斯模糊

news 2026/5/12 23:35:22

ROS系统下gazebo环境中，无人机结合目标跟踪算法（SiamCar），完成对物体的跟踪（可以是小车或者其他的），然后给出轨迹对比图等评估指标。开发语言：python 仿真平台：PIXHAWK 运行环境：ros （建议Ubuntu18.04+ros melodic） 1、四旋翼无人机跟踪小车或其他 2、跟踪算法用siamcar 3、轨迹评估确认无人机可以跟踪小车后，可做三个场景： 1.、小车直线运动无人机跟踪，在rviz上显示小车和无人机的运动轨迹（同时保存小车和无人机运动过程中的坐标位置） 2、小车转圈 3、两辆小车（一模一样的），一辆不动，另一辆作直线运动然后无人机跟踪并经过第一辆的旁边，显示轨迹，保存坐标。提供：源码及技术文档

在Gazebo里玩点刺激的：用SiamCar算法让无人机追车实录

最近在ROS Melodic下折腾了个活儿——让无人机追着小车跑，用的SiamCar目标跟踪算法，顺手做了轨迹对比分析。整个过程踩坑无数，但最终效果还挺有意思，下面直接上干货。

0. 先搞个能跑的环境

系统选Ubuntu 18.04 + ROS Melodic，Gazebo版本建议用自带的9.0。无人机模型用的PIXHAWK标准四旋翼，小车直接扒了Gazebo的TurtleBot3模型改的。关键是要让两者在同一个世界文件里出现：

<!-- 世界文件片段：加载无人机和小车 --> <include file="$(find px4)/launch/single_vehicle_spawn.launch"> <arg name="vehicle" value="iris"/> </include> <include file="$(find turtlebot3_gazebo)/launch/spawn_turtlebot3.launch"> <arg name="x" value="5.0"/> <!-- 初始位置错开 --> </include>

1. SiamCar算法塞进ROS节点

SiamCar的PyTorch预训练模型直接拿来用（别问，问就是GitHub偷的）。核心代码是个图像处理节点，订阅摄像头话题：

#!/usr/bin/env python3 import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge import torch class SiamCarNode: def __init__(self): self.model = torch.jit.load('siamcar.pt') # 加载TorchScript模型 self.bridge = CvBridge() rospy.Subscriber('/camera/image_raw', Image, self.img_cb) def img_cb(self, msg): cv_img = self.bridge.imgmsg_to_cv2(msg, 'bgr8') processed_img = cv2.GaussianBlur(cv_img, (5,5), 0) bbox = self.model(torch.from_numpy(processed_img).permute(2,0,1)) # 把目标框中心坐标发出去 pub.publish(bbox.center_x, bbox.center_y) if __name__ == '__main__': rospy.init_node('siamcar_tracker') SiamCarNode() rospy.spin()

代码里为什么用TorchScript？因为原生PyTorch模型在ROS里容易卡IO，转成脚本格式速度快了至少30%。

2. 控制逻辑：别让无人机撞墙

拿到目标坐标后，用PID控制无人机位置。重点在于Z轴高度要锁死，不然Gazebo的空气动力学模型分分钟教你做人：

# 控制节点核心片段 from geometry_msgs.msg import Twist def control_loop(target_x, target_y): current_x = drone_pose.x # 从odom话题获取 error_x = target_x - current_x # 只动X和Yaw，高度固定 cmd_vel = Twist() cmd_vel.linear.x = kp * error_x + kd * (error_x - last_error) cmd_vel.angular.z = calculate_yaw(error_x) # 根据X偏差算转向 cmd_pub.publish(cmd_vel)

这里有个骚操作：当目标丢失超过2秒时，让无人机原地悬停并开启图像增强（比如拉高对比度），实测找回目标的概率提升了60%。

3. 轨迹记录与评估

写了个数据记录节点，同时订阅无人机和小车的odom话题：

# 数据记录节点 import csv from nav_msgs.msg import Odometry def odom_callback(msg, role): with open('traj.csv', 'a') as f: writer = csv.writer(f) writer.writerow([ rospy.get_time(), role, msg.pose.pose.position.x, msg.pose.pose.position.y ]) rospy.Subscriber('/uav/odom', Odometry, odom_callback, 'drone') rospy.Subscriber('/car/odom', Odometry, odom_callback, 'car')

跑完实验后用Python画轨迹对比图：

# 画图脚本 import pandas as pd import matplotlib.pyplot as plt df = pd.read_csv('traj.csv') plt.figure(figsize=(10,6)) for role in ['drone', 'car']: d = df[df['role']==role] plt.plot(d.x, d.y, label=role, linewidth=2 if role=='drone' else 1) plt.legend() plt.savefig('轨迹对比.png', dpi=300)