鸽了一年,本专栏终于有空更新了QAQ(其实就是懒)


汇总



0. 前言


首先说明一下本文采用的无人机是 Parrot 家的 Bebop 2 无人机,通过Parrot官方的SDK接口,可以和ROS进行很好的通讯,可以直接控制无人机XY平面的加速度以及Z方向的速度。

由于我现在已经毕业了,没有时间和条件再重新做一次仿真,就将之前实物实验的过程总结一下分享给大家,可以根据本系的列前两节仿真环境搭建的说明进行尝试。


1. 硬件准备


本节针对Bebop 2无人机实物实验的准备工作加以说明,若使用仿真环境,可以直接跳到下一小节:2.物体跟踪


在本专栏前两篇文章中,Bebop无人机已经可以在仿真环境下实现简单的运动指令的相应,可以前后左右方向加速度的控制以及上下方向速度的控制。

参考本专栏第二篇文章 无人机目标跟踪与运动控制②——运动指令和视觉图像 中的第一节:安装 bebop_autonomy 包,将驱动和ROS程序包装上。bebop_autonomy 是基于Parrot公司的官方程序包ARDroneSDK3开发的一个ROS程序包,用于对Bebop系列无人机进行通信和控制。

此时启动无人机,此时不要打开遥控器,pc连接至无人机的WiFi,然后就可以和第二篇仿真环境那样,运行launch启动文件,通过指令控制无人机的运动了。


1.1 无人机连接路由器

上面说了,每次控制bebop的时候都需要连接其网络,此时既不能控制其他机器人,也不能控制多架无人机了,此时我们可以将无人机连接到我们的wifi网络中进行配置,具体操作如下:

  • 安装 android-tools-adb: sudo apt-get install android-tools-adb
  • 下载git文件:git clone https://github.com/guozhenglong/multiple_bebops.git
  • 修改multiple_bebops目录下的 shortpress_3.sh 文件,输入 WIFI 账号和密码:

  • 按动一下电源开关使无人机开机,然后用USB数据线将其与电脑连接

  • 连接后,再连续按动电源4下,此时无人机会发出响声,这样电脑与Bebop 2内置模块便实现有线连接了,此时还可以访问无人机的储存空间

  • 在电脑上通过终端使用adb连接:adb connect 192.168.43.1:9050

  • 在 /multiple-bebops 目录下直接运行:./copy_files.sh


1.2 只修改无人机ip

如果只需要修改bebop无人机的默认ip地址,操作方法如下:

adb connect 192.168.43.1:9050
adb shell mount -o remount,rw /
adb shell
cd sbin
nano broadcom_setup.sh
  • 在打开的 broadcom_setup.sh 文件中,找到IFACE IP AP=”192.168.42.1”,修改后面的数字即可,但是不可超过255。

  • 保存并退出,拔掉数据线重启无人机即可。

  • 然后请将launch文件中的ip修改为Bebop的ip。(Bebop这里有两个启动文件,分别是 bebop_node.launch 和 bebop_nodelet.launch,其中 bebop_nodelet.launch 启动文件可以获取图像信息)


修改ip之后别忘了修改launch启动文件。
修改ip适用于使用多架无人机进行编队的场景,这样同一个局域网内各个无人机的地址就不会冲突了。


1.3 连接无人机

修改 WIFI 连接的账号密码之后,按一下开关启动bebop2无人机,然后连按开关按钮3下,无人机会发出哔声吗,灯由闪烁变为长亮。

此时在PC端测试能否ping通无人机的ip地址。

之后便如仿真一样,运行launch启动文件,通过ros指令控制无人机的运动了。


2. 物体跟踪


2.1 实现思路

上一篇文章说了,在运行启动文件

roslaunch bebop_driver bebop_nodelet.launch

之后,只需要通过订阅 /bebop2/image_raw 话题便可以得到摄像头信息,即可做一些图像处理相关的工作。通过订阅无人机摄像头的callback程序中,通过语句

cv_image = self.bridge.imgmsg_to_cv2(data, “bgr8”)

获取订阅的摄像头图像,并赋给 cv_image。接下来,便可以通过 Python 下的 cv_bridge 对图像信息进行处理。

