[发明专利]基于单目视觉的位置姿态测量系统精度检校方法和装置

权利要求书

1.基于单目视觉的位置姿态测量系统精度检校方法，其特征在于，包括以下步骤：

通过相机预先从不同角度拍摄三幅靶标的参考图像；

将IMU与所述靶标固连，通过所述相机获取一张所述靶标的实时图像；

由所述相机的投影模型得到每个标志点和对应的像点之间的关系为：

式中：为第i幅图上第j个标志点像坐标，X_j为第j个标志点物坐标，Pⁱ为第i幅图对应的投影矩阵；为第i幅图中第j个像点和其对应的物点的距离在相机光轴方向的投影，称为射影深度；

构建测量矩阵W：

其中，为测量坐标系下所测数据，使用共轭梯度法迭代每个点的射影深度通过测量坐标系的实测坐标更新测量矩阵，对W阵通过SVD分解：

W＝V·diag(σ₁,…σ_N)·U^T

由P、X阵，可知，W的秩小于等于4，即σ₅＝σ₆＝…＝σ₉＝0因此有：

此时W阵可以分解为一个表示相机运动的P阵和一个表示靶标形状的X阵：

式中V₁,U₁分别为V,U的前4列；分解不唯一，上式在相差任意非奇异4x4矩阵H的情况下都满足：

W＝PHH^-1X

当存在非奇异矩阵H满足欧式变换条件：

P_pH＝αK[RT]

式中α是满足矩阵方程的任意非零实数；将H改写为H＝[H_a,H_b],H_a是H阵的前3列，H_b是H阵第四列；从欧式变换矩阵中分割出旋转部分得：

P_pH_a＝αKR

对上式进行转置并令ω＝KK^T，可得：

将展开，可得3x3对称阵，用D_ab表示该阵第a行b列的元素：

D_ab是对称阵，将其拆为方程组，列出上三角6个元素对应方程：

对M进行奇异值分解：

M＝Udiag(σ₁,σ₂,σ₃,0)V^T

式中U_a为U的前3列；因H满足下式P_pH_b＝[0,0,0]^T，带入任意P_p解出H_b；最终解出H，完成欧式重构：

C是P_e中前3行，前3列数据，即为所求的第一位姿数据，获取靶标的第一位姿数据；

标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，具体包括步骤：

基于正交矢量和动态滤波的联合标定法，先使用正交矢量法标定两坐标系姿态关系，再使用动态滤波法估计两坐标系位置关系，滤波状态量为：

其中，为IMU在地理坐标系的位置；为IMU在地理坐标系的速度；b_a为IMU中加速度计漂移误差；为IMU坐标系和靶标坐标系之间的相对位置关系；

滤波量测量为：

其中，z为相机测得的靶标的位置信息；

其中，由IMU直接输出，由已标定姿态和IMU、相机输出姿态计算得到：

其中，由正交矢量法标定得到；

将所述第一位姿数据转到IMU坐标系下，得出第二位姿数据；

将所述第二位姿数据与POS自身输出的位置姿态数据进行比对，检校所述POS的测量精度。

2.基于单目视觉的位置姿态测量系统精度检校装置，用于检测POS输出的位姿数据的精度，其特征在于，包括靶标、相机和检校模块；

所述靶标与所述POS的IMU固连；

所述相机，固定设置，用于预先从不同角度拍摄三幅靶标的参考图像，并在固定后拍摄一张所述靶标的实时图像；

所述检校模块，用于：

由所述相机的投影模型得到每个标志点和对应的像点之间的关系为：

式中：为第i幅图上第j个标志点像坐标，X_j为第j个标志点物坐标，Pⁱ为第i幅图对应的投影矩阵；为第i幅图中第j个像点和其对应的物点的距离在相机光轴方向的投影，称为射影深度；

构建测量矩阵W：

其中，为测量坐标系下所测数据，使用共轭梯度法迭代每个点的射影深度通过测量坐标系的实测坐标更新测量矩阵，对W阵通过SVD分解：

W＝V·diag(σ₁,…σ_N)·U^T

由P、X阵，可知，W的秩小于等于4，即σ₅＝σ₆＝…＝σ₉＝0因此有：

此时W阵可以分解为一个表示相机运动的P阵和一个表示靶标形状的X阵：

式中V₁,U₁分别为V,U的前4列；分解不唯一，上式在相差任意非奇异4x4矩阵H的情况下都满足：

W＝PHH^-1X

当存在非奇异矩阵H满足欧式变换条件：

P_pH＝αK[RT]

式中α是满足矩阵方程的任意非零实数；将H改写为H＝[H_a,H_b],H_a是H阵的前3列，H_b是H阵第四列；从欧式变换矩阵中分割出旋转部分得：

P_pH_a＝αKR

对上式进行转置并令ω＝KK^T，可得：

将展开，可得3x3对称阵，用D_ab表示该阵第a行b列的元素：

D_ab是对称阵，将其拆为方程组，列出上三角6个元素对应方程：

对M进行奇异值分解：

式中U_a为U的前3列；因H满足下式P_pH_b＝[0,0,0]^T，带入任意P_p解出H_b；最终解出H，完成欧式重构：

C是P_e中前3行，前3列数据，即为所求的第一位姿数据，获取靶标的第一位姿数据；

标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，将所述第一位姿数据转到IMU坐标系下，得出第二位姿数据；

将所述第二位姿数据与POS自身输出的位置姿态数据进行比对，检校所述POS的测量精度；

所述检校模块，还用于：

基于正交矢量和动态滤波的联合标定法来标定IMU坐标系和靶标坐标系之间的固定相对位姿关系，先使用正交矢量法标定两坐标系姿态关系，再使用动态滤波法估计两坐标系位置关系，滤波状态量为：

其中，为IMU在地理坐标系的位置；为IMU在地理坐标系的速度；b_a为IMU中加速度计漂移误差；为IMU坐标系和靶标坐标系之间的相对位置关系；

滤波量测量为：

其中，z为相机测得的靶标的位置信息；

其中，由IMU直接输出，由已标定姿态和IMU、相机输出姿态计算得到：

其中，由正交矢量法标定得到。