【RM2019-全方位开源】哈尔滨工程大学

慢慢开坑，不定时更新。本人2019年创梦之翼队长以及电控组长（因为某些人员问题，我自己全责）以及研发组长。现战队技术指导，编外人员，就不挂队旗了。

水平有限，很可能有错误，欢迎大家多多指教！多交流！

0.前言

                               
登录/注册后可看帖子

重点：
(0)属实觉得论坛这个写长帖子的编辑方式不方便（小声哔哔）(1)非视觉识别贴不讨论如何识别、如何提高帧率以及识别率，本贴是电控向的帖子，只关于自瞄应用于嵌入式控制（个人不太习惯将电控称为嵌入式，既然都这样分组了那就这样吧）部分。
(2)我们去年使用的是卡尔曼滤波器做的自瞄的预测。
(3)正好毕设也是做的运动参数的预测和滤波，做了点儿简单的研究，技术水平有限，如有错误欢迎大家指正！
(4)我们队的控制采用了云台底盘两个板子进行控制，预测代码放在云台上，和陀螺仪数据一起处理的。
参考开源：【哈尔滨工程大学】创梦之翼战队RM19全方位开源汇总贴  https://bbs.robomaster.com/forum.php?mod=viewthread&tid=9233&fromuid=14674
(5)代码是去年为了复活赛赶出来的，全套步兵代码只花了不到一周的时间就写完了，在命名以及有些逻辑细节上可能存在一些BUG，望谅解~（代码还没有上传，有时间整理整理就上传）
(6)前言就写一写废话吧，可直接跳到第一节（也可以直接参考官方的知乎专栏的内容，但是其后附的代码不能直接使用。对于一点儿都不懂卡尔曼的人比较很困难，至少我第一次看这个的时候是这个感觉https://zhuanlan.zhihu.com/p/38745950）。

16年大一的时候先是去了学校的的飞卡学做硬件，后来大一下才到了RM团队。在团队一直负责无人机，后来队伍人员出了点问题加上本来因为队伍人就少，就负责了无人机的机械、电控（本来是还得弄视觉的，学了一周视觉，因为实在是不能一周速成就让研究生学长帮了忙。所以在队伍管理的时候我基本上每个方向的评审都有我的影子，这个也造成了19年分区赛的方案太过于保守，我全责）。顺便说一下，我女朋友并不是打比赛打掉的。也就是在17年深圳比赛的时候认识了哈工大的某个设计工程车的人，也就是某位队长(手动滑稽，不说是谁了，怕某人来哈工程找我)。也就是那一年让我坚定入坑，这一晃就快4年了。这几年有大一17年分区赛的时候凭借自己的操作还有运气极好的分组打进国赛的兴奋和力挽狂澜的舒畅；也有大一分区赛因为自己失误面对东北大学的失利；还有大二因为整个队只有3个有技术的人导致各样失利的无奈。我因为经常打比赛（我们学校科创体质比较特殊，除了RM外我还打了10来个其他比赛）假期回家的时间很短，所以有些时候父母不太能理解。但是在19年暑假的时候我父母来到深圳看了我打比赛，也是通过这次机会才了解到我为了什么在付出。我也希望后来的朋友们能够继续做下去这个比赛。我马上本科毕业了，保研去了另外的一所已经不做RM的学校读研，而且以后研究方向也不是控制向了，所以想留一点儿资料给自家队伍，结果留了一些资料队内没人一起讨论，所以就在论坛开出来大家一起交流一下。本来是准备计划是准备把一些论坛上没有的一些资料给补一补，讲一讲我自认为的“原理”。但是时间有限（还有好多论文要看，头秃），我尽力。
大概规划一下以后要讲的，计划讲完这个将一下姿态解算、两个控制板的构成（一个大板做底盘和动作处理部分，一个小板复杂陀螺仪解算、滤波和云台控制，之前有一个队伍之前问过我这个，虽然我觉得挺好理解，但有时间也还是讲一讲）、还有我们队云台用的效果很好的巴特沃斯低通滤波器、超级电容（这个水平真有限，不一定将的明白）。还有什么我们队大家看的上的都可以留言，在不侵犯现在队伍核心利益的情况下进行适当开源。


