一种标靶整体优化的空间位置与姿态解求方法

摘要

本发明公开了一种标靶整体优化的空间位置与姿态解求方法，属于汽车的生产、维修、服务领域。一种标靶整体优化的空间位置与姿态解求方法，将不同状态的标靶成像可以看作是同一标靶点在相机上的成像，把不同状态的相机看作相机在不同的空间位置对标靶摄影，那么就类似于三维重建的相关理论，已知的空间点投影到相机中成像，利用反投影误差最小进行约束，进而进行最小二乘求解，得到相机位置，剔除残差较大的相机(此相机对应着运动中抖动等因素导致的标靶点提取误差较大等)，然后通过反算得到标靶的精确姿态与位置。

说明书

技术领域

本发明涉及汽车的生产、维修、服务领域，具体地说，涉及一种标靶整体优化的空间位置与姿态解求方法。

背景技术

四轮定位仪是用于检测汽车车辆定位装置的精密仪器。四轮定位仪器通过连续的捕捉标靶，进而进行标靶的空间位置与姿态结算，进而进行角度结算等，因此在四轮定位仪中，标靶的定位定姿对于最终结果起着决定性作用。提高标靶的定位定姿的方法，有利于进一步提高四轮定位的解算精度。

但是现有的技术研究与实践过程中发现，现有的四轮定位标靶的空间位置与空间姿态解算过程中，对于同一标靶运动的过程，在空间上形成的轨迹，只是一种简单的空间拟合方式，拟合的曲线与实际情况严重不符，且在推车过程中出现的晃动等因素必定导致拟合出现很大的偏差。

四轮定位仪标靶结算主要存在两种类型：1)无任何优化的直接解求:一般四轮定位仪获取推车停止时刻的图像作为参考，经过计算得到标靶的空间位置与姿态，进而进行四轮定位参数解求，此种方法损失掉大量的中间数据，仅仅依赖静止时的数据，导致结果不稳定。2)在对标靶空间位置进行拟合的方法，总体来说精度较差，若车轮在空间抖动剧烈，拟合方法无法有效的剔除粗差导致拟合出现偏差，影响四轮定位参数解求精度。

本发明提出一种标靶整体优化方法，采用最小二乘法的技术进行标靶的整体的空间位置与姿态优化平差求解，在理论上最完善，在平差过程中剔除因抖动/运动速度过快等因素引起的标靶点提取误差，最大限度的解算标靶的空间位置和姿态，为四轮定位参数解求打下基础。

发明内容

为解决上述技术问题，本发明采用整体优化的方法，对运动标靶的各个空间位置进行优化，并且剔除误差较大的标靶序列，从而实现对于标靶的精确定位定姿，进而用于高精度的四轮定位测量中。该方法主要在于思路的转换，将运动的标靶转换成静止不动的，将静止不动的相机转换成运动的，从而纳入到运动结构恢复的范畴，可以利用最小二乘法进行平差处理。

为实现上述目的，本发明采用如下技术方案：

一种标靶整体优化的空间位置与姿态解求方法，其包括以下步骤：

1)获取标靶的成像点数据，包括标靶的空间姿态R_i、空间位置T_i、成像后的标靶的影像的坐标(u，v)、标靶的实际空间上的坐标值(X，Y)；

2)通过标靶的空间姿态R_i和空间位置T_i，根据空间的几何关系进行标靶—相机公式推导，即可以反求出相机的空间姿态R_i^-1和空间位置-R_i^-1*T_i，此求解出来的相机空间姿态和空间位置作为最小二乘法平差中相机的初始的空间姿态和位置；

3)根据已知的相机内参矩阵A，标靶的空间姿态矩阵R_i，列向量矩阵T_i和尺度因子s，将步骤2)中的标靶—相机公式进行推导，得到标靶在实际空间的坐标值计算值计算该值与实际空间上的坐标值(X，Y)的差值，即得到残差矩阵L；

4)把步骤2)中的标靶—相机公式进行泰勒展开式，针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B；

5)通过平差理论，求解x＝(B^TB)^-1B^TL，其中x为相机相关角度和空间位置的改正值；

6)如果改正值x小于限差，其中，x为相关角度的改正值，则角度值<0.000001弧度，x为空间位置的改正值，空间位置<0.000001，则平差收敛，通过改正后的角度值计算出优化以后的相机空间位置与姿态，并且可以反解出标靶的空间位置与空间姿态；

