第三次任务公布很久了，一直忙，最近有些时间，做了一下。场地占用面积太大，只能改变客厅家私的位置，必须速战速决，时间长了，老婆会生气的
都说STEM（抱歉，喜欢说STEAM的同学，我不认可A能与STEM相提并论），窃以为，数学才是STEM的老大。本次任务，我用数学方法解决，地上的线都是摆设，仅仅用于验证有没有犯规而已，全程没有用到巡线。我不会调PID，超级不喜欢巡线，巡线有一个很大的弱点，就是找不到北
官方原始地图让我想起成都，街道横平竖直，但都是东南-西北和西南-东北走向的，要是在成都问路，人家告诉你要朝西北走，第二个路口拐向东北，你作何感想:D
哎，想吃成都的美食了。咳咳，扯远了
预备知识：
数学：平面直角坐标系，坐标旋转，直线方程，三角函数
S1编程：全向移动，大地坐标系（大师之路全向移动一节）
首先，关于规则，需要认真地，用数学思想解读一番
将原地图顺时针旋转45度，以起点为原点，起点到marker2的连线为x轴，建立平面直角坐标系，还是北京的街道比较舒服哈
说明一下，S1的坐标跟上图这个数学上常用的坐标不同，为了方便，先用数学常用的坐标系分析，实现的时候，交换x，y即可
规则规定，可以记录各marker到起点的方位角，那么这些方位角都是什么意思呢？有什么用呢？
在这样一张图里面，很容易得到各点坐标信息，这些信息不能出现在程序里面，但是可以帮助我们分析
marker1(0,1.5), marker2(1.5,0), marker3(1.5,2.5), marker4(2.5,1.5), s(0.5,0.5), k(1.5,1.5), t(2,2)
S1的初始航向为x轴方向，这样marker2的方位角就是0度，其坐标应该是(x2,0)，x2应该理解为不知道，但是纵坐标是0是确定的
同理，可以得到marker1的方位角是90度，其坐标是(0,y2)
marker3在直线l1上，l1的方程是y=(2.5/1.5)x，即y=5x/3，这个5/3应该是已知条件，它是marker2方位角的正切
同理，marker4在直线l2上，l2的方程是y=(1.5/2.5)x，即y=3x/5，这个3/5应该是已知条件
同理，终点在直线y=x上，至于两个陷阱，直接无视
如何找到marker1？我们需要到达g1点，题目要求视觉识别距离为0.5m，那么，我们期望g1点的坐标是(0.5,y1)，注意，只是期望，g1不一定就在那个交点上，允许有半个底盘宽的误差（这已经很奢侈了）
要到达g1点，我们需要找一个前进基地，那就是s点，坐标(0.5,0.5)，同样，它不一定就那个交点上
于是，程序的第一步，就是前进到s点，用move_with_degree_distance函数，向45度方向前进0.5*√2，大约0.7m。请注意，这里的45度和0.7m，都与地图数据没有任何关系，完全是计算出来的，计算的原始数据，就是marker1的方位角，以及识别距离为0.5m
前进到s点之后，航向0，云台航向角-90度，即图中的向下，搜索前进，必然会发现marker2。这是因为marker2的方位角为0，坐标是(0,x2), x2未知，那么S1在直线y=0.5上行走，必然会到达g2点
这里的航向0，云台航向角90度，是根据marker2的方位角是0，进而x2的纵坐标是0得到的，不属于地图数据
实际的程序，要考虑到东西比较多，而且后面还要用，我就写了一个函数
gimbal_yaw是云台航向角，direction是底盘前进方向，speed是行进速度
def position_y(gimbal_yaw,direction,speed):
global marker
gimbal_ctrl.yaw_ctrl(gimbal_yaw)
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.move_degree_with_speed(speed,direction)
while marker[1] < 1:
marker = RmList(vision_ctrl.get_marker_detection_info())
while abs(marker[3]-0.5) > 0.05:
chassis_ctrl.move_degree_with_speed(0.1,direction)
marker = RmList(vision_ctrl.get_marker_detection_info())
while marker[1] < 1:
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.stop()
云台航向角与底盘前进方向成90度夹角，具体是什么样的方向由参数决定，行进过程中，会发现标签，发现之后转龟速，直到标签位于视野正中
调用position_y函数，停在g2点，记录下此时S1的坐标（x2,0.5）,如何记录？参见大师之路的全向移动，参考写法是
x2 = chassis_ctrl.get_position_based_power_on(rm_define.chassis_forward)
找到g2点，开火打marker2，收宝箱:D
下面要找marker1，g2点对marker1没有什么用处，退回s点
根据刚才得到的x2，航向180度，走x2-0.5，即到s点，实际的程序故意多退了一点点，因为S1底盘是长方形...
然后如法炮制，航向90度，云台航向角180度，搜索前进，到达g1点，记录g1点坐标(0.5,y1)，开火打marker1，收宝箱。记录y1的参考写法是
y1 = chassis_ctrl.get_position_based_power_on(rm_define.chassis_translation)
接下来，s点不能对找marker3和marker4提供任何帮助了，我们需要一个新的前进基地，那就是k点，坐标(x2，y1)，注意，x2和y1是我辛辛苦苦得到的，不是预设值哈
从(0,5,y1)到(x2,y1)，航向和距离都可以算出来
到这里，上半场结束
插播一个很离奇的bug
python代码：chassis_ctrl.move_with_distance(30, 1)
scrach代码（懒得贴图了，就这样写吧，大家应该看得懂）：控制底盘向30度平移1米
就这么简单的一句话，大家猜猜是什么情况？是不是应该朝右前方30度前进1米？
事实上，类似的这条语句
chassis_ctrl.move_with_time(30, 1) （scratch语句是 控制底盘向30度平移1秒 ）
确实是朝右前方30度前进了1秒。
然而，chassis_ctrl.move_with_distance(30, 1)的行为如下图的红色箭头（绿色箭头是期望行为）
绿色是期望路线，向30度方向前进。红色是实际路线，先向45度方向前进一段，然后回0度继续前进
我又尝试了各种角度，仅仅只有0度，正负45度，正负90度，正负135度和正负180度是期望行为。
下图的红色箭头是chassis_ctrl.move_with_distance(60, 1)的实际效果
绿色是期望路线，向60方向前进。红色是实际路线，先向45度方向前进一段距离，然后再向90度方向平移
因为chassis_ctrl.move_with_time函数给出了我们期望的行为，那就可以排除官方故意定义成先往45度方向前进的可能，这就是一个bug了
插播结束，下半场我们来搞定marker3和marker4
由题意，见上图，marker3和marker4一定在那两条黑色虚线上，怎么找呢？
这里，不好意思，规则的漏洞要利用一下，或者说S1的不完美让我找到了解决问题的简单方法。
宝箱是用视觉标签实现的，而视觉标签是一个平面物事。什么意思呢？你只能在正面识别到标签，反面是识别不到的，侧面就更没戏了。
那就是说，如果视觉标签出现在场地中间部分，那就存在你走到跟前都视而不见的情况，那这个任务就跟你的路线强相关了，然而任务的已知条件并不包括标签的朝向，你是两眼一抹黑的，你找到一条正确的路线完全就是大海捞针，这不科学:D 除非宝箱是一个立体图形，S1从各个方向都可以识别到它，可惜，大疆没有把这个功能做出来。官方的锅咱就不背了哈:D
在k点，云台航向角0度的时候，摄像头的水平方向视角是96度（没说S1自己啥参数都不能知道吧），上图红色虚线度视角才90度，这个视角（在工程意义上）覆盖marker4的解空间。换言之，航向0度前进，必然发现marker4，至于是不是在正中不重要，发现之后，自动瞄准开火就可以了。同理，云台航向角90度的时候，航向90度前进必然发现marker3
这个视角没有覆盖的地方怎么办？由于S1在识别标签上的固有缺陷，marker不可能在直线y=-x+3下方，这就足够了。换个场景，如果你是航母舰队的司令官，有敌舰队的这些信息，你会毫不犹豫地下达攻击命令
这里需要一个前进发现标签的功能，在大概距离为0.5（实际程序略小一些）的地方停下来，我写了一个函数
gimbal_yaw是云台航向角，direction 是前进方向，speed是行进速度，t是标签高度的阈值
def position_x(gimbal_yaw,direction,speed,t):
global marker
gimbal_ctrl.yaw_ctrl(gimbal_yaw)
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.move_degree_with_speed(speed,direction)
while marker[1] < 1:
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.move_degree_with_speed(0.15,direction)
while marker[6] < t:
marker = RmList(vision_ctrl.get_marker_detection_info())
while marker[1] < 1:
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.stop()
前进，发现标签之后降为龟速，停在标签高度为t的地方，t由调试决定
具体路线是，航向0度，云台航向角0度，前进发现marker4，即走到g4点，记录此时的横坐标xt，开火收marker4
回到k点，航向90度，云台航向角90度，前进发现marker3，即走到g3点，记录此时点纵坐标yt，开火收marker3
根据xt和yt，走到坐标为(xt,yt)的t点，然后朝着终点的方位角前进，多少距离就无所谓了，就当是冲线吧
这里我使用了什么地图信息了吗？0度和90度都是分析出来的，是一个绰绰有余的估计值，标签的高度阈值是调试得到的，跟地图没关系，然后，xt，yt是S1在路上测量出来的。
下面是视频
S1的初始方向，如果按官方给的地图那样看，是斜45度放置，这样在路上就可以做到全是0度和90度的移动
因为坐标系定义不同，实际的效果跟我上传的地图不一样，是关于y=x的镜像，原理是一样的
识别到最后一个标签之后，后退了一点点，这是一个补丁，因为停下来的位置离标签近了一点点，大家选择性忽略就好，场地已拆，再搭一遍很麻烦的，请大家谅解
完整的程序如下：
global marker
marker = RmList()
global sight
sight = RmList()
def start():
global marker
init()
chassis_ctrl.set_trans_speed(0.5)
chassis_ctrl.move_with_distance(45,0.7)
time.sleep(0.5)
xs = chassis_ctrl.get_position_based_power_on(rm_define.chassis_forward)
ys = chassis_ctrl.get_position_based_power_on(rm_define.chassis_translation)
position_y(-90,0,0.3)
xk = chassis_ctrl.get_position_based_power_on(rm_define.chassis_forward)
fire()
chassis_ctrl.set_trans_speed(0.5)
chassis_ctrl.move_with_distance(180,xk-xs+0.05)
position_y(-180,90,0.3)
yk = chassis_ctrl.get_position_based_power_on(rm_define.chassis_translation)
fire()
chassis_ctrl.set_trans_speed(0.5)
chassis_ctrl.move_with_distance(0,xk-ys)
xk = chassis_ctrl.get_position_based_power_on(rm_define.chassis_forward)
yk = chassis_ctrl.get_position_based_power_on(rm_define.chassis_translation)
position_x(0,0,0.3,0.22)
xt = chassis_ctrl.get_position_based_power_on(rm_define.chassis_forward)
fire()
chassis_ctrl.set_trans_speed(0.5)
chassis_ctrl.move_with_distance(180,xt-xk)
position_x(90,90,0.3,0.22)
yt = chassis_ctrl.get_position_based_power_on(rm_define.chassis_translation)
fire()
chassis_ctrl.set_trans_speed(0.5)
chassis_ctrl.move_with_distance(-90,0.15)
chassis_ctrl.move_with_distance(0,xt-xk)
chassis_ctrl.move_with_distance(45,0.7)
media_ctrl.play_sound(rm_define.media_sound_recognize_success)
def init():
global sight
robot_ctrl.set_mode(rm_define.robot_mode_free)
gimbal_ctrl.set_rotate_speed(270)
vision_ctrl.enable_detection(rm_define.vision_detection_marker)
vision_ctrl.set_marker_detection_distance(0.5)
gun_ctrl.set_fire_count(1)
sight = RmList(media_ctrl.get_sight_bead_position())
gimbal_ctrl.pitch_ctrl(-15)
def position_x(gimbal_yaw,direction,speed,t):
global marker
gimbal_ctrl.yaw_ctrl(gimbal_yaw)
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.move_degree_with_speed(speed,direction)
while marker[1] < 1:
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.move_degree_with_speed(0.15,direction)
while marker[6] < t:
marker = RmList(vision_ctrl.get_marker_detection_info())
while marker[1] < 1:
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.stop()
def position_y(gimbal_yaw,direction,speed):
global marker
gimbal_ctrl.yaw_ctrl(gimbal_yaw)
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.move_degree_with_speed(speed,direction)
while marker[1] < 1:
marker = RmList(vision_ctrl.get_marker_detection_info())
while abs(marker[3]-0.5) > 0.05:
chassis_ctrl.move_degree_with_speed(0.1,direction)
marker = RmList(vision_ctrl.get_marker_detection_info())
while marker[1] < 1:
marker = RmList(vision_ctrl.get_marker_detection_info())
chassis_ctrl.stop()
def fire():
global sight
global marker
x0 = gimbal_ctrl.get_axis_angle(rm_define.gimbal_axis_yaw)
y0 = gimbal_ctrl.get_axis_angle(rm_define.gimbal_axis_pitch)
x = 96 * ( marker[3] - sight[1] ) + x0
y = 54 * ( sight[2] - marker[4] ) + y0
gimbal_ctrl.angle_ctrl(x,y)
led_ctrl.gun_led_on()
gun_ctrl.fire_once()
time.sleep(1)
led_ctrl.gun_led_off()
gimbal_ctrl.angle_ctrl(x0,y0)
最后再发一些我发现的bug
最近对S1的云台跟随底盘和底盘跟随云台两种模式很感兴趣，做了一些实验，发现了一些异常
按理说，DJI怎么定义这些行为都可以，但至少应该能够自圆其说吧，我发现的一些事情完全是矛盾的，只能认为是bug了。
一共有5个，云台跟随底盘1个，底盘跟随云台4个，先上视频，视频中有scratch代码
Bug1:
云台跟随底盘模式，设置云台以90度跟随底盘
def start():
chassis_ctrl.set_trans_speed(1)
gimbal_ctrl.set_rotate_speed(540)
robot_ctrl.set_mode(rm_define.robot_mode_gimbal_follow)
gimbal_ctrl.set_follow_chassis_offset(90)
chassis_ctrl.move_with_distance(0,1)
chassis_ctrl.stop()
while True:
pass
代码很简单，就是想让云台与底盘的夹角等于90度，然而，云台实际的航向角是71度，差得也太远了吧
这一段代码还告诉我们一个事实，在底盘平移结束之后，云台与底盘之间的相对状态应该不变。最后那个死循环就是想让S1还处在程序控制之下，而不是退回默认的底盘跟随云台模式。
然而，bug2描述的现象跟这个事实矛盾
这个bug并不是不能解决，在设置成云台跟随底盘模式之前，加上这两句话
robot_ctrl.set_mode(rm_define.robot_mode_free)
gimbal_ctrl.yaw_ctrl(90)
就可以达到预期效果，云台航向角大概是89.4度（这种情况没有录视频）。因为这一改，又导致了新问题，即下面的bug4和bug5
Bug2:
底盘跟随云台模式，设置底盘以90度跟随云台，然后向0度方向平移1秒
def start():
chassis_ctrl.set_trans_speed(1)
gimbal_ctrl.set_rotate_speed(540)
robot_ctrl.set_mode(rm_define.robot_mode_chassis_follow)
gimbal_ctrl.set_follow_chassis_offset(90)
chassis_ctrl.move_with_time(0,1)
chassis_ctrl.stop()
while True:
pass
S1先转动底盘与云台成90度夹角，然后向（新的）0度平移，这是预期的动作，然而，平移完成之后，又转动底盘是怎么回事呢？明明应该保持90度夹角啊，我没有写语句让它归位啊。这与上一段代码描述的行为是矛盾的
Bug3:
底盘跟随云台模式，设置底盘以90度跟随云台，然后向0度方向平移1米
仅仅是把1秒改成了1米，而程序已开始设置了平移速度为1米/秒，所以可以预期S1的行为应该与上一段代码大致相同
def start():
chassis_ctrl.set_trans_speed(1)
gimbal_ctrl.set_rotate_speed(540)
robot_ctrl.set_mode(rm_define.robot_mode_chassis_follow)
gimbal_ctrl.set_follow_chassis_offset(90)
chassis_ctrl.move_with_distance(0,1) #就改了这一句
chassis_ctrl.stop()
while True:
pass
可是，S1的行为让人大跌眼镜，它直接往前平移了1米，直接无视底盘以90度跟随云台的语句
Bug4:
在上一段代码的基础上，预先将云台转90度，然后再设置底盘以90度跟随云台。根据bug1改进的经验，期望能够实现我们的企图
def start():
chassis_ctrl.set_trans_speed(1)
gimbal_ctrl.set_rotate_speed(540)
robot_ctrl.set_mode(rm_define.robot_mode_free) #加上这一句和下一句
gimbal_ctrl.yaw_ctrl(90)
robot_ctrl.set_mode(rm_define.robot_mode_chassis_follow)
gimbal_ctrl.set_follow_chassis_offset(90)
chassis_ctrl.move_with_distance(0,1)
chassis_ctrl.stop()
while True:
pass
实际情况是云台转90度OK，在切换模式的时候，S1明显抖了一下，这不科学啊。如果在比赛中，这个动作会产生航向角的偏差，导致较大的走位误差
而且，平移动作完成之后，底盘又转了90度，为什么没有保持与云台的90度夹角呢？
Bug5:
在上一段代码的基础上，把平移1米改成平移1秒，既然bug2和bug3的行为有这么大的差异，我对改成1秒之后的行为很感兴趣
def start():
chassis_ctrl.set_trans_speed(1)
gimbal_ctrl.set_rotate_speed(540)
robot_ctrl.set_mode(rm_define.robot_mode_free)
gimbal_ctrl.yaw_ctrl(90)
robot_ctrl.set_mode(rm_define.robot_mode_chassis_follow)
gimbal_ctrl.set_follow_chassis_offset(90)
chassis_ctrl.move_with_time(0,1) #只改了这一句
chassis_ctrl.stop()
while True:
pass
这回的行为与bug4完全不一样，我也完全看不懂了。其实我期望的行为就是基本像bug4那样，再改进一下让切换模式的时候不要抖，平移完了之后保持云台与底盘的相对位置
联想到帖子中场休息那个bug，走时间和走距离行为差别很大，加上云台以90度跟随底盘或者底盘以90度跟随云台模式之后，花样就更多了一些。而且各种行为之间自相矛盾
建议，设置云台跟随底盘和底盘跟随云台模式的时候，同时带上offset参数，默认是0，不要用另一个函数来设置offset，这样应该可以避免bug4的情况。
1
3164
取消
点赞
评论
分享至：
