添加降低自由度以提高PnP姿态解算精度的一种算法

[JellyfishKnight]

既然PnP求解姿态时有三个自由度，那么我们可以降低自由度来构造关于yaw的单峰函数。通过求极值点来求出正确的yaw。官方的机器人制造规范手册中说明了，装甲板安装时，roll应该为0°，pitch应该为15°。因此装甲板在陀螺仪坐标系中的姿态关于roll和pitch这两个量的大小就已经固定了，需要解算的就是yaw的值。
构造单峰函数的第一件事是确定函数的应变量和自变量。显然自变量是yaw，而应变量应该是反投影过后的装甲板图像点与识别到的装甲板图像点之间的差。由于装甲板识别出来在本质上是一个平行四边形，因此主要考虑平行四边形怎么作差。主要有以下三种思路：
（1）对应的点的二维坐标直接使用欧氏距离作为平行四边形的差；
（2）对应的两个平行四边形的面积作差；
（3）对应的两个平行四边形的夹角加上法向量信息作差；
对于方法（1）（2），笔者都尝试过，结果就是在通常的情况下，构造出来的函数有多个极值点。对于方法三，采用的方法为计算平行四边形中一组夹角的差加上法向量的信息，即夹角加上正负号，即可构造出一个比较合理的单峰函数，但是比较遗憾的是笔者暂时未在matlab上进行验证，并且在实际情况中发现还是会有偶然的情况使得函数走样，不过在一般的情况下能够满足大部分的需求并且不会出错。

由图可知，反投影的装甲板的角点和识别到的装甲板的角点是保持了角点之间的顺序和相对位置关系的（图中平行四边形角点的编号0、1、2、3）。因此我们可以选择∠103进行作差，可知实际姿态和绕yaw轴相对0点对称位置的反投影可以被过滤掉，但是此时中心对称的反投影结果和实际姿态一样。这时候我们可以通过求法向量夹角的方式确定正负号，从而将中心对称的反投影过滤掉。
此方法在2024年的南部赛区区域赛Helios战队的自瞄算法中已经实践测试。

[gezp]

没看明白，装甲板安装规范只是规定了安装位置，但是机器人本身姿态是不确定的，摄像头的位姿也可能不是固定的，那么，roll,pitch也没法固定吧？PNP求解装甲板位姿，最重要的是平移项，去确定装甲板位置，而不是旋转项吧？希望可以补充更多的信息，可以先描述一下问题。

[Ericsii]

现阶段赛场上面多用chenjunnn整车观测模型来做自瞄需要PnP对装甲板姿态观测比较准。其实可以简单用ceres实现一个限制上下界的Optimization based PnP应该可以达到类似效果

[JellyfishKnight]

没看明白，装甲板安装规范只是规定了安装位置，但是机器人本身姿态是不确定的，摄像头的位姿也可能不是固定的，那么，roll,pitch也没法固定吧？PNP求解装甲板位姿，最重要的是平移项，去确定装甲板位置，而不是旋转项吧？希望可以补充更多的信息，可以先描述一下问题。

这种方法是适用于求平地机器人的装甲板姿态部分的，关注的是其旋转向量的精度，因为平移的精度一般来说是足够的。摄像头的位姿可以通过陀螺仪加坐标系建模搞定。但是缺点是会损失击打斜坡上的机器人的能力，但会让平地上使用扩展卡尔曼观测目标小陀螺状态更加准确

[JellyfishKnight]

现阶段赛场上面多用chenjunnn整车观测模型来做自瞄需要PnP对装甲板姿态观测比较准。其实可以简单用ceres实现一个限制上下界的Optimization based PnP应该可以达到类似效果

但是主要求解不是针对于装甲板的yaw轴姿态吗，限制上下界应该并不能优化装甲板在可视范围内的求解精度吧？对于roll和pitch的话其本身变化的程度可能就确实比较大，感觉不太确定能达到比较好的效果

[Ericsii]

由于装甲板识别出来在本质上是一个平行四边形，因此主要考虑平行四边形怎么作差。

一个问题是相机透视中，装甲板并不是一个平行四边形，在非正对的情况下透视变换的结果是非规则的四边形

[JellyfishKnight]

由于装甲板识别出来在本质上是一个平行四边形，因此主要考虑平行四边形怎么作差。

一个问题是相机透视中，装甲板并不是一个平行四边形，在非正对的情况下透视变换的结果是非规则的四边形

是这样的，这应该也是目前这个方法在一些偶然的情况下会出问题的原因。但是我个人认为利用夹角和法向量来判断应该是没问题的，至于具体的方法可能还需要一些优化