7)如果改正值x大于限差，其中，x为相关角度的改正值，则角度值﹥0.000001弧度，x为空间位置的改正值，空间位置﹥0.000001，则需要进行下一次的迭代，返回步骤3，对步骤3的残差矩阵L进行分析，将残差矩阵L对应的标靶空间坐标点(X，Y)通过标靶—相机公式投影到影像上来，计算其投影点与对应影像的坐标点(u，v)的差值，如果标靶点残差大于2.0像素，则该点属于噪点，则将其剔除后再次进行平差，如果剔除的数据点大于总点数的20％，则认为该标靶影像获取效果较差，将其作为候选的误差标靶；

8)根据步骤6得到的标靶的空间姿态，将标靶空间坐标点(X，Y)通过标靶—相机公式投影到影像上来，计算其投影点与对应坐标点(u，v)的误差，并统计其平均误差，利用该平均误差对步骤7中的候选的误差标靶进行判断其是否为错误影像帧，如果影像平均误差大于1.5像素，则认为该影像为误差影像，将其进行剔除，不参与后续的车轮的姿态计算。

进一步的技术方案，所述的步骤1)中获取标靶的空间姿态R_i、空间位置T_i的方法，包括以下步骤：

11)通过四轮定位仪的相机获取标靶的起始影像和实时影像；

12)提取标靶的起始影像和实时影像中的椭圆数据，对椭圆数据进行排列；

13)计算标靶的空间位置T、空间姿态R，包括起始影像的空间位置T₀、空间姿态R₀和实时影像的空间位置T_i、空间姿态R_i；

14)通过实时影像的空间姿态R_i与起始影像的空间姿态R₀，获得起始影像到实时影像的标靶的空间姿态关系：R_i0＝Ri*R₀^-1，通过罗德里格斯公式求解旋转轴，获得旋转的角度；

15)如果旋转的角度满足一定条件，比如每间隔2度获取一张图片，将其所求解的影像的标靶点坐标(u，v)与相对应的实际的空间点坐标(X,Y)、标靶的空间位置Ti和标靶的空间姿态Ri保存，作为下一步的数据。

进一步的技术方案，所述的步骤13)中计算标靶的空间位置T、空间姿态R的方法，包括以下步骤：

131)成像后的标靶的影像的坐标(u，v)与标靶的实际空间上的坐标值(X，Y)之间的对应关系用公式1表示:

其中，s为尺度因子，r1、r2和r3分别为标靶的姿态R的列向量，t为标靶的空间位置T的列向量；

132)标靶在相机上成像的点与实际中的标靶上的某一点唯一对应可通过数学几何的方式，采用单应性矩阵H来联系，对于平面模板来说，假设其位于世界坐标系Z＝0的地方，同时，用r_i表示旋转矩阵R的第i列，由公式1简化得公式2：

sm＝HM

H＝A[r₁>2>

其中，A为摄像机内参数矩阵，H＝(h₁₁，h₁₂，h₁₃，h₂₁，h₂₂，h₂₃，h₃₁，h₃₂，h₃₃)，可以得到2N个关于h的方程，根据已知的标靶点成像空间坐标(u，v)和标靶点在标靶中的空间位置(X，Y)，通过线性方程求解可求解出H的最优解；

133)相机标定后，利用相机的内参矩阵A和前面所求的单应矩阵H，即可以反求标靶的空间位置T和空间姿态R，得公式3：

其中，λ＝1/||A^-1h₁||＝1/||A^-1h₂||，h₁，h₂，h₃为H的列向量，T为平移向量。

进一步的技术方案，述的步骤133)中，计算出的空间姿态R可以进一步优化，具体方法如下：

对R进行奇异值分解，即R＝USV^T，其中，S＝diag(σ₁，σ₂，σ₃)，则Q＝UV^T为R的最佳估计矩阵。

进一步的技术方案，所述的步骤14)中，旋转矩阵R可以求出对应的旋转角度的求解方法如下：

假设先绕着Y轴旋转角度继而绕着X轴旋转ω，最后绕着Z轴旋转κ，则相应的旋转矩阵R为:

其中a1，a2，a3，b1，b2，b3，c1，c2，c3为相乘后的对应的矩阵的元素；

