单目纯视觉避障方案——2020中国机器人大赛FIRA避障仿真组决赛代码开源

单目纯视觉避障方案——2020中国机器人大赛FIRA避障仿真组决赛代码开源

特别鸣谢两位吴同学，董同学和曾同学为最终版代码做出的努力

比赛环境介绍

比赛仿真环境地址：https://github.com/zerowind168/SIMUROSOT-ROBOCHALLENGE

此为 2020 年 FIRA SIMUROSOT ROBO CHANLLENGE 中国赛区的比赛环境

操作系统: ubuntu 16.04

ROS版本: Kinectic

赛题要求

该比赛分为两个任务。简单来说，任务一提供了障碍物的位置信息，机器人可以通过一个单目摄像头返回的图像信息进行避障，最终到达对面的球门；任务二则只提供了视觉信息，最终目标仍然是到达对面的球门。

由于机器人采用双轮差动的方式行驶，给赛题的完成增加了不少难度。

比赛思路

最终版本代码的思路主要分为以下几个部分：

1. 场地信息提取
2. 通过计算图片的“质心”得到下一步的行驶方向
3. 解算速度并发送至机器人驱动模块

该方案的出彩点

本方案的核心代码只有一行，且通过路径图像的质点实时导航的方式基本不需要过多的处理。

仍需优化的地方

1. 通过路径图像的质点导航，可能被诱导向错误路径行驶或者导致机器人擦边障碍物;
2. 检测球门的代码条件需要完全看不到视野内的障碍物，可能会导致看到蓝色球门时只能看到一根门柱，最终无法过门撞到柱子;
3. 速度过慢，理论来讲增大速度的同时增加偏转幅度并不会导致避障效果的降低。

代码

# coding=utf-8
#四川大学ros避障代码,纯视觉算法
import rospy
import numpy as np
import math
import time
import cv2 as cv
from robot import Robot
from geometry_msgs.msg import Twist

#主要调整参数,调整转向幅度
k = -0.0086
#调整机器人速度,一般来说速度越快绝对值应该也增大,否则相当于一定程度减小转向幅度
v = 0.5
#erate，腐蚀系数，扩大障碍物及灰色背景的面积，一般来说腐蚀系数越大越安全，但太大可能导致可行的路径被遮挡
erate = 150
#背景灰度总和阀值，可调参数
gary_scale_value = 48000000
#向ros发布命令的周期与频率,rate=1/dt,一般需要和图像处理速度匹配
dt = 0.1
rate = 10

def navigation(bgr_image):
    H, W, C = bgr_image.shape
    image = np.array(bgr_image, dtype= np.uint8)
    hsv_image = cv.cvtColor(image, cv.COLOR_BGR2HSV)
    hsv_image = cv.GaussianBlur(hsv_image, (5, 5),0)
    #场景中颜色hsv上下界值
    lower_hsv_gray = np.array([0,0,46])         #灰色下界
    upper_hsv_gray = np.array([180,43,220])     #灰色上界
    lower_hsv_blue = np.array([100,43,46])      #蓝色下界
    upper_hsv_blue = np.array([155,255,255])    #蓝色上界
    lower_hsv_white = np.array([0,0,221])       #白色下界
    upper_hsv_white = np.array([180,30,255])    #白色上界
    lower_hsv_black = np.array([0,0,0])         #黑色下界
    upper_hsv_black = np.array([180,255,46])    #黑色上界
    print("carema turned on")


    #vigation mask
    mask_gray = cv.inRange(hsv_image,lowerb=lower_hsv_gray,upperb=upper_hsv_gray)#Gray，场景外颜色
    mask_blue = cv.inRange(hsv_image,lowerb=lower_hsv_blue,upperb=upper_hsv_blue)#Blue，对面球门颜色
    mask_white = cv.inRange(hsv_image,lowerb=lower_hsv_white,upperb=upper_hsv_white)#White，白线颜色
    mask_black = cv.inRange(hsv_image,lowerb=lower_hsv_black,upperb=upper_hsv_black) #black，障碍物颜色

    #反色处理，障碍物为黑色，可行路径为白色
    mask_path = ~(mask_black + mask_gray + mask_blue + mask_white)

    #膨胀+腐蚀去除场景中白线,膨胀增加白色,腐蚀增加黑色
    kernel = np.ones((5,5),np.uint8)
    mask_path = cv.erode(mask_path,kernel)
    mask_path = cv.dilate(mask_path,kernel)
    kernel = np.ones((55,55),np.uint8)
    mask_path = cv.dilate(mask_path,kernel)
    mask_path = cv.erode(mask_path,kernel)

    #腐蚀,增加障碍物（黑色）的面积,通过erate调整
    kernel = np.ones ((erate, erate), np.uint8)
    mask_path = cv.erode (mask_path, kernel)

    #moments函数求矩，可供计算空间矩，中心矩，和中心归一化矩
    #求整个可行路径的矩，对于01二值化的图像,m00代表灰度的总和,白色灰度255,黑色灰度是0
    moments_path = cv.moments(mask_path)
    gray_scale_path = moments_path['m00']

    moments_gray = cv.moments(mask_gray)
    gray_scale_gray = moments_gray['m00']

    moments_black = cv.moments(mask_black)
    gray_scale_black = moments_black['m00']

    moments_blue = cv.moments(mask_blue)
    gray_scale_blue = moments_blue['m00']

    #当有可行路径mask_path总灰度>255时，计算可行路径mask_path的质心并解算出w角速度，否则flag_stop=1,trutlebot原地旋转
    flag_stop=0
    if(gray_scale_path > 200):
        #算路径质心的x坐标
        x = int(moments_path['m10']/moments_path['m00'])
        #算图像中心x坐标与路径质心x坐标差值offset
        offset = x-(W/2)
        #根据offset解算出角速度，调整k可以调整偏转程度
        w = k * offset
        print (w)
    else:
        w = math.radians (5)
        flag_stop=1

    #flag_rush用于标志是否能使用向球门冲的函数
    #设置条件可以理解为机器人看到的背景像素足够多，不能看到障碍物，可以看到蓝色球门
    flag_rush = 0
    if(gray_scale_gray>gary_scale_value and gray_scale_black<200 and gray_scale_blue>200):
        x = int (moments_blue['m10'] / moments_blue['m00'])
        # 算图像中心x坐标与球门质心坐标x坐标差值offset
        offset = x - (W / 2)
        # 根据offset解算出角速度，调整k可以调整偏转程度
        w = k * offset
        if (abs (w) < math.radians (1)) :
            flag_rush=1
            print("flag_rush")
    return w,flag_rush,flag_stop

#运动函数，v值自定，w值根据navigation（）解算得出
def run(robot):
    rate_run = rospy.Rate (rate)
    twist = Twist ()
    for i in xrange(0, 9999):
        img = robot.get_image ()
        w, flag_rush, flag_stop = navigation (img)
        twist.linear.x = v
        twist.angular.z = w
        if flag_stop:
            twist.linear.x = 0
        if flag_rush:
            twist.linear.x = 0
            twist.angular.z = 0
            robot.publish_twist (twist)
            break
        robot.publish_twist(twist)
        rate_run.sleep()
    rushDoor(robot)

def rushDoor(robot):
    rate_run = rospy.Rate (rate)
    twist = Twist ()
    for i in xrange (0, 9999) :
        twist.linear.x = 0.5
        twist.angular.z = 0
        robot.publish_twist (twist)
        rate_run.sleep ()

if __name__ == '__main__':
    robot = Robot()
    run(robot)

效果展示