[发明专利]基于单个相机辅助的分布式POS主子节点相对位姿测量方法

权利要求书

1.基于单个相机辅助的分布式POS主子节点相对位姿测量方法，其特征在于，包括如下技术步骤：

步骤一、将主POS及子IMU安装在柔性杆臂结构架两端对应的安装节点上，将两个靶标分别粘贴在主POS和子IMU一侧的表面，定义主POS表面的靶标为主靶标；子IMU表面的靶标为子靶标；所述主POS包括GPS和主IMU；所述分布式POS包括：一个高精度的主POS和一个低精度的子IMU；

步骤二、完成单目相机的内外参数标定：通过位姿解算得到主靶标和相机之间的相对位姿关系，同理通过位姿解算得到子靶标和相机之间的相对位姿关系，进而得到主靶标和子靶标的位姿关系；由于靶标坐标系和IMU坐标系不重合，所以需坐标转换将主子靶标位姿关系转化为主子节点位姿关系；

步骤三、利用步骤二中相机得到的主子节点之间的位姿关系，与主POS导航信息进行信息融合，实现主子节点高精度位姿测量，最终得到较精确的子IMU导航信息；

所述步骤三中的信息融合是指，利用主POS精确的组合导航信息，与视觉测量的主子节点间位姿信息相融合，最终得到准确的子IMU导航信息；

所述信息融合实现方式包括：系统量测量Z由相机测量得到的主POS载体坐标系和子IMU载体坐标相对姿态角和相对位移量，以及主POS姿态角和主经纬度信息、子IMU姿态角和子经纬度信息来提供；

其中，量测方程为：

量测量Z＝[Z₁ Z₂]，其中：

其中δψ、δθ和δγ分别为子IMU与主POS之间的航向角、俯仰角、横滚角之差，δθ_x、δθ_y、δθ_z分别为相机测得的子IMU与主POS之间载体坐标系失准角；δS_x、δS_y、δS_z分别为相机测得的主POS和子IMU相对位移形变量，δL、δλ、δh分别为子IMU与主POS之间的纬度、经度、高度之差；C矩阵为位移与经纬度的转化关系阵，其表达式为：

2.根据权利要求1所述的基于单个相机辅助的分布式POS主子节点相对位姿测量方法，其特征在于，所述参数标定采用张正友标定法，完成单目相机的内外参数标定。

3.根据权利要求2所述的基于单个相机辅助的分布式POS主子节点相对位姿测量方法，其特征在于，所述相机位姿解算：是利用靶标上的特征点之间的位置关系、图像中提取的特征点的中心位置和相机内参数来共同解算相机坐标系和靶标坐标系之间的相对位置和姿态关系的方法；且单目视觉测量之前首先要对相机进行内外参数标定。

4.根据权利要求3所述的基于单个相机辅助的分布式POS主子节点相对位姿测量方法，其特征在于，所述相机内外参数标定包括：

假定靶标平面在世界坐标系Z_w＝0的平面上，靶标平面上的点P的齐次坐标为P＝(X_w,Y_w,0,1)^T，P点投影到像平面上对应的齐次坐标为p＝(u,v,1)^T，R＝[r₁,r₂,r₃]，R和t分别为摄像机坐标系相对于世界坐标系的旋转矩阵和平移矢量；

设H＝[h₁,h₂,h₃]＝λK[r₁,r₂,r₃]，其中和根据旋转矩阵R的正交性，即r₁^Tr₂＝0和||r₁||＝||r₂||＝1，每幅图像可以获得以下两个对内参数矩阵K的基本约束方程：

由于摄像机有5个未知参数，所以当所拍得图像数目不小于3时，就可以求出唯一的一组解。