CN107292927B - 一种基于双目视觉的对称运动平台位姿测量方法 - Google Patents

Abstract

一种基于双目视觉的对称运动平台位姿测量方法，涉及位姿测量。在平台表面设计不同形式的靶标，用于识别不同的靶标面；设置2个相机，由2个相机组合视场完全覆盖靶标的运动范围；分别对单个相机的内参、2个相机和各个侧靶标面与顶靶标面之间的相对位姿进行标定；在平台运动初始位置建立世界坐标系o_w‑x_wy_wz_z；对运动平台进行图像采集与处理，提取靶标信息；对成像效果进行判断，确定靶标解算方法，并根据空间几何关系，解算出运动平台相对世界坐标系的位姿。可适用于对称、多面、形状规则的空间六自由度运动平台的位姿测量。结构简单、方便灵活、应用场景更广。

Description

一种基于双目视觉的对称运动平台位姿测量方法

技术领域

本发明涉及位姿测量，尤其是涉及一种基于双目视觉的对称运动平台位姿测量方法。

背景技术

位姿测量是对目标对象相对于参考坐标系的位置和姿态的测量，它是测量控制领域的重要研究内容，在众多领域应用时都很关键。

现有位姿的测量方法大致可以分为两类：一类是内侧法，即在目标对象内部安装惯性传感器测量系统，对测量数据进行处理、计算后得出目标对象的位姿；另一类是外测法，即在目标对象表面设计靶标，利用相机对靶标进行图像采集并进行图像处理，建立相应的数学模型，实现对目标对象位姿的测量。由这两类位姿测量方法衍生出许多具体的位姿测量方法，比如单目视觉、双目视觉、航姿参考系统、惯性测量单元等等。

虽然有较多方法可以测量位姿，但是对于空间六自由度运动物体的位姿测量而言，多数都存在一定的限制。例如单目视觉，虽然精度比较高，但相机的视场相对双目视觉较小，比较适合测量静态或运动范围很小的物体的位姿，对于较大空间运动的物体很有可能无法获取目标对象表面完整靶标，从而无法解算位姿；航姿参考系统，其精度与地球的重力场和磁场有较大关系，在失重环境下无法正常工作，而且航姿参考系统在位姿计算过程中会产生漂移，鲁棒性不强，总体误差相对于视觉较大；惯性测量单元与航姿参考系统类似，也存在一定的局限性。

使用双目视觉手段，可以有效地扩大视场，且系统稳定性强，精度高，误差小，对环境要求不高，可以有效弥补上述测量方法所存在的缺陷。

针对规则对称运动平台的高精度位姿测量，采用双目视觉测量方法，不仅需要合理设计靶标，以及相机安装方式，以满足平台运动范围的要求；还需对单目、双目相机，以及不用靶标面之间的相对位姿进行标定；并根据成像效果，确定解算方案，以满足运动平台的实时高精度位姿测量。

发明内容

本发明的目的在于提供一种基于双目视觉的对称运动平台位姿测量方法。

本发明包括以下步骤：

1)在平台表面设计不同形式的靶标，用于识别不同的靶标面；

2)设置2个相机，由2个相机组合视场完全覆盖靶标的运动范围；

3)分别对单个相机的内参、2个相机和各个侧靶标面与顶靶标面之间的相对位姿进行标定；

4)在平台运动初始位置建立世界坐标系o_w-x_wy_wz_z；对运动平台进行图像采集与处理，提取靶标信息；

5)对成像效果进行判断，确定靶标解算方法，并根据空间几何关系，解算出运动平台相对世界坐标系的位姿。

在步骤1)中，所述不同形式的靶标的相关设计原则可包括：

(1)靶标的设计基于PnP(Perspective n Points)理论，具体可以根据靶标标志点的形状、大小或数量等物理特征设计；

(2)可以使用棋盘或是圆点等已有的标定板作为靶标，其目的在于程序识别不同的靶标面；

(3)分别在每个侧靶标面设置3×3、4×4、5×5、6×6的圆点标定板，在顶靶标面设置7*7的圆点标定板。

在步骤2)中，所述设置2个相机，可根据视场、距离、安装角等几何参数之间的关系，设计2个相机的安装位置，使双目相机的组合视场可以完全覆盖靶标的运动范围，参数设计公式如下：

