首先把第1次任务描述一下：在不同路况的双线内行走，地图1是直线，地图2是有钝角的折线，地图3是有锐角的折线。地图及要求如下：
对进、出双线的单线区域大疆工程师是要求从左侧进、右侧出，但单线不能出轮子边线（即进的时候时靠左骑着单线进，出的时候时靠右骑着单线出，不是正中骑着单线进出，这样要求实际上是提高了难度）
第一部分：RoboMaster S1线识别模式的分析
要成功完成这个任务，需要较深入的研究S1的线识别模式，对识别到的42位列表数据进行分析。
首先拿最简单的单线来分析：S1视野图像的坐标范围是左上角（0，0）到右下角（1，1），向右横向为x轴正向，向下纵向为y轴正向。视野图地面点的坐标与地面坐标系不是比例对应关系，具体坐标可由以下这个公式计算，y=1-tanθ*h （y为视野图纵向坐标，θ为俯视角（摄像头视线与垂线的夹角），h为摄像头距地面的高度约25cm（即S1前向y=0.75的点距离S1为0.25m））。y轴向距离与地面坐标系距离的对应关系见下图中的网格与卷尺的关系。
（一）单线工况1：单线在S1正中央，云台向下俯视20°时，识别的10个点位置如下图中的红点标记。
（二）双线分析：云台向下俯视20°时，S1分别位于左侧线正上方、双线正中央、双线偏左线位置时分析识别到的点数据分布，如下图。结论：S1位于双线中间时默认识别右线，若一直向左平移S1，识别的都是右侧的线，识别左线的临界点为左侧麦轮内侧与左线相贴（此时不稳定，识别的线在左右线震荡），再向左移出临界点后稳定识别出左线。
（三）双线中含拐角的分析：
1.若拐角为钝角：线的识别与分布基本同直线，只不过前后两部的切线角不一致，按照巡直线的程序可通用钝角拐角的巡线，无需特殊处理。
2.若拐角为锐角：S1在行进的过程中会不断被拐角的线信息所干扰，到拐角附近时识别出的线类型也会在1、2之间震荡，这时具体怎么识别处拐角类型、停车转动等在后面再介绍，这里先看看S1在不同位置时识别出的点的分布以及识别出的线类型。
第二部分：三个任务地图的实现
说明：在理解进出问题上，我在做地图一和地图二的时候，是按照压左线进，压右线出的，不是按照压进出单线靠左、靠右进出的，我的难度是大于任务要求的，考虑到我在做地图三的时候已经按任务要求做了，所以相关的代码和视频也就不改了，贴上来自己做记录也给小伙伴能做参考用。
CodeMaster评分的时候直接按地图三评就可以了。
（一）、纯直线地图
进入的时候是自动从单线识别出双线的，出的时候靠识别标志牌出双线压右线出去。途中以识别出的右线的第一点的x-0.8为error调整PID实现与右线保持一定偏移距离，因左右线平行，从而实现在双线中间行走。代码和实现视频如下图：
（二）、含钝角拐角的地图
进出双线都靠识别标志牌，具体实现同地图一。途中以识别出的右线的第一点的x-0.8为error调整PID实现与右线保持一定偏移距离，因左右线平行，从而实现在双线中间行走。
（三）、含锐角拐角的地图
地图三我采用了两种方法进行了实现，第一种是通过分析S1在不同位置识别出的线路状态，针对性的编写代码，可拓展性差，主要是为了理清思路；第二种是较通用的程序，对行进路况进行了概化分析，进行了函数模块化设计，比较容易扩展到更复杂的双线地图环境。分别介绍如下：
方法一：按行进阶段分步分析如下：
1.stage==0时，靠左压开始单线进出，途中以识别出的单线的第一点的x-0.6为error调整PID实现与单线保持一定左偏移距离。
2.当接近双线后，S1将识别到右侧的线，第一点的x值将大于0.8，以此为标志位将stage赋值为1，开始双线模式。途中以识别出的右线的第一点的x-0.8为error调整PID实现与右线保持一定左偏移距离在双线中央前进，直到右线因拐角而退出视野时，S1识别出左线（自定义了左线判别函数），以此为标志位将stage赋值为2。
3.S1继续向前以x-0.2靠左线行驶，当前甲板距离前面拐角顶点约40cm的时候，S1此时将线路类型识别为2，且识别为右线，以此为标志位判断应该停车旋转了。此时第一点的切线角α1约为-60度，与夹角绝对值大小相等，故S1需向右旋转180+α1，为调整姿态实际取值180+α1-10。将stage赋值为3，开始下一路况行走。
4.S1继续向前以x-0.8靠右线行驶，当接近第二个内锐角拐角时，S1此时将线路类型识别为1，且识别为左线，以此为标志位判断应该停车旋转了。此时第一点的切线角α1约为60度，与夹角绝对值大小相等，故S1需向左旋转180-α1，为调整姿态实际取值180+α1-8。将stage赋值为4，开始下一路况行走。
5.S1继续向前以x-0.8靠右线行驶，当双线快走完时，识别到单线，此时识别的第一点x1<0.6,以此为标志位，将stage赋值为5，开始最后单线部分的路况行走。
6.S1继续向前以x-0.4靠左线行驶，直到走出单线，结束任务。
特别说明：这个方法只是为了研究清楚S1线路识别的过程和标志位的判定方法，虽然可以实现任务功能，但程序效果并不稳定，特别是S1对线路类别2的识别不稳定，所以方法一仅供参考研究。
方法二：在总结了前面两个地图以及地图三方法一的经验教训后，可以把地图三的实现分解为以下几个关键点，
1.识别出是右线，则以x-0.8靠右线行驶；
2.识别出是左线，则以x-0.8靠左线行驶；
3.判断前方是否有拐角；
4.前方有拐角的话：是锐角还是钝角（计算夹角的函数）？钝角的话继续按1、2方案行驶；锐角的话需要判断是左锐角还是右锐角（左右锐角识别函数）；
5.左、右锐角的判停函数，解决什么时候停车的问题；
6.主循环中的锐角处理函数，解决S1在前进中发现锐角、停车、旋转的问题；
7.进出双线的方案同方法一。
说明：把上面几个函数模块做完后，整个程序的稳定性以及可扩展性就很好了。程序可以适应更复杂的线路配置了。所以就以此作为CodeMaster第一次任务的作业提交吧！！
pid_yawRotate = rm_ctrl.PIDCtrl()
list_LineList = RmList()
variable_x1 = 0
variable_x8 = 0
variable_angle1=0
variable_angle8=0
variable_stage = 0
#拐角顶点（y最小）的索引号
def index_of_corner():
global list_LineList
y=list_LineList[4]
m=1
for n in range(9):
if y > list_LineList[8+4*n]:
y = list_LineList[8+4*n]
m=n+2
return m
#拐角顶点在S1左前方时的停车点
def left_corner_stop_point():
global list_LineList
if len(list_LineList)==42:
n=index_of_corner()
x=list_LineList[(4*n-1)]
y=list_LineList[4*n]
if x<0.5 and y>0.55:
return True
else:
return False
else:
return False
#拐角顶点在S1右前方时的停车点，x1是为了防止误判第一个右拐角
def right_corner_stop_point():
global list_LineList
n=index_of_corner()
x=list_LineList[(4*n-1)]
y=list_LineList[4*n]
x1=list_LineList[3]
if x>0.5 and y>0.4 and x1<0.6:
return True
else:
return False
#计算前方线路的夹角
def calc_angle():
global list_LineList
x1=list_LineList[3]
r1=list_LineList[5] #第1点的切线角
x8=list_LineList[31]
r8=list_LineList[33] #第8点的切线角
if x1<x8:
angle=180+r1-r8
else:
if r8>0:
angle=r8-r1-180
else:
angle=r8-r1+180
return angle
def handle_angle():
global variable_x1
global variable_x8
global list_LineList
global pid_yawRotate
global variable_angle1
global variable_angle8
global variable_stage
angle=calc_angle()
if angle<90 and left_corner_stop_point()==True:
chassis_ctrl.stop()
gimbal_ctrl.set_rotate_speed(90)
gimbal_ctrl.rotate_with_degree(rm_define.gimbal_right,(180-angle+10))
chassis_ctrl.move(0)
if angle<90 and right_corner_stop_point()==True:
chassis_ctrl.stop()
gimbal_ctrl.set_rotate_speed(90)
gimbal_ctrl.rotate_with_degree(rm_define.gimbal_left,(180-angle+20))
chassis_ctrl.move(0)
def param_init():
global variable_x1
global list_LineList
global variable_angle1
global variable_angle8
list_LineList=RmList(vision_ctrl.get_line_detection_info())
variable_x1=list_LineList[3]
variable_angle1=list_LineList[13]
variable_angle8=list_LineList[33]
def ctrl_deviation(a=0.8):
global variable_x1
global pid_yawRotate
pid_yawRotate.set_error(variable_x1-a)
gimbal_ctrl.rotate_with_speed(pid_yawRotate.get_output(),0)
def start():
global variable_x1
global variable_x8
global list_LineList
global pid_yawRotate
global variable_angle1
global variable_angle8
global variable_stage
time.sleep(25) #放这里可以运行,bug放在while前不能运行
robot_ctrl.set_mode(rm_define.robot_mode_chassis_follow)
vision_ctrl.enable_detection(rm_define.vision_detection_line)
vision_ctrl.line_follow_color_set(rm_define.line_follow_color_blue)
gimbal_ctrl.rotate_with_degree(rm_define.gimbal_down,20)
chassis_ctrl.set_trans_speed(0.4)
pid_yawRotate.set_ctrl_params(200,0,6)
media_ctrl.record(1)
chassis_ctrl.move(0)
#从左侧进入双线
while variable_stage==0:
param_init()
ctrl_deviation(a=0.6)
if variable_x1>0.8:
variable_stage=1
#双线行走
while variable_stage==1:
param_init()
if variable_x1<0.4:
ctrl_deviation(a=0.2)
elif variable_x1>0.6:
ctrl_deviation(a=0.8)
else:
variable_stage=2
if abs(variable_angle1-variable_angle8)>10: #提高直线段效率
handle_angle()
#从右侧出双线
while variable_stage==2:
param_init()
ctrl_deviation(a=0.4)
if len(list_LineList)<42:
rmexit()
程序中夹角计算函数的原理图
欢迎批评指正共同提高，有问题可微信联系交流，微信号：cumtzd
2
2644
取消
点赞
评论
分享至：
