RoboMaster视觉教程（10）目标预测

目标预测应该是RM视觉中最难也是最重要的部分了，预测直接决定了子弹发射后的命中率，实话实说我预测做得不好。预测之所以难做有很多原因，我觉得最困难的地方在于预测是需要上下位机配合的，也就是妙算/微型电脑与战车上的STM32需要配合起来才可以达到比较好的效果。

我虽然也会写一些嵌入式上的程序对STM32也比较了解，但水平太差。17年比赛的时候基地的自瞄和电控都是我负责的，当时我连基地自转都控制不好比赛时候总撞墙，自瞄做了等于白做。。。

言归正传，虽然我预测做得不怎么样，但是对这方面也还是有点经验可以与大家分享的。

为什么要进行目标预测

首先说一下预测的必要性，为什么要进行目标预测。因为摄像头和微型电脑性能的限制，帧率不会达到很高。

每次摄像头在拍摄到战车的装甲的时候会有8ms左右的延迟（按120fps算）之后再加上处理花费的时间和串口传输的时间，这样从装甲板出现到识别到发出控制指令就已经有十几毫秒过去了。

下位机接收到数据后云台也需要一定的时间才能旋转到指定位置的，当云台旋转到位后此时装甲板早已不在之前出现的位置了。

若敌方车辆不断移动，那自瞄将总是滞后于装甲板的实际位置，若敌方的车辆是小陀螺不断自旋就更难打中了。

预测问题的几种情况及分析

• 摄像头在小车底盘上，小车不动
• 摄像头在小车枪管上，小车不动
• 摄像头在小车底盘上，小车动
• 摄像头在小车枪管上，小车动

第一种情况是最简单的。在第一种情况下摄像头是第三视角，相当于小车坐标系和大地坐标系是等价的。通过识别装甲板得到的位置就是敌方战车装甲板在大地坐标系下的坐标，这样通过相邻两帧间的时间差和位置差就可以得到装甲板移动的速度，进而通过速度乘以时间即可得到预测量进行预测补偿。

第二种情况下摄像头是第一视角，摄像头会随着云台的运动而运动，此时测得的装甲板位置是相对于摄像头的也就是相对于枪管的，若想预测则需要知道枪管当前偏转的角度进而进行坐标换算，把装甲板位置换算得出的转角加上枪管当前的转角就能得到物体实际的转角，进而可以计算角速度再得到预测量进行补偿。

第三和第四种情况下小车是动的，这就意味着小车坐标系和大地坐标系不再重合，如果想要得到准确的预测结果就需要规定一个原点，在此基础上测出小车的位移，再将其与装甲板位置及枪管的转角进行叠加转换得到大地坐标系下的位置进而进行补偿。

怎样做预测

从上面的分析中我们可以看出，预测最重要的就是确定敌方装甲板在大地坐标系（可以用直角坐标系也可以是极坐标系）下的位置。

而在现实中小车往往是在运动的，惯性导航又很难做，所以可以退而求其次用小车坐标系近似大地坐标系。因为在实战中大多数情况是停下来打的，而且当小车运动速度不快时造成的误差也不会太大。

一般来说大家都会把摄像头装在枪管上，这样得到的装甲板位置就是以摄像头所在位置为原点在摄像头坐标系下的位置。

首先需要将该位置坐标的z轴和y轴进行偏移（假设摄像头的是放在枪管上面且放在正中间，则x轴正好在枪管的中间处不需要偏移）。

偏移后就得到了以云台为原点的坐标，再将此时装甲板的坐标通过反三角函数转换成角度（把装甲板位置换算成角度比把枪管转角换算成三维坐标容易），之后把装甲板的角度和枪管的角度相加就能得到了在小车坐标系下的角度。

用这个角度通过卡尔曼滤波或者简单的两帧相减就能得到角速度，进而将其乘以预测时间得到预测量进行补偿。

如果觉得预测难做其实不做预测也是可以的只是效果差些，在妙算上直接将得到的相对于摄像头的装甲板偏转角作为误差传给pid进行控制（pid本自身就有预测作用），可以实现在目标运动时速度无静差而位置有静差而在目标停下时位置静差会消除的跟踪瞄准效果，而且运动时的静差会随着摄像头帧率的提高而减小，在使用330fps的摄像头时若识别到装甲板不会跟丢且误差在一个装甲板内。

如果进行预测，则预测在STM32上做比较好。上位机命令的发送频率毕竟比下位机的控制频率低，可以通过串口把每次识别到装甲板后换算得到的转角发送给下位机，然后下位机每个控制周期对装甲板位置进行预测后再控制云台的旋转，可以得到比较好的结果。

关于预测，大疆官方有一篇知乎可以看看 https://zhuanlan.zhihu.com/p/38745950

这一篇没有代码只讲了思路，因为我自己也没有做得很好，今年分区赛前没有做预测，因为把摄像头得到的相对位置当成了速度来做预测（云台跟随时每次得到转角可以看成是相对速度，但是若云台转过后会马上变为负从而会抵消之前的预测效果），一直没有效果（被自己蠢哭），到最后又因为电控不给力无法得到枪管的pitch和yaw转角（临近考试周又还有好几辆车的程序要写，所以电控就没时间来做这一块了）就没有做预测。