式中，X、Y分别是左右相机云台安装点相距横梁左右端点的距离；L是空间的水平跨度；H₁、H₂分别是横梁左右端点的垂直高度；α、β分别是左右相机的安装角，即相机主线与横梁的锐角夹角；γ横梁与水平方向的夹角；b是相机的半视场角；D₁、D₂分别是左右相机工作距离限制的最大值；D₃是相机满足另外一个方向宽度限制的最小值；l是相机主线，即云台安装点到相机光心的距离。

在步骤3)中，所述各个侧靶标面与顶靶标面之间的相对位姿进行标定的具体步骤可为：

(1)在侧靶标面与顶靶标面建立各自独立的坐标系，利用双目相机计算和提取侧靶标面中的标志点在顶靶标面坐标系o₁-x₁y₁z₁下的表示，然后采用LM算法(Levenberg-Marquardt)，得到两个靶标面之间的相对位姿关系，即两个靶标面局部坐标系之间的齐次变换矩阵

同时进行保存；其中，下标1表示顶靶标面，n表示第n个侧靶标面，R_1n表示第n个侧靶标面与顶靶标面局部坐标系之间的旋转矩阵，若规定一个坐标系旋转关系分解为绕坐标系的z轴顺时针旋转ψ角度、y轴顺时针旋转θ角度和x轴顺时针旋转

角度，则

T_1n＝[t_nx t_ny t_nz]^T表示第n个侧靶标面局部坐标系o_n-x_ny_nz_n原点o_n与顶靶标面局部坐标系o₁-x₁y₁z₁原点o₁之间的平移向量，t_nx表示o_n与o₁在顶靶标面局部坐标系x轴方向上的平移距离，t_ny表示o_n与o₁在顶靶标面局部坐标系y轴方向上的平移距离，t_nz表示o_n与o₁在顶靶标面局部坐标系z轴方向上的平移距离。

(2)各个侧靶标面与顶靶标面之间相对位姿标定的意义在于：以侧靶标面为基准解算出的运动平台的相对位姿都需转换成以顶靶标面为基准的相对位姿，使运动平台的位姿变化更具物理意义和直观性。

在步骤4)中，所述对运动平台进行图像采集与处理，提取靶标信息，其中需要对成像标志点进行排序，以确定空间点坐标和图像坐标之间的对应关系，具体步骤可为：

(1)采用图像处理提取标定板外轮廓，然后对轮廓进行分割与直线拟合，通过直线求交点获得标点板的四个角点；

(2)在图像内描述一个正方形区域，通过投影变换，使标定板变为正投影，并进行矫正；以靠近黑色三角形最近的点作为第一个点，按顺时针行排列对其余点进行排序；

(3)对排序后的点进行正投影反变换，得到实际成像下的图像坐标，即完成了图像内标志点的排序。

在步骤5)中，所述对成像效果进行判断，确定靶标解算方法，对应原则可包括：

(1)程序检测到包含顶靶标面在内的多个靶标面同时进入相机视野时，仅选择顶靶标面的7*7靶标成像作为位姿解算的基准；

(2)程序检测到不包含顶靶标面在内的多个侧靶标面同时进入某个相机视野时，出于对精度的考虑，程序通过判断各侧靶标面相对于其相机坐标系o_c-x_cy_cz_c基准平面o_c-x_cy_c的倾斜角度α_n，选择α_n较小的靶标面成像作为位姿解算的基准，其中，下标n表示第n个侧靶标面。

(3)当运动平台在相机组合视场边缘时，顶靶标面成像不完整，程序检测到仅有一个侧靶标面成像时，则以此侧靶标面成像为基准来解算平台的相对位姿；

(4)以侧靶标面为基准解算出的运动平台相对世界坐标系的位姿，还需转换为顶靶标面相对世界坐标系的位姿，以便运动平台的位姿变化更具物理意义和直观性；

位姿转换的数学公式为：

其中，[x_w y_w z_w 1]^T是世界坐标系下任意一点的齐次坐标；[x_n y_n z_n 1]^T是第n个侧靶标面局部坐标系下该点的齐次坐标；[x₁ y₁ z₁ 1]^T是顶靶标面1局部坐标系下该点的齐次坐标；M_wn是第n个侧靶标面局部坐标系与世界坐标系之间的齐次变换矩阵；M_n1是第n个侧靶标面局部坐标系与顶靶标面1局部坐标系之间的齐次变换矩阵,即标定矩阵；M_w1是顶靶标面局部坐标系与世界坐标系之间的齐次变换矩阵；M都由旋转矩阵R和平移向量T组成；

(5)运动平台的相对位姿通过向量

