请选择 进入手机版 | 继续访问电脑版
正在上传图片(0/1)

CoderMaster第三次任务—数学方法巧解(11月6日增加5个bug报告)

 2
手机看帖 1 1832
第三次任务公布很久了,一直忙,最近有些时间,做了一下。场地占用面积太大,只能改变客厅家私的位置,必须速战速决,时间长了,老婆会生气的
都说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的情况。
评论
上传
你需要登录之后才能回帖    登录 | 注册
boolrobot  机甲大师 RoboMaster S1认证用户 2019-10-25 2#
认证设备
取消 点赞 评论
分享至:
回复:
上传
取消 评论
快速回复 返回顶部 返回列表