单目纯视觉避障方案——2020中国机器人大赛FIRA避障仿真组决赛代码开源
特别鸣谢两位吴同学,董同学和曾同学为最终版代码做出的努力
比赛环境介绍
比赛仿真环境地址:https://github.com/zerowind168/SIMUROSOT-ROBOCHALLENGE
此为 2020 年 FIRA SIMUROSOT ROBO CHANLLENGE 中国赛区的比赛环境
操作系统: ubuntu 16.04
ROS版本: Kinectic
赛题要求
该比赛分为两个任务。简单来说,任务一提供了障碍物的位置信息,机器人可以通过一个单目摄像头返回的图像信息进行避障,最终到达对面的球门;任务二则只提供了视觉信息,最终目标仍然是到达对面的球门。
由于机器人采用双轮差动的方式行驶,给赛题的完成增加了不少难度。
比赛思路
最终版本代码的思路主要分为以下几个部分:
- 场地信息提取
- 通过计算图片的“质心”得到下一步的行驶方向
- 解算速度并发送至机器人驱动模块
该方案的出彩点
本方案的核心代码只有一行,且通过路径图像的质点实时导航的方式基本不需要过多的处理。
仍需优化的地方
- 通过路径图像的质点导航,可能被诱导向错误路径行驶或者导致机器人擦边障碍物;
- 检测球门的代码条件需要完全看不到视野内的障碍物,可能会导致看到蓝色球门时只能看到一根门柱,最终无法过门撞到柱子;
- 速度过慢,理论来讲增大速度的同时增加偏转幅度并不会导致避障效果的降低。
代码
# 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)
效果展示
2020中国机器人大赛FIRA避障仿真组算法alpha版本效果展示