表示，X可根据靶标点在世界坐标系和图像坐标系o-uv之间的对应关系，并考虑相机内参的畸变，采用最小二乘法，解算出表征空间几何关系的齐次矩阵

其中，T＝[t_x t_y t_z]^T是坐标系原点之间的平移向量，t_x、t_y、t_z即是运动平台的位置参数；其中，

是坐标系之间的旋转矩阵，r₁、r₂，…，r₉是其对应的九个元素，则运动平台的姿态参数，偏航角ψ、俯仰角θ、滚转角

可通过以下计算公式求得：

本发明可适用于对称、多面、形状规则的空间六自由度运动平台的位姿测量。

本发明通过采用双目视觉手段对空间运动的对称运动平台进行相对位置和姿态的测量，根据PnP(Perspective n Points)理论在不同靶标面设计不同形式的靶标；依据设计公式合理布置2个相机，使2个相机的组合视场可以完全覆盖靶标的运动范围；分别对单个相机的内参，2个相机，以及不同靶面之间的相对位姿进行标定；在运动起始位置建立世界坐标系；利用双目相机对运动平台进行图像采集与图像处理，提取靶标信息，并根据成像效果和相应原则，确定位姿解算方法，通过空间几何关系，实时解算出运动平台的相对位姿。本发明具有成本低、精度高、结构简单、方便灵活、应用场景广泛等优点。本发明的实施例可提供为方法、系统或是计算机程序产品。

本发明的有益效果是，利用双目视觉对空间六自由度运动对称平台进行位姿测量，系统稳定性强，精度高，对环境要求不高，可有效弥补惯性传感器测量位姿时所存在的缺陷；且双目相机位置的灵活布置和靶标的灵活设计，使本发明结构简单、方便灵活、应用场景更广。

附图说明

图1为规则、多面、对称平台的示意图。

图2为该基于双目视觉的对称运动平台位姿测量方法的流程图。

图3为相机安装的横向几何参数示意图。

图4为一种常用标定板，圆点标定板。

图5为本发明的一种具体工程效果图。

图6为平台沿Y方向匀速运动时位置解算结果。

图7为平台沿Y方向匀速运动时在X方向移动的位置解算结果。

具体实施方式

以下结合附图对本发明的实施例进行说明，应当理解，此处所描述的实例仅用于说明和解释本发明，并不用于限定本发明。针对一种对称、多面、形状规则的空间六自由度运动平台进行位姿测量，其平台示意图如图1所示，顶面1为顶靶标面，是一个正方形；四个侧面2为侧靶标面，是等边三角形；四个侧面3是正方形。

图2所示为本发明的一种基于双目视觉的对称运动平台位姿测量方法流程图。首先根据相机视场、平台运动范围、相机安装角等几何参数之间的关系，合理布置双目相机；根据n点透视问题(Perspective n Points,简称PnP)，设计不同形式的靶标。然后分别对单个相机的内参、2个相机以及各个侧靶标面与顶靶标面之间的相对位姿进行标定。最后是在平台运动初始位置建立世界坐标系o_w-x_wy_wz_z，对运动平台进行图像采集与处理，提取靶标信息，对成像效果进行判断，确定靶标解算方法，并根据空间几何关系，解算出运动平台相对世界坐标系的位姿。具体步骤如下：

步骤一：合理布置2个相机，使其组合视场覆盖靶标的运动范围。

(1)合理布置2个相机，应根据视场、距离、安装角等几何参数之间的关系，设计2个相机的安装位置，其几何参数示意图如图3所示。通过设计合理的参数，使得双目相机的组合视场可以完全覆盖靶标的运动范围。其参数设计公式如下：

式中，X、Y分别是左右相机云台安装点相距横梁左右端点的距离；L是空间的水平跨度；H₁、H₂分别是横梁左右端点的垂直高度；α、β分别是左右相机的安装角，即相机主线与横梁的锐角夹角；γ横梁与水平方向的夹角；b是相机的半视场角；D₁、D₂分别是左右相机工作距离限制的最大值；D₃是相机满足另外一个方向宽度限制的最小值；l是相机主线，即云台安装点到相机光心的距离。

步骤二：各靶标面靶标的设计。

(1)靶标的设计基于PnP(Perspective n Points)理论，具体可以根据靶标标志点的形状、大小或数量等物理特征来设计；

(2)可以使用棋盘或是圆点等已有的标定板作为靶标，其目的在于识别不同的靶标面；(3)分别在每个侧靶标面设置3×3、4×4、5×5、6×6的圆点标定板，在顶靶标面设置7×7的圆点标定板。如图4所示为一种常用的圆点标定板。

