利用OpenCV库实现TB3机器人循线跟踪功能

发表于 | 分类于

膜拜OpenCV库，利用神库手撕TB3的循线跟踪程序，附完整代码！
followbot

人生乃是一面镜子，
从镜子里认识自己，
我要称之为头等大事，
也只是我们追求的目的！

仿真环境搭建

首先搭建我们的仿真环境，编写launch文件加载实验环境。
关于如何编写launch文件，已在《Launch文件编写技巧》一文中详细解释过。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
  #引入空白世界环境
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
    <arg name="world_name" value="$(find followbot)/world/course.world"/>
    #可自行设计标识线形状和颜色，只需将图片插入到course.world中
  </include>

  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  #引入TB3机器人模型
  <arg name="x_pos" default="0.0"/>
  <arg name="y_pos" default="0.0"/>
  <arg name="z_pos" default="0.0"/>

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
  <param name="publish_frequency" type="double" value="30.0" />
  </node>

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model turtlebot3 -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
</launch>

world

实现功能三步走

■ 利用摄像头采集图像并传给OpenCV库。
■ 对图像进行处理并识别标记线中心。
■ 建立控制器对TB3进行运动控制。

采集图像

在ROS中，图像是以senser_msgs/Image类数据类型发布。要想获得图像数据流，需要订阅指定的图像信息话题。这里我们主要关心的是/camera/rgb/image_raw(原始图像)和/camera/rgb/image_raw/compressed(压缩图像)，这里我们采用原始图像数据进行处理。

1
2
3
4
5
6
7
8
9
10
11
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image

def image_callback(msg):
pass

rospy.init_node('follower')
image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, image_callback)
#image_sub = rospy.Subscriber('/camera/rgb/image_raw/compressed',CompressedImage,image_callback)
rospy.spin()

上述程序便构成了一个订阅图像信息的最小代码块。

图像处理

先将ROS中的图像格式转变成OpenCV中的图像格式;为了消除光照等外界干扰对算法稳定性的影响，将RGB图像转变成HSL空间;设置阈值范围对图像进行掩膜操作，提取出标识线的0-1图像;将图像进行分割可使显示的图像信息能够集中到标识线上;我们只关心1/5高处的20行宽的图像信息，故对掩膜图像进行切片操作;对切片后的图像取中心矩并标记标识线的中心。

图像处理环节的代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
def image_callback(self, msg):
     image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
     #image = self.bridge.compressed_imgmsg_to_cv2(msg)

     h, w, d = image.shape
     search_left = 1 * w / 5      
     search_right = 4 * w / 5
     top=1 * h / 5
     dst=image[0:h , search_left:search_right]
     
     hsv = cv2.cvtColor(dst, cv2.COLOR_BGR2HSV)

     lower_yellow = numpy.array([10, 10, 110])
     upper_yellow = numpy.array([255, 255, 250])
     mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

     h, w, d = dst.shape
     search_top = 4 * h / 5
     search_bot = 4 * h / 5 + 20
    
     mask[0:search_top,0:w] = 0
     mask[search_bot:h,0:w] = 0

     M = cv2.moments(mask)
     if M['m00'] > 0:
         cx = int(M['m10'] / M['m00'])
         cy = int(M['m01'] / M['m00'])
         cv2.circle(dst, (cx, cy), 10, (0, 0, 255), -1)

mask

运动控制

建立比例环节的微分运动控制器对TB3进行运动纠偏，实现循线跟踪功能。

1
2
3
4
5
6
7
err = cx - w / 2
      self.twist.linear.x = 0.60
      self.twist.angular.z = -float(err) / 400
      self.cmd_vel_pub.publish(self.twist)
  else:
      self.twist.angular.z=0.1
      self.cmd_vel_pub.publish(self.twist)

经验分享

1.对于利用树莓派实行循线跟踪功能，局域网信号的时延以及固有频率的限制，算法该设计的太过复杂。
2.可通过调节阈值范围及控制器的增益来适应不同实验环境。
3.通过将切片后的掩膜图像信息以及标识中心线后的图像打印出来能够很好的帮助我们对算法进行调参工作。
4.树莓派摄像头传过来的是压缩图像信息，注意对图像格式进行更改。
5.算法设计时需要考虑到TB3机器人的速度限制(-0.26m/s0~0.26m/s),转速限制(-1.82rad/s~1.82rad/s)

附

完整程序源代码：https://github.com/sun-coke/follow-bot

-------------本文结束 感谢阅读-------------
0%