其中由旋转矩阵R可以求出对应的旋转角度，如下所示:

ω＝-arcsin b₃。

κ＝arctan(b₁/b₂)

进一步的技术方案，所述的步骤2)中，相机的空间姿态R_i^-1和空间位置-R_i^-1*T_i的计算方法方法，包括以下步骤：

21)成像后的标靶的影像的坐标(u，v)与标靶的实际空间上的坐标值(X，Y)之间的对应关系用公式1表示:

其中，s为尺度因子，r1、r2和r3分别为标靶的空间姿态R的列向量，t为标靶的空间位置T的列向量；A为相机内参矩阵；

22)由公式1，可以得到推导得到公式4：

其中，s为尺度因子，r1、r2和r3分别为标靶的空间姿态R的列向量，t为标靶的空间位置T的列向量；A为相机内参矩阵,大小为3*3，R为标靶的空间姿态矩阵，大小为3*3，T为列向量，大小为3*1，其中矩阵A和矩阵R是正定矩阵，可逆。

23)由公式4，可以得到推导得到公式5：

24)由公式5，可以得到推导得到公式6：

即得到标靶实际空间的坐标值与标靶成像的像空间坐标值之间的关系，其中R^-1和-R^-1T分别代表了相机的空间姿态和空间位置。其中空间姿态采用围绕着X、Y、Z轴旋转的三个角度值表示，绕着不同的旋转轴旋转的先后顺序决定。

进一步的技术方案，所述的步骤3)中，实际空间的坐标值计算值的计算方法是，对公式6进行推导，得到公式7；

所述的步骤4)中，泰勒展开式，假设函数f(x)在x＝x0处连续，则f(x₀+δx)≈f(x₀)+f(x₀)′*δx，其中δx是微小量，省略了其二次项以后的数据项，把步骤2)中的标靶—相机公式进行泰勒展开式，即对公式5泰勒展开式，也可以展开表示成类似的形式，针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B。

本发明的方法是将不同状态的标靶成像可以看作是同一标靶点在相机上的成像，把不同状态的相机看作相机在不同的空间位置对标靶摄影，那么就类似于三维重建的相关理论，已知的空间点投影到相机中成像，利用反投影误差最小进行约束，进而进行最小二乘求解，得到相机位置，剔除残差较大的相机(此相机对应着运动中抖动等因素导致的标靶点提取误差较大等)，然后通过反算得到标靶的精确姿态与位置。

本发明的方法主要有两部分，即数据筛选保存以及标靶数据的整体优化。如果车辆位移很小，相关标靶数据的保存没有任何意义，因此需要进行数据的相关筛选与保存，筛选的条件是通过判断实时获取的影像与起始影像之间的角度，进而进行影像的筛选。

有益效果

与现有技术相比，本发明具有如下显著优点：

1、本发明通过对标靶进行整体优化求解，使标靶得到的空间姿态更加合理与准确。更加有利于四轮定位相关参数的解求。

2、本发明通过对标靶的不同时间数据进行筛选，在尽可能获取较多影像数据的同时，去除了影像间位移较小的数据，使得优化结果更加稳定可靠。

3、本发明通过迭代权值自动筛选误差较大的标靶点，尽可能的去除了车辆抖动等因素导致的优化精度太差等，使得位姿求解更加精确。

附图说明

图1为排列后的标靶在相机上成像的点与实际中的标靶上的点的示意图；

图2为标靶的运动示意图；

图3为图2中的1号标靶不动，相机相对位置运动示意图；

图4为获得标靶的空间姿态R_i、空间位置T_i的流程示意图

图5为标靶整体优化的空间位置与姿态解求方法的流程示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步详细的说明。

实施例

如图2所示，对一个标靶运动中形成多个影像序列，其中假设标靶从状态1运动至状态4，相机位置不动，如图3所示，其中车辆在状态2的位置发生了部分晃动，标靶在空间位置姿态上发生改变，需要对其空间位置进行优化，并剔除误差较大的标靶序列，从而实现对于标靶的精确定位定姿。

如图5所示，一种标靶整体优化的空间位置与姿态解求方法，其包括以下步骤：

1)获取标靶的成像点数据，包括标靶的空间姿态R_i、空间位置T_i、成像后的标靶的影像的坐标(u，v)、标靶的实际空间上的坐标值(X，Y)；