步骤三：分别对单个相机的内参、2个相机以及各个侧靶标面与顶靶标面之间的相对位姿进行标定。

(1)利用C++程序对单个相机的内参进行标定，获得矫正后的相机的六个内参(f,κ,s_x,s_y,c_x,c_y)。其中，f表示相机的主距，即光心与成像平面之间的垂直距离；κ表示径向畸变量级；s_x、s_y是缩放比例因子，对于针孔相机模型来讲表示图像传感器上水平和垂直方向上相邻像素之间的距离；(c_x,c_y)^T是图像的主点，对于针孔相机模型而言，这个点是投影中心在成像平面上的垂直投影。

(2)利用相机矫正后的内参，对2个相机之间的相对位姿关系进行标定，获得2个相机坐标系之间的齐次变换矩阵

其中，R_c是两个坐标系之间的旋转矩阵，T_c是两个坐标系原点之间的平移向量。

(3)在侧靶标面与顶靶标面建立各自独立的坐标系，利用双目相机计算和提取侧靶标面中的标志点在顶靶标面坐标系o₁-x₁y₁z₁下的表示，然后采用LM算法(Levenberg-Marquardt)，得到两个靶标面之间的相对位姿关系，即两个靶标面局部坐标系之间的齐次变换矩阵

其中，下标1表示顶靶标面，n表示第n个侧靶标面，R_1n表示第n个侧靶标面与顶靶标面局部坐标系之间的旋转矩阵，若规定一个坐标系旋转关系分解为绕坐标系的z轴顺时针旋转ψ角度、y轴顺时针旋转θ角度和x轴顺时针旋转

角度，则

T_1n＝[t_nx t_ny t_nz]^T表示第n个侧靶标面局部坐标系o_n-x_ny_nz_n原点o_n与顶靶标面局部坐标系o₁-x₁y₁z₁原点o₁之间的平移向量，t_nx表示o_n与o₁在相对顶靶标面局部坐标系x轴方向上的平移距离，t_ny表示o_n与o₁在相对顶靶标面局部坐标系y轴方向上的平移距离，t_nz表示o_n与o₁在相对顶靶标面局部坐标系z轴方向上的平移距离。

(4)各个侧靶标面与顶靶标面之间相对位姿标定的意义在于：以侧靶标面为基准解算出的运动平台的相对位姿都需转换成以顶靶标面为基准的相对位姿，使运动平台的位姿变化更具物理意义和直观性。

步骤四：在运动起始位置建立世界坐标系；2个相机分别对运动平台进行图像采集，并依次进行图像滤波、轮廓分割、特征提取与质心定位，提取靶标信息。

步骤五：对成像标志点进行排序，以确定空间点坐标和图像坐标之间的对应关系。提取标定板外轮廓，然后对轮廓进行分割与直线拟合，通过直线求交点获得标点板的四个角点；在图像内描述一个正方形区域，通过投影变换，使标定板变为正投影，并进行矫正；以靠近黑色三角形最近的点作为第一个点，按顺时针行排列对其余点进行排序；对排序后的点进行正投影反变换，得到实际成像下的图像坐标，即完成图像内标志点的排序。

步骤六：对靶标成像效果进行判断，确定靶标解算方法，并根据空间几何关系，解算出运动平台相对初始世界坐标系的位姿。其对应原则如下：

(1)程序检测到包含顶靶标面在内的多个靶标面同时进入相机视野时，仅选择以顶靶标面的7*7靶标成像作为位姿解算的基准；

(2)程序检测到不包含顶靶标面在内的多个侧靶标面同时进入某个相机视野时，出于对精度的考虑，程序通过判断各侧靶标面相对于其相机坐标系o_c-x_cy_cz_c基准平面o_c-x_cy_c的倾斜角度α_n，选择α_n较小的靶标面成像作为位姿解算的基准；其中，下标n表示第n个侧靶标面。

(3)当运动平台在相机组合视场边缘时，顶靶标面成像不完整，程序检测到仅有一个侧靶标面成像时，则以此侧靶标面成像为基准来解算平台的相对位姿；

(4)以侧靶标面为基准解算出的运动平台相对世界坐标系的位姿，还需转换为顶靶标面相对世界坐标系的位姿，以便运动平台的位姿变化更具物理意义和直观性。

位姿转换的数学公式为：

