CN108663043B - 基于单个相机辅助的分布式pos主子节点相对位姿测量方法 - Google Patents

Abstract

本发明涉及基于单个相机辅助的分布式POS主子节点相对位姿测量方法，包括如下技术步骤：将主POS及子IMU安装在柔性杆臂结构架两端对应的安装节点上，将两个靶标分别粘贴在主POS和子IMU一侧的表面，定义主POS表面的靶标为主靶标；子IMU表面的靶标为子靶标；完成单目相机的内外参数标定：利用得到的主子节点之间的位姿关系，与主POS导航信息进行信息融合，实现主子节点高精度位姿测量，最终得到较精确的子IMU导航信息，本发明针对子IMU精度低的问题，采用视觉辅助手段，提高主子之间位姿测量精度，得到子惯性测量单元较高精度的导航结果。

Description

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

技术领域

本发明专利涉及POS成像技术领域，尤其是一种基于单个相机辅助的分布式POS主子节点相对位姿测量方法，适用于测量载机存在弹性变形时载荷之间的基线长度，提高载荷之间相对位置的姿态精度。

背景技术

高精度POS可以为高分辨率航空遥感系统提供高精度的时间和空间信息，通过运动补偿提高成像质量，是实现高分辨率成像的关键；我国在单POS成像方面取得了一定进展，但由于对地观测载荷的需求牵引，如集成高分辨测绘相机、全谱段成像光谱仪，机载分布式阵列天线SAR和柔性多基线干涉SAR以及舰载稀疏阵列成像雷达等，多个或多种载荷安装在飞机不同位置，采用传统单POS系统无法实现多点的高精度位置姿态测量，因此迫切需要建立高精度分布式POS系统，为航空遥感系统中多个载荷提供高精度的运动参数；

发明内容

为了解决上述技术问题，本发明提供一种基于单个相机辅助的分布式POS主子节点相对位姿测量方法，克服传统初始对准动态条件下对准精度低的缺点，使其具有精度高、抗干扰能力强的特点，可用于测量载机存在挠曲变形时载荷之间的基线长度，提高载荷之间相对位置姿态精度。

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

步骤一、将主POS及子IMU安装在柔性杆臂结构架两端对应的安装节点上，将两个靶标分粘贴在主POS及子IMU一侧的表面，定义主POS表面的靶标为主靶标；子IMU表面的靶标为子靶标；

进一步的，所述主POS包括GPS和主IMU；

进一步的，所述分布式POS包括：一个高精度的主POS和一个低精度的子IMU；

作为一种应用举例说明，所述主靶标粘贴在所述主POS的表面，子靶标粘贴在子IMU的表面；

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

作为一种应用举例说明，所述参数标定采用张正友标定法，完成单目相机的内外参数标定；

作为一种举例说明，所述相机位姿解算：是利用靶标上的特征点之间的位置关系、图像中提取的特征点的中心位置和相机内参数来共同解算相机坐标系和靶标坐标系之间的相对位置和姿态关系的方法；因此，单目视觉测量之前首先要对相机进行内外参标定；

进一步的，所述相机标定基本原理如下：

假定靶标平面在世界坐标系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时，就可以求出唯一的一组解；

作为一种应用举例说明，所述张正友标定法所需的标定模板(即靶标)为黑白棋盘格，具体算法包括：

a)从不同角度拍摄若干张棋盘格标定板的图像；

b)检测出图像中的特征点，即角点；

c)求出摄相机的内外参数；

d)求出畸变系数；

e)优化标定结果。

由以上所述的张正友标定方法可确定左右摄像机的内部参数和外部参数；

作为一种应用举例说明，当相机内外参数标定后；所述位姿解算采用P3P位姿计算方法，求得靶标坐标系相对于相机坐标系的位姿关系；