对图像信息进行滤波处理,转换到HSV颜色空间,识别特定颜色,并框出外切矩形,计算外切矩形的中心点在无人机视角的位置,进而控制无人机的运动


2.2 目标跟随

这里仅举一个对特殊颜色物体的识别,并实现根据物体位置的改变进行视角控制跟随的demo

首先分块对代码进行说明:


  • 定义结构元素:
kernel = cv.getStructuringElement(cv.MORPH_RECT, (3, 3))
height, width = cv_image.shape[0:2]
screen_center = width / 2
screen_center_h = height / 2

offset = 50
offset_h = 30
lower_b = (75, 43, 46)
upper_b = (110, 255, 255)

  • 转成HSV颜色空间
hsv_frame = cv.cvtColor(cv_image, cv.COLOR_BGR2HSV)
mask = cv.inRange(hsv_frame, lower_b, upper_b)

  • 进行腐蚀操作
mask2 = cv.morphologyEx(mask, cv.MORPH_OPEN, kernel)            # 开运算去噪
mask3 = cv.morphologyEx(mask2, cv.MORPH_CLOSE, kernel)            # 闭运算去噪
cv.imshow("mask", mask3)

  • 找出面积最大的色块
 _, contours, _ = cv.findContours(mask3, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
maxArea = 0
maxIndex = 0
for i, c in enumerate(contours):
    area = cv.contourArea(c)
    if area > maxArea:
        maxArea = area
        maxIndex = i

  • 绘制图像
# 绘制轮廓
cv.drawContours(cv_image, contours, maxIndex, (255, 255, 0), 2)
# 获取外切矩形
x, y, w, h = cv.boundingRect(contours[maxIndex])
cv.rectangle(cv_image, (x, y), (x + w, y + h), (255, 0, 0), 2)

  • 获取中心像素点位置
center_x = int(x + w / 2)
center_y = int(y + h / 2)
cv.circle(cv_image, (center_x, center_y), 5, (0, 0, 255), -1)

  • 显示图像
cv.imshow("Image", cv_image)

此时的效果图为:


  • 接下来是根据色块中心点的位置,将二维平面进行分块,对不同区域实行不同的速度指令(最简单最容易理解的方法,如果想让整个运动更加平滑,建议加入PID控制,后续篇章会介绍一下使用PID控制Bebop无人机的轨迹跟踪,因此本章就不过多介绍了)
twist = Twist()
# 左右转向和移动
if center_x < screen_center - offset:
    twist.linear.x = 0.0
    twist.linear.y = 0.2
    twist.angular.z = 0.2
    print "turn left"
elif screen_center - offset <= center_x <= screen_center + offset:
    twist.linear.x = 0.0
    twist.linear.y = 0.0
    twist.angular.z = 0
    print "keep"
elif center_x > screen_center + offset:
    twist.linear.x = 0.0
    twist.linear.y = -0.2
    twist.angular.z = -0.2
    print "turn right"
else:
    twist.linear.x = 0
    twist.angular.z = 0
    print "stop"
# 上下移动
if center_y < screen_center_h - offset_h:
    twist.linear.z = 0.2
    print "up up up"
elif screen_center_h - offset_h <= center_y <= screen_center_h + offset_h:
    twist.linear.z = 0
    print "keep"
elif center_y > screen_center_h + offset_h:
    twist.linear.z = -0.2
    print "down down down"
else:
    twist.linear.z = 0
    print "stop"
cv.waitKey(3)
# 发布运动指令
try:
    self.cmd_pub.publish(twist)
except CvBridgeError as e:
    print e

2.3 完整代码

import rospy
import cv2 as cv
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image


class image_converter:
    def __init__(self):
        self.cmd_pub = rospy.Publisher("/bebop/cmd_vel", Twist, queue_size=1)            # 发布运动控制信息
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/bebop/image_raw", Image, self.callback)        # 订阅摄像头信息

    def callback(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")            # 获取订阅的摄像头图像
        except CvBridgeError as e:
            print e

        # 对图像进行处理
        kernel = cv.getStructuringElement(cv.MORPH_RECT, (3, 3))        # 定义结构元素
        height, width = cv_image.shape[0:2]
        screen_center = width / 2
        screen_center_h = height / 2

        offset = 50
        offset_h = 30
        lower_b = (75, 43, 46)
        upper_b = (110, 255, 255)

        hsv_frame = cv.cvtColor(cv_image, cv.COLOR_BGR2HSV)                # 转成HSV颜色空间
        mask = cv.inRange(hsv_frame, lower_b, upper_b)                                
        mask2 = cv.morphologyEx(mask, cv.MORPH_OPEN, kernel)            # 开运算去噪
        mask3 = cv.morphologyEx(mask2, cv.MORPH_CLOSE, kernel)            # 闭运算去噪
        cv.imshow("mask", mask3)

        # 找出面积最大的区域
        _, contours, _ = cv.findContours(mask3, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
        maxArea = 0
        maxIndex = 0
        for i, c in enumerate(contours):
            area = cv.contourArea(c)
            if area > maxArea:
                maxArea = area
                maxIndex = i
        # 绘制轮廓
        cv.drawContours(cv_image, contours, maxIndex, (255, 255, 0), 2)            

        # 获取外切矩形
        x, y, w, h = cv.boundingRect(contours[maxIndex])
        cv.rectangle(cv_image, (x, y), (x + w, y + h), (255, 0, 0), 2)
        # 获取中心像素点
        center_x = int(x + w / 2)
        center_y = int(y + h / 2)
        cv.circle(cv_image, (center_x, center_y), 5, (0, 0, 255), -1)
        # 显示图像
        cv.imshow("Image", cv_image)

        # 运动控制
        twist = Twist()
        # 左右转向和移动
        if center_x < screen_center - offset:
            twist.linear.x = 0.0
            twist.linear.y = 0.2
            twist.angular.z = 0.2
            print "turn left"
        elif screen_center - offset <= center_x <= screen_center + offset:
            twist.linear.x = 0.0
            twist.linear.y = 0.0
            twist.angular.z = 0
            print "keep"
        elif center_x > screen_center + offset:
            twist.linear.x = 0.0
            twist.linear.y = -0.2
            twist.angular.z = -0.2
            print "turn right"
        else:
            twist.linear.x = 0
            twist.angular.z = 0
            print "stop"
        # 上下移动
        if center_y < screen_center_h - offset_h:
            twist.linear.z = 0.2
            print "up up up"
        elif screen_center_h - offset_h <= center_y <= screen_center_h + offset_h:
            twist.linear.z = 0
            print "keep"
        elif center_y > screen_center_h + offset_h:
            twist.linear.z = -0.2
            print "down down down"
        else:
            twist.linear.z = 0
            print "stop"
        cv.waitKey(3)
        # 发布运动指令
        try:
            self.cmd_pub.publish(twist)
        except CvBridgeError as e:
            print e


if __name__ == '__main__':
    try:
        rospy.init_node("cv_bridge_test")
        rospy.loginfo("Starting cv_bridge_test node")
        image_converter()
        rospy.spin()

    except KeyboardInterrupt:
        print "Shutting down cv_bridge_test node."
        cv.destroyAllWindows()

3. 视觉功能拓展

既然可以获取到实时的视频流信息,那么可玩性就很高了。


  • 比如将物体识别改为黑白线条的识别,摄像头向下移动,控制无人机左右移动和转向运动,就可以让无人机实现巡线运动;
  • 另外我之前尝试过在PC端部署YOLOv3算法,用的是 darknet_ros ,由于现在没有条件重新做实验, 只放一张当时实验的图当作结尾吧,而且也只有缩略图了QAQ。 有需要的同学可以参考(我当时参考的是这篇):ROS下实现darknet_ros(YOLO V3)检测,不过要记得将配置文件更改为GPU,不然帧率只有0.2左右,实测2080的卡跑起来帧率大概在45左右。


参考资料


[1] ROS控制多架Bebop 2无人机

[2] https://bebop-autonomy.readthedocs.io/en/latest/installation.htmly

[3] https://github.com/guozhenglong/multiple_bebops