其中，[x_w y_w z_w 1]^T是世界坐标系下任意一点的齐次坐标；[x_n y_n z_n 1]^T是第n个侧靶标面局部坐标系下该点的齐次坐标；[x₁ y₁ z₁ 1]^T是顶靶标面1局部坐标系下该点的齐次坐标；M_wn是第n个侧靶标面局部坐标系与世界坐标系之间的齐次变换矩阵；M_n1是第n个侧靶标面局部坐标系与顶靶标面局部坐标系之间的齐次变换矩阵,即标定矩阵；M_w1是顶靶标面局部坐标系与世界坐标系之间的齐次变换矩阵；Μ由旋转矩阵R和平移向量T组成。

(5)运动平台的相对位姿通过向量

表示，X可根据靶标点在世界坐标系和图像坐标系o-uv之间的对应关系，并考虑相机内参的畸变，采用最小二乘法，解算出表征空间几何关系的齐次矩阵

其中，T＝[t_x t_y t_z]^T是坐标系原点之间的平移向量，t_x、t_y、t_z即是运动平台的位置参数；其中，

是坐标系之间的旋转矩阵，r₁、r₂，…，r₉是其对应的九个元素。则运动平台的姿态参数，偏航角ψ、俯仰角θ、滚转角

可通过以下计算公式求得：

具体实施例的效果：

本位姿测量方法的一种具体工程，其效果图如图5所示，规则、多面、对称运动平台1如图1所示，其各个靶标面设计有不同形式的靶标2；平台1悬浮在机架3构成的空间内，其运动范围大约为1200mm×1200mm的正方形区域，绕平面的旋转角度范围为0°～360°；相机4固定在机架横梁5合适位置上。通过相机4对运动平台1进行图像采集，图像处理，解算出靶标2的位姿信息，即平台的位姿。利用高精度滚珠丝杆对运动平台的动态位置测量结果进行验证。平台沿Y方向匀速运动时的位置解算以图6、图7为例。理论上平台仅沿Y方向匀速运动20mm，而位姿解算程序测得平台沿Y方向实际移动19.9mm，误差仅为0.1mm，同时在X方向上的误差小于0.3mm。

本发明公开了一种基于双目视觉的对称运动平台位姿测量方法。利用双目相机对运动平台进行图像采集，提取靶标信息，通过上位机处理与分析，实时解算出运动平台的位置和姿态。具体方法是在对称平台表面设计不同形式的靶标；合理布置2个相机，使其组合视场完全覆盖靶标的运动范围；分别进行单目相机、双目相机，以及不同靶面之间相对位姿的标定；在运动起始位置建立世界坐标系；对成像靶标综合处理，进行质心定位，并根据成像效果，确定解算原则；通过空间几何关系，实时解算出运动平台的位置和姿态。本发明技术方案采用双目视觉，通过设计有效形式的靶标和高效的标定、解算方法，可实现对称运动平台的高精度位姿测量。

Claims (1)

1.一种基于双目视觉的对称运动平台位姿测量方法，其特征在于包括以下步骤：

1)在平台表面设计不同形式的靶标，用于识别不同的靶标面；所述不同形式的靶标的相关设计原则包括：

(1)靶标的设计基于PnP理论，具体根据靶标标志点的形状、大小或数量物理特征设计；

(2)使用棋盘或是圆点已有的标定板作为靶标，其目的在于程序识别不同的靶标面；

(3)分别在每个侧靶标面设置3×3、4×4、5×5、6×6的圆点标定板，在顶靶标面设置7×7的圆点标定板；

2)设置2个相机，由2个相机组合视场完全覆盖靶标的运动范围，所述设置2个相机，根据视场、距离、安装角几何参数之间的关系，设计2个相机的安装位置，使双目相机的组合视场完全覆盖靶标的运动范围，参数设计公式如下：

式中，X、Y分别是左右相机云台安装点相距横梁左右端点的距离；L是空间的水平跨度；H₁、H₂分别是横梁左右端点的垂直高度；α、β分别是左右相机的安装角，即相机主线与横梁的锐角夹角；γ横梁与水平方向的夹角；b是相机的半视场角；D₁、D₂分别是左右相机工作距离限制的最大值；D₃是相机满足另外一个方向宽度限制的最小值；l是相机主线，即云台安装点到相机光心的距离；

3)分别对单个相机的内参、2个相机和各个侧靶标面与顶靶标面之间的相对位姿进行标定；所述各个侧靶标面与顶靶标面之间的相对位姿进行标定的具体步骤为：