在靶标上选取可构成三角形的三个特征点P₁,P₂,P₃，选取特征点如图2所示，A、B、C为靶标上特征点；AB、BC、AC长度已知；A'、B'为A、B、C分别在图像坐标系的坐标；O到像平面A'B'C'的距离为焦距f。通过图像提取可以得到A'、B'、C'三点的图像坐标，由此可以得到A'B'、B'C'、A'C'距离。f已知，可以得到OA'、OB'、OC'距离。通过余弦定理，得到∠A'OB'，∠A'OC'，∠B'OC'的值。令∠A'OB'＝α，∠A'OC'＝β，∠B'OC'＝γ公式如下：

此时AB、AC、BC的长度已知，利用∠A'OB'，∠A'OC'，∠B'OC'计算OA、OB、OC的长度，令a＝BC，b＝AC，c＝AB；x＝OA，y＝OB，z＝OC；

得

其中a，b，c；α，β，γ为已知值，x，y，z为未知值；解出x，y，z后，可以得到每个特征点在相机坐标系坐标；

根据上式求得特征点A、B、C在相机坐标系下的坐标，(X_AC,Y_AC,Z_AC)(X_BC,Y_BC,Z_BC)、(X_CC,Y_CC,Z_CC)；特征点在靶标坐标系下的坐标(X_AW,Y_AW,Z_AW)、(X_BW,Y_BW,Z_BW)、(X_CW,Y_CW,Z_CW)为已知量；

用R和T分别表示相机坐标系C与靶标坐标系W之间的旋转矩阵和平移向量，根据坐标系变换理论可得：

C＝RW+T

靶标上三个坐标点可以构成三个向量n_w＝(n_w1,n_w2,n_w3,)，第三个向量由前两个向量叉乘得到，转换到相机坐标系下可以构成三个向量n_c＝(n_c1,n_c2,n₃,)，向量只有旋转关系，所以可得关系式

设靶标先绕X轴旋转α，再绕Y轴旋转β，最后绕Z轴旋转γ得到旋转矩阵R：

姿态角求解：

对应的T为：T＝C-RW

步骤三、利用步骤二中相机得到的主子节点之间的位姿关系，与主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矩阵为位移与经纬度的转化关系阵，其表达式为：

进一步的，主节点位于主POS载体坐标系的原点，子节点位于子IMU载体坐标系的原点；使用主POS高精度的运动参数以及相机测量得到的主子节点相对位姿信息作为量测信息，与子惯性测量单元进行信息融合，最终得到准确的子IMU导航信息。

为了更好的说明本发明方法之设计依据，现举例本发明的设计原理如下：

首先，将高精度主惯性测量单元及低精度子惯性测量单元安装在柔性杆臂结构架的对应安装节点上，将主靶标和子靶标分别贴在主POS和子IMU一侧；

其次，主POS进行初始对准，实现位置、速度、姿态信息的输出；相机捕获带有主靶标和子靶标的图像，并通过位姿解算得到主靶标和子靶标间的位姿关系；

最后，子IMU借助主POS位置速度姿态信息和视觉获取的主子IMU位姿关系实现信息融合，获取子系统精确的速度、姿态信息；

有益效果：

1、本发明针对子IMU精度低的问题，采用视觉辅助手段，提高主子之间位姿测量精度，得到子惯性测量单元较高精度的导航结果；

2、高精度主POS导航信息与相机测量的主/子IMU之间的相对位姿关系进行信息融合，最终得到高精度的子IMU运动参数，该方法提高整体系统的实时导航精度，可快速实现载荷之间高精度相对位置姿态信息；

3、该发明之算法，设计科学优化，运行安全可靠；

附图说明

图1是本发明基于单个相机辅助的分布式POS主子节点相对位姿测量方法之工作结构状态示意图

图2是本发明基于单个相机辅助的分布式POS主子节点相对位姿测量方法之位姿解算示意图

图3是本发明基于单个相机辅助的分布式POS主子节点相对位姿测量方法之流程结构示意图

具体实施方式

下面，参考附图1至图3所示，一种基于单个相机辅助的分布式POS主子节点相对位姿测量方法，包括如下技术步骤：

步骤一、将主POS101及子IMU102安装在柔性杆臂结构架103两端对应的安装节点上，将两个靶标分粘贴在主POS101及子IMU102一侧的表面，定义主IMU表面的靶标为主靶标；子IMU表面的靶标为子靶标；