1.自瞄方案
自瞄方案在我的认知里面一共两种，一是用PID的方法进行处理。二是用卡尔曼或者其他的方式进行预测。
下面我说说我认为各存在什么问题。
1.1 PID算法
(1)对于PID的方案的话对于我们队伍来说是问题不大的，因为我们的摄像头是330帧的，处理也只花的了3ms左右（视觉组那边的数据），相对于我们队云台采用1kHz的控制频率来说是问题不大的，平滑度勉强可以接受。我觉得平滑度问题是一个对于个别队伍来说比较棘手的问题，大家不都是某些6、70W经费的队伍，在氪金买土豪电脑方面存在一些小小的困难。因为如果电脑的处理频率较为低的情况下对发送指令间断的话会出一耸一耸的情况，有些指令可见图1。
(2)PID算法还有一个比较致命的缺点，那就是它是以误差驱动的控制器，没有误差是输出不变化的，如果误差变化频率较快，如sin这种跟随起来是比较困难的。还有就是当别人距离你较远的时候，你子弹的飞行时间是不能忽略不计的，这个时候你枪口只能勉强跟上，不能提前发射，这个时候是很致命的。顺便提一下，我们队用PID算法在330帧中距离的情况下对陀螺车命中率其实挺高的，操作手体验据说是很不错的（19年我们队机械能干活的几乎全部跑完了，只剩了一个熟练工。大法师一个人设计完了步兵、英雄、工程底盘，这里向大法师致敬。我们队历来都属于典型机械不够电控视觉来凑的队伍）。
1.2 卡尔曼滤波器
(1)对于前面提到的平滑问题来说，卡尔曼是具有补帧的功能的，这个我在测试的时候我是感受到了，330帧硬生生的补到了1KHz。但是具体为什么卡尔曼能够补的原理我不是很明白，我觉得我的理解可能有错误，希望有大佬能够解答。
(2)对于误差问题卡尔曼滤波器就完全没有问题，毕竟状态转移部分状态更新就是对状态进行预估的，后面会讲到。
(3)要说卡尔曼滤波器有什么缺点，我觉得就是在数据处理这儿可能有一些瑕疵。比如数据得转换坐标等（你也可以不转换，带着坐标转换做扩或者无迹变换，手动滑稽），这个函数构建可能比较麻烦，有些矩阵运算还得调DSP库运算，我当时DSP玩了半个下午才用明白。有人可能会说计算能力会不会不够，经过我的测试（使用FreeRTOS，STM32F405RGT6芯片，矩阵运算采用DSP库，任务优先级在所有任务中最高），在坐标转换、云台控制以及陀螺仪解算在同一任务在1kHz进行，是没有问题的。
1.3 总结
小结一下，在方案审定来看的话，卡尔曼滤波器肯定是各项性能优于PID跟踪的。唯一缺点就是麻烦，就像好东西肯定贵一个道理（AMD，YES！除外嗷）。
小弟水平有限，如有错误，多多指教。
今天就暂时更到这儿，改天继续~2020.3.18To be continued...

4.总结


1.确定合适的状态向量X状态转移矩阵φ（参见式（3-3））和合适的观测矩阵H（参见式（3-4））和合适的观测噪声均方差R与过程噪声均方差Q。
2.将视觉识别到的数据转换成偏转角度以及目标距离。
3.将数据与陀螺仪的数据通过getPoint3f()（似乎OpenCV中有，视觉小伙伴给我的）函数将角度数据转换为以机器人为坐标系原点的三维坐标系中。
4.将得到的3维数据通过卡尔曼滤波器递推公式计算目标物体的最佳估计状态。
5.通过式（3-5）计算目标预测位置，适当调整T值。
6.将预测到的目标位置转换为角度值（注意做好枪口角度补偿）。




我傻了，写了一大堆，然后发出来说我发的不符合规定......然后我删了一节发出来就发出来了，我没保存的有之前写的，裂开来。不想重新再写了，妈耶......
就这样吧烂尾就烂尾了。论坛的文章编辑属实有问题。

2.卡尔曼滤波器
卡尔曼滤波器呢，在书上的解释是：在几何上kalman filter可以看做状态变量在由观测生成的线性空间上的映射。所以卡尔曼滤波器和射影定理是息息相关的，但是实际是不影响使用的，感兴趣的朋友可以去查阅一下资料，这里我就不作推导了。
2.1 状态空间表示
状态空间模型对动态系统描述如下：（如图二或公式1所示，因为论坛似乎不能插入公式......）

X(k+1)=φX(k)+τW(k)

Y(k)=HX(k)+V(k)               ......(1)

其中k为离散时间，系统在k时刻状态为X(k)∈R(n维)；Y(k)为对应观测状态，且Y(k)∈R(m维)；W(k)∈R(r维)为输入白噪声；V(k)∈R(m维)为观测噪声。

(1)式中φ为状态转移矩阵，τ为噪声驱动矩阵，H为观测矩阵。

对了，这里解释一下大家常说的Q和R值是W(k)和V(k)对应的均值为零的白噪声方差阵。对于这两个参数的选取，后面会讲到。

整个(1)式的构造和现代控制理论的状态空间几乎是一样的，而且其的递推原理、状态表示和离散化的现代控制理论中的状态空间也差不多是一样的。

2.2 递推卡尔曼滤波器根据(1)式以及前面提到的射影定理等可以获得递推卡尔曼滤波器，其表示如下：（如图三所示）