(1)在侧靶标面与顶靶标面建立各自独立的坐标系，利用双目相机计算和提取侧靶标面中的标志点在顶靶标面坐标系o₁-x₁y₁z₁下的表示，然后采用LM算法，得到两个靶标面之间的相对位姿关系，即两个靶标面局部坐标系之间的齐次变换矩阵

同时进行保存；其中，下标1表示顶靶标面，n表示第n个侧靶标面，R_1n表示第n个侧靶标面与顶靶标面局部坐标系之间的旋转矩阵，若规定一个坐标系旋转关系分解为绕坐标系的z轴顺时针旋转ψ角度、y轴顺时针旋转θ角度和x轴顺时针旋转

角度，则

T_1n＝[t_nx t_ny t_nz]^T表示第n个侧靶标面局部坐标系o_n-x_ny_nz_n原点o_n与顶靶标面局部坐标系o₁-x₁y₁z₁原点o₁之间的平移向量，t_nx表示o_n与o₁在顶靶标面局部坐标系x轴方向上的平移距离，t_ny表示o_n与o₁在顶靶标面局部坐标系y轴方向上的平移距离，t_nz表示o_n与o₁在顶靶标面局部坐标系z轴方向上的平移距离；

(2)各个侧靶标面与顶靶标面之间相对位姿标定的意义在于：以侧靶标面为基准解算出的运动平台的相对位姿都需转换成以顶靶标面为基准的相对位姿，使运动平台的位姿变化更具物理意义和直观性；

4)在平台运动初始位置建立世界坐标系o_w-x_wy_wz_z；对运动平台进行图像采集与处理，提取靶标信息，具体步骤为：

(1)采用图像处理提取标定板外轮廓，然后对轮廓进行分割与直线拟合，通过直线求交点获得标点板的四个角点；

(2)在图像内描述一个正方形区域，通过投影变换，使标定板变为正投影，并进行矫正；以靠近黑色三角形最近的点作为第一个点，按顺时针行排列对其余点进行排序；

(3)对排序后的点进行正投影反变换，得到实际成像下的图像坐标，即完成图像内标志点的排序；

5)对成像效果进行判断，确定靶标解算方法，并根据空间几何关系，解算出运动平台相对世界坐标系的位姿；

所述对成像效果进行判断，确定靶标解算方法，对应原则包括：

(1)程序检测到包含顶靶标面在内的多个靶标面同时进入相机视野时，仅选择顶靶标面的7*7靶标成像作为位姿解算的基准；

(2)程序检测到不包含顶靶标面在内的多个侧靶标面同时进入某个相机视野时，出于对精度的考虑，程序通过判断各侧靶标面相对于其相机坐标系o_c-x_cy_cz_c基准平面o_c-x_cy_c的倾斜角度α_n，选择α_n较小的靶标面成像作为位姿解算的基准，其中，下标n表示第n个侧靶标面；

(3)当运动平台在相机组合视场边缘时，顶靶标面成像不完整，程序检测到仅有一个侧靶标面成像时，则以此侧靶标面成像为基准来解算平台的相对位姿；

(4)以侧靶标面为基准解算出的运动平台相对世界坐标系的位姿，还需转换为顶靶标面相对世界坐标系的位姿，以便运动平台的位姿变化更具物理意义和直观性；

位姿转换的数学公式为：

其中，[x_w y_w z_w 1]^T是世界坐标系下任意一点的齐次坐标；[x_n y_n z_n 1]^T是第n个侧靶标面局部坐标系下该点的齐次坐标；[x₁ y₁ z₁ 1]^T是顶靶标面1局部坐标系下该点的齐次坐标；M_wn是第n个侧靶标面局部坐标系与世界坐标系之间的齐次变换矩阵；M_n1是第n个侧靶标面局部坐标系与顶靶标面1局部坐标系之间的齐次变换矩阵,即标定矩阵；M_w1是顶靶标面局部坐标系与世界坐标系之间的齐次变换矩阵；M都由旋转矩阵R和平移向量T组成；

(5)运动平台的相对位姿通过向量

表示，X根据靶标点在世界坐标系和图像坐标系o-uv之间的对应关系，并考虑相机内参的畸变，采用最小二乘法，解算出表征空间几何关系的齐次矩阵

其中，T＝[t_x t_y t_z]^T是坐标系原点之间的平移向量，t_x、t_y、t_z即是运动平台的位置参数；其中，

是坐标系之间的旋转矩阵，r₁、r₂，…，r₉是其对应的九个元素，则运动平台的姿态参数，偏航角ψ、俯仰角θ、滚转角

是通过以下计算公式求得：

Images