进一步的，所述主POS101包括GPS和主IMU；

进一步的，所述分布式POS包括：一个高精度的主POS101和一个低精度的子IMU102；

作为一种应用举例说明，所述主靶标104粘贴在所述主POS101的表面，子靶标105粘贴在子IMU102的表面；

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

作为一种应用举例说明，所述参数标定采用张正友标定法，完成单目相机106的内外参数标定；

作为一种举例说明，所述相机106位姿解算：是利用靶标上的特征点之间的位置关系、图像中提取的特征点的中心位置和相机内参数来共同解算相机坐标系和靶标坐标系之间的相对位置和姿态关系的方法；因此，单目视觉测量之前首先要对相机106进行内外参标定；

进一步的，所述相机106标定基本原理如下：

假定靶标平面在世界坐标系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时，就可以求出唯一的一组解；

作为一种应用举例说明，所述张正友标定法所需的标定模板(即靶标)为黑白棋盘格，具体算法包括：

a)从不同角度拍摄若干张棋盘格标定板的图像；

b)检测出图像中的特征点，即角点；

c)求出摄相机的内外参数；

d)求出畸变系数；

e)优化标定结果。

由以上所述的张正友标定方法可确定左右摄像机的内部参数和外部参数；

作为一种应用举例说明，当相机106内外参数标定后；所述位姿解算采用P3P位姿计算方法，求得靶标坐标系相对于相机坐标系的位姿关系；

在靶标上选取可构成三角形的三个特征点P₁,P₂,P₃，选取特征点如图2所示，A、B、C为靶标上特征点；AB、BC、AC长度已知；A'、B'为A、B、C分别在图像坐标系的坐标；O到像平面A'B'C'的距离为焦距f。通过图像提取可以得到A'、B'、C'三点的图像坐标，由此可以得到A'B'、B'C'、A'C'距离。f已知，可以得到OA'、OB'、OC'距离。通过余弦定理，得到∠A'OB'，∠A'OC'，∠B'OC'的值。令∠A'OB'＝α，∠A'OC'＝β，∠B'OC'＝γ公式如下：

此时AB、AC、BC的长度已知，利用∠A'OB'，∠A'OC'，∠B'OC'计算OA、OB、OC的长度，令a＝BC，b＝AC，c＝AB；x＝OA，y＝OB，z＝OC；

得

其中a，b，c；α，β，γ为已知值，x，y，z为未知值；解出x，y，z后，可以得到每个特征点在相机106坐标系坐标；

根据上式求得特征点A、B、C在相机坐标系下的坐标，(X_AC,Y_AC,Z_AC)(X_BC,Y_BC,Z_BC)、(X_CC,Y_CC,Z_CC)；特征点在靶标坐标系下的坐标(X_AW,Y_AW,Z_AW)、(X_BW,Y_BW,Z_BW)、(X_CW,Y_CW,Z_CW)为已知量；

用R和T分别表示相机106坐标系C与靶标坐标系W之间的旋转矩阵和平移向量，根据坐标系变换理论可得：

C＝RW+T

靶标上三个坐标点可以构成三个向量n_w＝(n_w1,n_w2,n_w3,)，第三个向量由前两个向量叉乘得到，转换到相机坐标系下可以构成三个向量n_c＝(n_c1,n_c2,n₃,)，向量只有旋转关系，所以可得关系式

设靶标先绕X轴旋转α，再绕Y轴旋转β，最后绕Z轴旋转γ得到旋转矩阵R：

姿态角求解：

对应的T为：T＝C-RW；

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

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

作为一种举例说明，所述信息融合实现方式如下：

系统量测量Z由相机106测量得到的主POS101载体坐标系和子IMU102载体坐标相对姿态角和相对位移量，以及主POS101姿态角和主经纬度信息、子IMU102姿态角和子经纬度信息来提供；

进一步的，量测方程为：

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

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

为了更好的说明本发明方法之设计依据，现举例本发明的设计原理如下：

