Gazebo环境下基于ROS和OpenCV的阿克曼小车多车跟随（2025版）
本实验是一个综合实验，包括了车道线识别、ACC自适应巡航功能。
实验室资源方式简介
进入实操前，请确保阿里云账号满足以下条件：
个人账号资源
使用您个人的云资源进行操作，资源归属于个人。
平台仅提供手册参考，不会对资源做任何操作。
确保已完成云工开物300元代金券领取。
已通过实名认证且账户余额≥100元。
本次实验将在您的账号下开通实操所需计算型实例规格族c7a，费用约为：25元（以实验时长2小时预估，具体金额取决于实验完成的时间），需要您通过阿里云云工开物学生专属300元抵扣金兑换本次实操的云资源。
如果您调整了资源规格、使用时长，或执行了本方案以外的操作，可能导致费用发生变化，请以控制台显示的实际价格和最终账单为准。
领取专属权益及创建实验资源
在开始实验之前，请先点击右侧屏幕的“进入实操”再进行后续操作
领取300元高校专属权益优惠券（若已领取请跳过）
实验产生的费用优先使用优惠券，优惠券使用完毕后需您自行承担。
实验步骤
1、服务部署
点击链接，进入部署页面
按弹窗提示进行权限申请。其中【姓名】、【电话】、【邮箱】为必填项，完成填写后点击【确定】
说明请填写您的学校邮箱（.edu）,便于审核
提交申请后将提示
当申请通过后，将会收到短信提示可以进行部署
刷新部署页面，按下图设置【服务实例名称】、【地域】、【实例密码】
服务实例名称：test（可自定义命名）
地域：华东2（上海）
实例密码：Sjtu@520
说明输入实例密码时请注意大小写，请记住您设置的实例名称及对应密码，后续实验过程会用到。
完成填写后点击【下一步：确认订单】
核对实例信息及价格预览，无误请点击【立即创建】
重要领取300元优惠券后，资源应为0元/小时，且会提示【您当前账户的余额充足】！若提示余额不足等，请检查是否正确领取优惠券
创建成功，点击【去列表查看】
查看实例，点击左侧的图标展开目录
选择目录中的【云服务器ECS】
云服务器ECS—实例—远程连接
下拉展开更多登录方式，选择【通过VNC远程连接】
输入实例密码：Sjtu@520（请输入您设置的密码）后回车
点击左侧第3个图标，点击aliyun_ws文件夹
进入aliyun_ws文件夹后，在空白处点击鼠标右键，选择Open in Terminal
2、代码部分
多车跟随程序使用的是raceworld_4cars场景，由raceworld_4cars.launch文件启动。找到aliyun_ws/src/raceworld/launch文件夹中的raceworld_4cars.launch文件并打开。
在launch文件内加载了car1，car2，car3，car4四个组，每个组加载多个控制器，其中各个组中的left_rear_wheel_velocity_controller,right_rear_wheel_velocity_controller,left_front_wheel_velocity_controller, right_front_wheel_velocity_controller,left_steering_hinge_position_controller和right_steering_hinge_position_controller接收ROS消息，分别控制了左右后轮速度，左右前轮速度和转向角度。
<node name="controller_manager" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="left_rear_wheel_velocity_controller right_rear_wheel_velocity_controller left_front_wheel_velocity_controller right_front_wheel_velocity_controller left_steering_hinge_position_controller right_steering_hinge_position_controller joint_state_controller" />在加载控制器代码的下面，还加载了一个名为servo_comannds.py的代码文件。
<nodepkg="raceworld" type="servo_commands.py" name="servo_commands" output="screen" >打开aliyun_ws/src/raceworld/src文件夹中的servo_comannds.py文件我们可以看到，代码中新建了一个名为servo_comannds的节点，他订阅并接收/robot_name(car1，car2，car3，car4)/ackermann_cmd_mux/output消息。
def servo_commands(): rospy.init_node('servo_commands', anonymous=True) robot_name = rospy.get_param('~robot_name') rospy.Subscriber("ackermann_cmd_mux/output", AckermannDriveStamped, set_throttle_steer) # spin() simply keeps python from exiting until this node is stopped rospy.spin() if __name__ == '__main__': try: servo_commands() except rospy.ROSInterruptException: pass在接收到消息后，经过适当的转换，将速度和转向信息发布给之前所述六个控制器。
def set_throttle_steer(data): pub_vel_left_rear_wheel = rospy.Publisher("left_rear_wheel_velocity_controller/command", Float64, queue_size=1) pub_vel_right_rear_wheel = rospy.Publisher("right_rear_wheel_velocity_controller/command", Float64, queue_size=1) pub_vel_left_front_wheel = rospy.Publisher("left_front_wheel_velocity_controller/command", Float64, queue_size=1) pub_vel_right_front_wheel = rospy.Publisher("right_front_wheel_velocity_controller/command", Float64, queue_size=1) pub_pos_left_steering_hinge = rospy.Publisher("left_steering_hinge_position_controller/command", Float64, queue_size=1) pub_pos_right_steering_hinge = rospy.Publisher("right_steering_hinge_position_controller/command", Float64, queue_size=1) throttle = data.drive.speed*28 steer = data.drive.steering_angle pub_vel_left_rear_wheel.publish(throttle) pub_vel_right_rear_wheel.publish(throttle) pub_vel_left_front_wheel.publish(throttle) pub_vel_right_front_wheel.publish(throttle) pub_pos_left_steering_hinge.publish(steer) pub_pos_right_steering_hinge.publish(steer)跟车程序由lane.py和platoon.py文件实现。
其中lane.py实现前车的巡线。在代码中首先新建了lane节点，并且订阅了名为/car1/camera/zed_left/image_rect_color_left的消息。该消息通过Gazebo中的car1小车上的左摄像头获取。在获取到摄像头图像信息后，把它转换为cv2图像数据。此外，还创建了一个/car1/ackermann_cmd_mux/output消息的发布者。
if __name__ == '__main__': try: rospy.init_node('lane', anonymous=True) rospy.Subscriber("/car1/camera/zed_left/image_rect_color_left", Image, camera_callback) rospy.spin() except rospy.ROSInterruptException: passpub = rospy.Publisher("/car1/ackermann_cmd_mux/output", AckermannDriveStamped,queue_size=1) isLeft = None获得的图像信息通过了高斯滤波，HSV图像转换，腐蚀，二值图转换，视角转换，灰度值处理等一系列操作。
#进行高斯滤波 blur = cv2.GaussianBlur(img,(1,1),0) #转换为HSV图像 hsv_img = cv2.cvtColor(blur,cv2.COLOR_BGR2HSV) #cv2.namedWindow("hsv image",0) #cv2.imshow('hsv image', hsv_img) kernel = np.ones((3, 3), dtype=np.uint8) #腐蚀操作 erode_hsv = cv2.erode(hsv_img,kernel, 1) #cv2.namedWindow("erode image",0) #cv2.imshow('erode image', erode_hsv) #转换为二值图，范围内的颜色为白色，其他范围为黑色 inRange_hsv = cv2.inRange(erode_hsv, color_dist['blue']['Lower'], color_dist['blue']['Upper']) #cv2.namedWindow("binary image",0) #cv2.imshow('binary image', inRange_hsv) gray_img = inRange_hsv #利用透视变换矩阵进行图片转换 gray_img = cv2.warpPerspective(gray_img, M, (640, 480), cv2.INTER_LINEAR) #cv2.namedWindow("gray image",0) #cv2.imshow('gray image', gray_img) #二值化 ret, origin_thr = cv2.threshold(gray_img, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) #cv2.namedWindow("origin_thr image",0) #cv2.imshow('origin_thr image', origin_thr)原始图像如下。
腐蚀操作后图像如下。
视角转换后图像如下。
随后根据图像大小设置滑动窗口的参数。
#7个滑动窗口 nwindows=7 #设置每个滑动窗口的高度 window_height=int(binary_warped.shape[0] / nwindows) nonzero=binary_warped.nonzero() nonzeroy=np.array(nonzero[0]) nonzerox=np.array(nonzero[1]) lane_current=lane_base #滑动窗口宽度的一半 margin=100 minpix=25并且在视角转换后的图像中绘制出来。
for window in range(nwindows): #设置滑动窗口坐标点 win_y_low = binary_warped.shape[0] - (window + 1) * window_height win_y_high = binary_warped.shape[0] - window * window_height win_x_low = lane_current - margin win_x_high = lane_current + margin good_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_x_low) & (nonzerox < win_x_high)).nonzero()[0] lane_inds.append(good_inds) img1 = cv2.rectangle(img1, (win_x_low, win_y_low), (win_x_high, win_y_high), (0, 255, 0), 3) if len(good_inds) > minpix: lane_current = int(np.mean(nonzerox[good_inds])) elif window >= 3: break随后绘制车道线，计算出质心坐标后绘制两个定位用的圆形。其中蓝色圆形位于车道线上，绿色圆形使用的是质心的坐标。
for num in range(len(ploty) - 1): cv2.line(img1, (int(plotx[num]), int(ploty[num])), (int(plotx[num + 1]), int(ploty[num + 1])),(0, 0, 255), 8) aP[0] = aimLaneP[0] - math.sin(theta) * (LorR) * roadWidth / 2 aP[1] = aimLaneP[1] - math.cos(theta) * (LorR) * roadWidth / 2 #车道线上绘制蓝色圆 img1 = cv2.circle(img1, (int(aimLaneP[0]), int(aimLaneP[1])), 10, (255, 0, 0), -1) #质心绘制绿色圆 img1 = cv2.circle(img1, (int(aP[0]), int(aP[1])), 10, (0, 255, 0), -1)绘制后的图像如下。
计算目标点的真实坐标。并根据偏差计算转角数据。
# 计算目标点的真实坐标 if lastP[0] > 0.001 and lastP[1] > 0.001: if (((aP[0] - lastP[0]) ** 2 + (aP[1] - lastP[1]) ** 2 > 2500) and Timer < 2): # To avoid the mislead by walkers aP = lastP[:] Timer += 1 else: Timer = 0 lastP = aP[:] steerAngle = k * math.atan(2 * I * aP[0] / (aP[0] * aP[0] + (aP[1] + D) * (aP[1] + D))) #print("steerAngle=", steerAngle) st = steerAngle * 4.0 / 3.1415新建一个AckermannDriveStamped类型数据，给速度和转角信息复制后即可发布。
msg=AckermannDriveStamped(); msg.header.stamp=rospy.Time.now() msg.header.frame_id="base_link" msg.drive.speed=0.2 msg.drive.steering_angle=steerAngle*0.95; print("Speed:",msg.drive.speed) print("Steering_Angle:",msg.drive.steering_angle) pub.publish(msg) print("Message From lan.py Published\n")platoon.py负责实现前后车的跟车。
其中zensha，kousha，frame_id和node_name四个参数是在终端运行rosrun时自行赋值的参数。随后新建后车阿克曼消息发布者和前后车里程计消息的订阅者。
if __name__ == '__main__': zensha = sys.argv[1] #前车名 kousha = sys.argv[2] #后车名 frame_id = sys.argv[3] node_name = sys.argv[4] PI = 3.1415926535 pose_flag0 = False pose_flag1 = False initial_distance = 0 #前后车初始距离 thresh_distance = 0.2 #后车和目标点距离差 target_point = Odometry() #目标点 ugv0_msg = Odometry() #前车里程计消息 ugv1_msg = Odometry() #后车里程计消息 leader_msg_queue = [] #新建Ackermann消息 msg = AckermannDriveStamped() msg.header.frame_id = frame_id msg.drive.acceleration = 1 msg.drive.jerk = 1 msg.drive.steering_angle_velocity = 1 rospy.init_node(node_name) pub = rospy.Publisher("%s/ackermann_cmd_mux/output"%(kousha), AckermannDriveStamped,queue_size=100) # 订阅前车ugv0里程计消息 rospy.Subscriber("/%s/base_pose_ground_truth"%(zensha), Odometry, pose_callback0,queue_size=10) # 订阅后车ugv1里程计消息 rospy.Subscriber("/%s/base_pose_ground_truth"%(kousha), Odometry, pose_callback1,queue_size=10) rate = rospy.Rate(100) while not rospy.is_shutdown(): follow() rate.sleep()当开始接收到前后车里程计消息时，pose_flag=True，开始跟车。
def pose_callback0(msg): global pose_flag0, ugv0_msg #print(msg) pose_flag0 = True ugv0_msg.child_frame_id = msg.child_frame_id ugv0_msg.header = msg.header ugv0_msg.pose = msg.pose ugv0_msg.twist = msg.twist def pose_callback1(msg): global pose_flag1, ugv1_msg #print(msg) pose_flag1 = True ugv1_msg.child_frame_id = msg.child_frame_id ugv1_msg.header = msg.header ugv1_msg.pose = msg.pose ugv1_msg.twist = msg.twist首次运行程序时计算初始距离当作前后车安全距离。将前车里程计消息加入队列作为未来跟车的目标点。当目标距离小于thresh_distance时从队列中取出新的目标点。当目标距离大于5时清楚重开队列。在计算并设置好速度与转角之后，将AckermannDriveStamped类型的消息发布给后车。
def follow(): global PI, pose_flag0, pose_flag1, initial_distance, thresh_distance, target_point, ugv0_msg, ugv1_msg, leader_msg_queue, msg, pub if not pose_flag0 or not pose_flag1: #接收到odometry消息后才开始运行 return leader_msg_queue.append(ugv0_msg) #求目标点当前距离 target_distance = math.sqrt((target_point.pose.pose.position.x - ugv1_msg.pose.pose.position.x) ** 2 + (target_point.pose.pose.position.y - ugv1_msg.pose.pose.position.y) ** 2) #求前后车当前距离 follower_distance = math.sqrt((ugv0_msg.pose.pose.position.x - ugv1_msg.pose.pose.position.x) ** 2 + (ugv0_msg.pose.pose.position.y - ugv1_msg.pose.pose.position.y) ** 2) #print(target_distance, follower_distance, initial_distance) #前后车当前距离小于初始距离时停车 if follower_distance < initial_distance: print("距离太近自动停车") msg.drive.speed = 0; msg.drive.steering_angle = 0; msg.header.stamp = rospy.Time.now() pub.publish(msg); else: #target_point为初始值时直接赋值 if target_point.pose.pose.position.x == 0 and target_point.pose.pose.position.y == 0: target_point = ugv0_msg #求前后车初始距离 initial_distance = math.sqrt((ugv0_msg.pose.pose.position.x - ugv1_msg.pose.pose.position.x) ** 2 + (ugv0_msg.pose.pose.position.y - ugv1_msg.pose.pose.position.y) ** 2) print("两车初始距离: ",initial_distance) #后车接近目标点时按队列方式从数组中取出下一个目标点 if target_distance < thresh_distance: target_point = leader_msg_queue[0] leader_msg_queue = leader_msg_queue[1:] #后车与目标点差距过大时清除并重开队列 elif target_distance > 5: leader_msg_queue = [] target_point = ugv0_msg #四元数转欧拉角 (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([ugv1_msg.pose.pose.orientation.x, ugv1_msg.pose.pose.orientation.y, ugv1_msg.pose.pose.orientation.z, ugv1_msg.pose.pose.orientation.w]) gamma = yaw delta = math.atan2(target_point.pose.pose.position.y-ugv1_msg.pose.pose.position.y,target_point.pose.pose.position.x-ugv1_msg.pose.pose.position.x); if delta < 0: delta += (2 * PI) if gamma < 0: gamma += (2 * PI) theta = delta - gamma if theta > PI: theta -= (2 * PI) if theta < -PI: theta += (2 * PI) r = math.sqrt((ugv0_msg.pose.pose.position.x - ugv1_msg.pose.pose.position.x) ** 2 + (ugv0_msg.pose.pose.position.y - ugv1_msg.pose.pose.position.y) ** 2) k = 0.5 if r > 0.4: msg.drive.speed = 0.3 * r #修改数值调节跟车时后车速度 else: msg.drive.speed = 0.05 msg.drive.steering_angle = k * theta #print("后车速度: ", msg.drive.speed, " 后车角度: ", msg.drive.steering_angle) msg.header.stamp = rospy.Time.now() pub.publish(msg);3、实验操作
在aliyun_ws路径下新建终端，输入命令开启raceworld_4cars Gazebo场景。
source devel/setup.bash roslaunch raceworld raceworld_4cars.launch
在aliyun_ws路径下新建终端，新建终端，输入如下命令启动platoon.py和lane.py。
source devel/setup.bash rosrun raceworld platoon.py "car1" "car2" "base_link2" "follow1"source devel/setup.bash rosrun raceworld platoon.py "car2" "car3" "base_link3" "follow2"source devel/setup.bash rosrun raceworld platoon.py "car3" "car4" "base_link4" "follow3"source devel/setup.bash rosrun raceworld lane.py随后我们可以看到跟车程序的实际运行状况。
在lane.py终端中可以看到打印出来的控制信息。
清理资源
计算巢—服务实例—复制服务实例ID，点击【删除】
在弹窗粘贴服务实例ID，并进行勾选，点击【确定删除】
完成安全验证后，即可成功释放实例。
回到云服务器ECS——实例，检查是否成功释放资源
场景简介
本实验是一个综合实验，包括了车道线识别、ACC自适应巡航功能。
背景知识
学生需要有一定的C++ 基础或者Python基础来完成此实验。
关闭实验
在完成实验后，如果无需继续使用资源，选择不保留资源，单击结束实操。在结束实操对话框中，单击确定。
在完成实验后，如果需要继续使用资源，选择付费保留资源，单击结束实操。在结束实操对话框中，单击确定。请随时关注账户扣费情况，避免发生欠费。
