Liu Han
Liu Han
既然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战队的自瞄算法中已经实践测试。
The docs does teach me how to built a module by myself, but how can I upload my own module to the module registry? I didn't see a useful guide...