首先，将高精度主惯性测量单元及低精度子惯性测量单元安装在柔性杆臂结构架的对应安装节点上，将主靶标104和子靶标105分别贴在主POS101和子IMU102一侧；

其次，主POS101进行初始对准，实现位置、速度、姿态信息的输出；相机捕获带有主靶标104和子靶标105的图像，并通过位姿解算得到主靶标104和子靶标105间的位姿关系；

最后，子IMU102借助主POS101位置速度姿态信息和视觉获取的主、子IMU102位姿关系实现信息融合，获取子系统精确的速度、姿态信息；

本发明针对子IMU102精度低的问题，采用视觉辅助手段，提高主子之间位姿测量精度，得到子惯性测量单元较高精度的导航结果；高精度主POS101导航信息与相机106测量的主/子IMU102之间的相对位姿关系进行信息融合，最终得到高精度的子IMU102运动参数，该方法提高整体系统的实时导航精度，可快速实现载荷之间高精度相对位置姿态信息；

以上公开的仅为本申请的一个具体实施例，但本申请并非局限于此，任何本领域的技术人员能思之的变化，都应落在本申请的保护范围内。

Claims (7)

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时，就可以求出唯一的一组解。

5.根据权利要求2所述的基于单个相机辅助的分布式POS主子节点相对位姿测量方法，其特征在于，所述张正友标定法所需的标定模板，即靶标采用黑白棋盘格，具体算法包括：从不同角度拍摄若干张棋盘格标定板的图像；检测出图像中的特征点，即角点；求出摄相机的内外参数；求出畸变系数；优化标定结果；此时即可确定左右摄像机的内部参数和外部参数。

6.根据权利要求5所述的基于单个相机辅助的分布式POS主子节点相对位姿测量方法，其特征在于，当相机内外参数标定后，所述位姿解算采用P3P位姿计算方法，求得靶标坐标系相对于相机坐标系的位姿关系。

7.根据权利要求6所述的基于单个相机辅助的分布式POS主子节点相对位姿测量方法，其特征在于，所述P3P位姿计算方法包括：在靶标上选取可构成三角形的三个特征点P₁,P₂,P₃，A、B、C为靶标上特征点；AB、BC、AC长度已知；A'、B'为A、B、C分别在图像坐标系的坐标；O到像平面A'B'C'的距离为焦距f；通过图像提取可以得到A'、B'、C'三点的图像坐标，由此可以得到A'B'、B'C'、A'C'距离；f已知，可以得到OA'、OB'、OC'距离；通过余弦定理，得到∠A'OB'，∠A'OC'，∠B'OC'的值，令∠A'OB'＝α，∠A'OC'＝β，∠B'OC'＝γ公式如下：

此时AB、AC、BC的长度已知，利用∠A'OB'，∠A'OC'，∠B'OC'计算OA、OB、OC的长度，令a＝BC，b＝AC，c＝AB；x＝OA，y＝OB，z＝OC；

得

其中a，b，c；α，β，γ为已知值，x，y，z为未知值；解出x，y，z后，可以得到每个特征点在相机坐标系坐标；

根据上式求得特征点A、B、C在相机坐标系下的坐标，

(X_AC,Y_AC,Z_AC)(X_BC,Y_BC,Z_BC)、(X_CC,Y_CC,Z_CC)；特征点在靶标坐标系下的坐标(X_AW,Y_AW,Z_AW)、(X_BW,Y_BW,Z_BW)、(X_CW,Y_CW,Z_CW)为已知量；

用R和T分别表示相机坐标系C与靶标坐标系W之间的旋转矩阵和平移向量，根据坐标系变换理论可得：

C＝RW+T

靶标上三个坐标点可以构成三个向量n_w＝(n_w1,n_w2,n_w3,)，第三个向量由前两个向量叉乘得到，转换到相机坐标系下可以构成三个向量n_c＝(n_c1,n_c2,n₃,)，向量只有旋转关系，所以可得关系式

设靶标先绕X轴旋转α，再绕Y轴旋转β，最后绕Z轴旋转γ得到旋转矩阵R：

姿态角求解：

对应的T为：T＝C-RW。

Images