假装图3......


这里我来一步一步解释一下递推卡尔曼滤波器。
其中(2)式是作为状态的一步预测。因为状态在没有外界干扰的情况下应该X(k+1)=φX(k)产生递推，所以这一步对状态做了预（这一步很重要，和后面预测有关联）。
其中(3)式是作为误差协方差阵的预测，也是同样的道理对协方差进行预测。相当于存在一定的误差，这个地方对误差通过设定的噪声方差进行推导。相当于对(2)式预测的质量进行了定量的评估，为后续进行了数据铺垫。
其中(4)式是通过协方差估计增益大小。这个是推导出来的。通过前面对预测质量的评估，然后定量的分析了这次参数的真实性大小。为后面误差的补偿做了数据铺垫。
其中(5)式是计算误差大小。通过观测值和(2)式通过递推公式的预测值进行计算，得到误差值。
其中(6)式是大家很熟悉的卡尔曼的公式。将预测值附加上增益乘上误差对预测值进行补偿，这样就得到我们认为的真实值。
其中(7)式是更新协方差矩阵。这个是通过射影定理的正交性推导得到的。


递推卡尔曼滤波器包括了两个更新过程，一个是时间更新，另一个是观测更新。要注意理解的是其中P(k|k-1)是作为状态的质量优劣的评定，R的大小为观测信息的质量优劣，Q为外界对系统的输入噪声，相当于对系统的影响。


2.3 大家手里面的卡尔曼温度计
大家可能在网上看到卡尔曼滤波最多的就是温度计的例子，但是温度计的例子和图3的公式几乎不太一样。我来大概的讲一下就懂了。假设：我现在在一个房间内，我需要测量这个房间的真实温度，但是我手头的温度计有方差为R的白噪声，而且房间开着窗户，空气的流动对室温有Q的白噪声影响。我前面的公式全部为向量的形式表示的，在温度计的例子中状态向量X则代表实际的室温。φ作为状态转移矩阵，这个地方因为除了外界空气流动的干扰就不会有其他影响，自己的温度对下一次的温度只有惯性作用，所以这个地方φ=1。因为只研究室温的话，那么同样能够得到τ=1。这时候你手里头有个温度计，这个温度计对这个室温进行测量得到的温度Y，得到Y的数值应该是实际的温度X测量的误差V（即R）的影响。同样也可以得到H=1。
现在在讲刚才的数值带入到图3的公式中：
x_hat(k+1)=x_kf(k);
P_hat(k+1)=P(k)+Q;
K(k+1)=P_hat(k+1)/(P_hat(k+1)+R);
e(k+1)=Y(k+1)-x_hat(k+1);
x_kf(k+1)=x_hat(k+1)+K(k+1)*e(k+1);
P(k+1)=(1-K(k+1))*P_hat(k+1);
是不是一下就和网上的卡尔曼的温度计差不多。

2.4 辣鸡的温度和风一直在吹的空气
这里顺讲一下Q和R的问题。
前面也讲到了Q和R呢是W(k)过程噪声和V(k)测量噪声分别对应的方差。那么如何得到Q和R值是很多人困惑的一点。怎么得到Q和R值首先得知道这两个分别是什么。
首先观测噪声，顾名思义是和测量的传感器的精度是有关的。例如温度计的测量误差为±0.1℃，那么这个时候那R=0.01有些时候看不到出厂说明书的话可以通过统计的方式获得。可以理解为用温度计多次测量一个恒定的温度，然后求这这些数据的方差得到R_hat，这个时候R_hat和R是非常接近的了。同样，对于过程噪声Q的话往往和过程产生的噪声有关。比如一个能够准确测量的温度计，测量恒定温度的前提下室内在没有空气流动干扰和有空气流动的干扰的时候的参数进行对比，分析方差。

2.5 温度计的仿真
见附件1，使用matlab进行仿，代码来源网络。


小弟水平有限，如有错误，多多指教。
今天就暂时更到这儿，有时间继续~2020.3.20To be continued...

支持大佬，大佬加油

机智的雨合 发表于 2020-4-1 22:25
知乎上面用一阶卡尔曼滤波来减小延迟(预测)好像是不可行的, 一阶卡尔曼并没有预测的功能弄, 看起来效果好是 ...

对，那个一维的卡尔曼收敛以后与一阶低通互补滤波无异，没有实际模型卡尔曼增益就不能动态调整，我觉得就是因为那个低通滤波没有选择合适的截止频率。

后续呢大佬

大佬加油

知乎上面用一阶卡尔曼滤波来减小延迟(预测)好像是不可行的, 一阶卡尔曼并没有预测的功能弄, 看起来效果好是因为作为对比的低通滤波器的参数没有调好.

登录/注册后可看帖子