2)通过标靶的空间姿态R_i和空间位置T_i，根据空间的几何关系进行标靶—相机公式推导，即可以反求出相机的空间姿态R_i^-1和空间位置-R_i^-1*T_i，此求解出来的相机空间姿态和空间位置作为最小二乘法平差中相机的初始的空间姿态和位置；

3)根据已知的相机内参矩阵A，标靶的空间姿态矩阵R_i，列向量矩阵T_i和尺度因子s，将步骤2)中的标靶—相机公式进行推导，得到标靶在实际空间的坐标值计算值计算该值与实际空间上的坐标值(X，Y)的差值，即得到残差矩阵L；

4)把步骤2)中的标靶—相机公式进行泰勒展开式，也针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B；

5)通过平差理论，求解x＝(B^TB)^-1B^TL，其中x为相机相关角度和空间位置的改正值；

6)如果改正值x小于限差，其中，x为相关角度的改正值，则角度值<0.000001弧度，x为空间位置的改正值，空间位置<0.000001，则平差收敛，通过改正后的角度值计算出优化以后的相机空间位置与姿态，并且可以反解出标靶的空间位置与姿态。

7)如果改正值x大于限差，其中，x为相关角度的改正值，则角度值﹥0.000001弧度，x为空间位置的改正值，空间位置﹥0.000001，则需要进行下一次的迭代，返回步骤3，对步骤3的残差矩阵L进行分析，我们将残差矩阵L对应的标靶空间坐标点(X，Y)通过标靶—相机公式投影到影像上来，计算其投影点与对应影像的坐标点(u，v)的差值，如果标靶点残差大于2.0像素，我们认为该点属于噪点，则将其剔除后再次进行平差，如果剔除的数据点大于总点数的20％，则认为该标靶影像获取效果较差，我们将其作为误差标靶的候选。如表1所示，图2中，状态2下，30％的点残差大于2.0像素，状态3下，20％的点残差大于2.0像素。如果剔除的点大于50％，我们则认为该图获取完全不正确，退出优化，剔除影像。反之，则进行下一次平差解算。

8)在得到标靶姿态后，我们将空间坐标点(X，Y)通过标靶—相机公式投影到影像上来，计算其投影点与对应坐标点(u，v)的误差，并统计其平均误差，利用该平均误差对候选的误差影像进行判断其是否为错误影像帧。如果影像平均误差大于1.5像素，则认为该影像为误差影像。如表1中所示，相机在各个状态下的平均投影像素误差分别0.5、1.9、1.0、0.8，其中状态2下，影像平均投影误差大于1.5像素，我们认为在此期间获取的影像效果不佳，导致误差较大，因此将状态2下的影像进行剔除，不参与后续的标靶姿态计算。

表1

状态1状态2状态3状态4剔除点/所有点2/259/255/254/25平均投影误差0.51.91.00.8

如图4所示，进一步的技术方案，所述的步骤1)中获取标靶的空间姿态R_i、空间位置T_i的方法，包括以下步骤：

11)通过四轮定位仪的相机获取标靶的起始影像和实时影像；

12)提取标靶的起始影像和实时影像中的椭圆数据，对椭圆数据进行排列；

13)计算标靶的空间位置T、空间姿态R，包括起始影像的空间位置T₀、空间姿态R₀和实时影像的空间位置Ti、空间姿态Ri；

14)通过实时影像的空间姿态Ri与起始影像的空间姿态R₀，获得起始影像到实时影像的标靶的空间姿态关系：R_i0＝Ri*R₀^-1，通过罗德里格斯公式求解旋转轴，获得旋转的角度；

15)如果旋转的角度满足一定条件(比如每间隔2度获取一张图片)，将其所求解的影像的标靶点坐标(u，v)与相对应的实际的空间点坐标(X,Y)、标靶的空间位置Ti和标靶的空间姿态Ri保存，作为下一步的数据。

如图1所示，通过对提取后的标靶点进行有序的排列，图中显示的即为排列后的结果，通过绘图的连线可以看出，即可以找到标靶在相机上成像的点与实际中的标靶上的某一点唯一对应。这种对应关系可通过数学几何的方式，采用单应性矩阵H来联系。

所述的步骤13)中计算标靶的空间位置T、空间姿态R的方法，包括以下步骤：

131)成像后的标靶的影像的坐标(u，v)与标靶的实际空间上的坐标值(X，Y)之间的对应关系用公式1表示:

其中，s为尺度因子，r1、r2和r3分别为标靶的姿态R的列向量，t为标靶的空间位置T的列向量；

132)标靶在相机上成像的点与实际中的标靶上的某一点唯一对应可通过数学几何的方式，采用单应性矩阵H来联系，对于平面模板来说，假设其位于世界坐标系Z＝0的地方，同时，用r_i表示旋转矩阵R的第i列，由公式1简化得公式2：

sm＝HM

H＝A[r₁>2>

其中，A为摄像机内参数矩阵，H＝(h₁₁，h₁₂，h₁₃，h₂₁，h₂₂，h₂₃，h₃₁，h₃₂，h₃₃)，可以得到2N个关于h的方程，根据已知的标靶点成像空间坐标(u，v)和标靶点在标靶中的空间位置(X，Y)，通过线性方程求解可求解出H的最优解；

133)相机标定后，利用相机的内参矩阵A和前面所求的单应矩阵H，即可以反求标靶的空间位置T和空间姿态R，得公式3：

其中，λ＝1/||A^-1h₁||＝1/||A^-1h₂||，h₁，h₂，h₃为H的列向量，T为平移向量。

述的步骤133)中，计算出的空间姿态R可以进一步优化，具体方法如下：

对R进行奇异值分解，即R＝USV^T，其中，S＝diag(σ₁，σ₂，σ₃)，则Q＝UV^T为R的最佳估计矩阵。

所述的步骤14)中，旋转矩阵R可以求出对应的旋转角度的求解方法如下：

假设先绕着Y轴旋转角度继而绕着X轴旋转ω，最后绕着Z轴旋转κ，则相应的旋转矩阵R为:

其中a1，a2，a3，b1，b2，b3，c1，c2，c3为相乘后的对应的矩阵的元素；

其中由旋转矩阵R可以求出对应的旋转角度，如下所示:

ω＝-arcsin b₃。

κ＝arctan(b₁/b₂)

进一步的技术方案，所述的步骤2)中，相机的空间姿态R_i^-1和空间位置-R_i^-1*T_i的计算方法方法，包括以下步骤：

21)成像后的标靶的影像的坐标(u，v)与标靶的实际空间上的坐标值(X，Y)之间的对应关系用公式1表示:

其中，s为尺度因子，r1、r2和r3分别为标靶的空间姿态R的列向量，t为标靶的空间位置T的列向量；A为相机内参矩阵；

22)由公式1，可以得到推导得到公式4：

其中，s为尺度因子，r1、r2和r3分别为标靶的空间姿态R的列向量，t为标靶的空间位置T的列向量；A为相机内参矩阵,大小为3*3，R为标靶的空间姿态矩阵，大小为3*3，T为列向量，大小为3*1，其中矩阵A和矩阵R是正定矩阵，可逆。

23)由公式4，可以得到推导得到公式5：

24)由公式5，可以得到推导得到公式6：

即得到标靶实际空间的坐标值与标靶成像的像空间坐标值之间的关系，其中R^-1和-R^-1T分别代表了相机的空间姿态和空间位置。其中空间姿态采用围绕着X、Y、Z轴旋转的三个角度值表示，绕着不同的旋转轴旋转的先后顺序决定了。

所述的步骤3)中，实际空间的坐标值计算值的计算方法是，对公式6进行推导，得到公式7；

所述的步骤4)中，泰勒展开式，假设函数f(x)在x＝x0处连续，则

f(x₀+δx)≈f(x₀)+f(x₀)′*δx，其中δx是微小量，省略了其二次项以后的数据项，把步骤2)中的标靶—相机公式进行泰勒展开式，即对公式5泰勒展开式，也可以展开表示成类似的形式，针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B。

为了验证算法的有效性，我们设计了对比试验，试验结果如下表：

表2：

表3：

表4：

表2是正常推车获取的标靶数据结果，表3中的状态2添加了扰动干扰，未加优化获取的标靶数据结果，表4中的状态2添加了扰动干扰，但是利用本发明的算法去除了扰动带来的数据误差的标靶数据结果。对比发现，本发明的算法的结果与正常推车获取的结果精度基本一致，在有扰动干扰的情况下，也可以获取较好的优化结果，算法稳定性强，能满足实际应用。