CN107576286B - 一种标靶整体优化的空间位置与姿态解求方法 - Google Patents
一种标靶整体优化的空间位置与姿态解求方法 Download PDFInfo
- Publication number
- CN107576286B CN107576286B CN201710786093.9A CN201710786093A CN107576286B CN 107576286 B CN107576286 B CN 107576286B CN 201710786093 A CN201710786093 A CN 201710786093A CN 107576286 B CN107576286 B CN 107576286B
- Authority
- CN
- China
- Prior art keywords
- target
- camera
- spatial
- spatial position
- formula
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000005457 optimization Methods 0.000 title claims abstract description 26
- 238000003384 imaging method Methods 0.000 claims abstract description 28
- 239000011159 matrix material Substances 0.000 claims description 60
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000002441 reversible effect Effects 0.000 claims description 8
- 238000012937 correction Methods 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 4
- YBJHBAHKTGYVGT-ZKWXMUAHSA-N (+)-Biotin Chemical compound N1C(=O)N[C@@H]2[C@H](CCCCC(=O)O)SC[C@@H]21 YBJHBAHKTGYVGT-ZKWXMUAHSA-N 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 229910052739 hydrogen Inorganic materials 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- FEPMHVLSLDOMQC-UHFFFAOYSA-N virginiamycin-S1 Natural products CC1OC(=O)C(C=2C=CC=CC=2)NC(=O)C2CC(=O)CCN2C(=O)C(CC=2C=CC=CC=2)N(C)C(=O)C2CCCN2C(=O)C(CC)NC(=O)C1NC(=O)C1=NC=CC=C1O FEPMHVLSLDOMQC-UHFFFAOYSA-N 0.000 claims description 3
- 238000012423 maintenance Methods 0.000 abstract description 2
- 238000004519 manufacturing process Methods 0.000 abstract description 2
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 238000012216 screening Methods 0.000 description 4
- 238000009795 derivation Methods 0.000 description 3
- 238000004321 preservation Methods 0.000 description 2
- 238000012163 sequencing technique Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Abstract
本发明公开了一种标靶整体优化的空间位置与姿态解求方法，属于汽车的生产、维修、服务领域。一种标靶整体优化的空间位置与姿态解求方法，将不同状态的标靶成像可以看作是同一标靶点在相机上的成像，把不同状态的相机看作相机在不同的空间位置对标靶摄影，那么就类似于三维重建的相关理论，已知的空间点投影到相机中成像，利用反投影误差最小进行约束，进而进行最小二乘求解，得到相机位置，剔除残差较大的相机(此相机对应着运动中抖动等因素导致的标靶点提取误差较大等)，然后通过反算得到标靶的精确姿态与位置。
Description
技术领域
本发明涉及汽车的生产、维修、服务领域，具体地说，涉及一种标靶整体优化的空间位置与姿态解求方法。
背景技术
四轮定位仪是用于检测汽车车辆定位装置的精密仪器。四轮定位仪器通过连续的捕捉标靶，进而进行标靶的空间位置与姿态结算，进而进行角度结算等，因此在四轮定位仪中，标靶的定位定姿对于最终结果起着决定性作用。提高标靶的定位定姿的方法，有利于进一步提高四轮定位的解算精度。
但是现有的技术研究与实践过程中发现，现有的四轮定位标靶的空间位置与空间姿态解算过程中，对于同一标靶运动的过程，在空间上形成的轨迹，只是一种简单的空间拟合方式，拟合的曲线与实际情况严重不符，且在推车过程中出现的晃动等因素必定导致拟合出现很大的偏差。
四轮定位仪标靶结算主要存在两种类型：1)无任何优化的直接解求:一般四轮定位仪获取推车停止时刻的图像作为参考，经过计算得到标靶的空间位置与姿态，进而进行四轮定位参数解求，此种方法损失掉大量的中间数据，仅仅依赖静止时的数据，导致结果不稳定。2)在对标靶空间位置进行拟合的方法，总体来说精度较差，若车轮在空间抖动剧烈，拟合方法无法有效的剔除粗差导致拟合出现偏差，影响四轮定位参数解求精度。
本发明提出一种标靶整体优化方法，采用最小二乘法的技术进行标靶的整体的空间位置与姿态优化平差求解，在理论上最完善，在平差过程中剔除因抖动/运动速度过快等因素引起的标靶点提取误差，最大限度的解算标靶的空间位置和姿态，为四轮定位参数解求打下基础。
发明内容
为解决上述技术问题，本发明采用整体优化的方法，对运动标靶的各个空间位置进行优化，并且剔除误差较大的标靶序列，从而实现对于标靶的精确定位定姿，进而用于高精度的四轮定位测量中。该方法主要在于思路的转换，将运动的标靶转换成静止不动的，将静止不动的相机转换成运动的，从而纳入到运动结构恢复的范畴，可以利用最小二乘法进行平差处理。
为实现上述目的，本发明采用如下技术方案：
一种标靶整体优化的空间位置与姿态解求方法，其包括以下步骤：
1)获取标靶的成像点数据，包括标靶的空间姿态Ri、空间位置Ti、成像后的标靶的影像的坐标(u，v)、标靶的实际空间上的坐标值(X，Y)；
2)通过标靶的空间姿态Ri和空间位置Ti，根据空间的几何关系进行标靶—相机公式推导，即可以反求出相机的空间姿态Ri -1和空间位置-Ri -1*Ti，此求解出来的相机空间姿态和空间位置作为最小二乘法平差中相机的初始的空间姿态和位置；
3)根据已知的相机内参矩阵A，标靶的空间姿态矩阵Ri，列向量矩阵Ti和尺度因子s，将步骤2)中的标靶—相机公式进行推导，得到标靶在实际空间的坐标值计算值计算该值与实际空间上的坐标值(X，Y)的差值，即得到残差矩阵L；
4)把步骤2)中的标靶—相机公式进行泰勒展开式，针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B；
5)通过平差理论，求解x＝(BTB)-1BTL，其中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)中获取标靶的空间姿态Ri、空间位置Ti的方法，包括以下步骤：
11)通过四轮定位仪的相机获取标靶的起始影像和实时影像；
12)提取标靶的起始影像和实时影像中的椭圆数据，对椭圆数据进行排列；
13)计算标靶的空间位置T、空间姿态R，包括起始影像的空间位置T0、空间姿态R0和实时影像的空间位置Ti、空间姿态Ri；
14)通过实时影像的空间姿态Ri与起始影像的空间姿态R0，获得起始影像到实时影像的标靶的空间姿态关系：Ri0＝Ri*R0 -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的地方，同时，用ri表示旋转矩阵R的第i列，由公式1简化得公式2：
sm＝HM
H＝A[r1 r2 t]
其中，A为摄像机内参数矩阵，H＝(h11，h12，h13，h21，h22，h23，h31，h32，h33)，可以得到2N个关于h的方程，根据已知的标靶点成像空间坐标(u，v)和标靶点在标靶中的空间位置(X，Y)，通过线性方程求解可求解出H的最优解；
133)相机标定后，利用相机的内参矩阵A和前面所求的单应矩阵H，即可以反求标靶的空间位置T和空间姿态R，得公式3：
其中，λ＝1/||A-1h1||＝1/||A-1h2||，h1，h2，h3为H的列向量，T为平移向量。
进一步的技术方案，述的步骤133)中，计算出的空间姿态R可以进一步优化，具体方法如下：
对R进行奇异值分解，即R＝USVT，其中，S＝diag(σ1，σ2，σ3)，则Q＝UVT为R的最佳估计矩阵。
进一步的技术方案，所述的步骤14)中，旋转矩阵R可以求出对应的旋转角度的求解方法如下：
假设先绕着Y轴旋转角度继而绕着X轴旋转ω，最后绕着Z轴旋转κ，则相应的旋转矩阵R为:
其中a1，a2，a3，b1，b2，b3，c1，c2，c3为相乘后的对应的矩阵的元素；
其中由旋转矩阵R可以求出对应的旋转角度，如下所示:
ω＝-arcsin b3。
κ＝arctan(b1/b2)
进一步的技术方案，所述的步骤2)中，相机的空间姿态Ri -1和空间位置-Ri -1*Ti的计算方法方法，包括以下步骤：
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(x0+δx)≈f(x0)+f(x0)′*δx，其中δx是微小量，省略了其二次项以后的数据项，把步骤2)中的标靶—相机公式进行泰勒展开式，即对公式5泰勒展开式，也可以展开表示成类似的形式，针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B。
本发明的方法是将不同状态的标靶成像可以看作是同一标靶点在相机上的成像，把不同状态的相机看作相机在不同的空间位置对标靶摄影，那么就类似于三维重建的相关理论，已知的空间点投影到相机中成像，利用反投影误差最小进行约束，进而进行最小二乘求解，得到相机位置，剔除残差较大的相机(此相机对应着运动中抖动等因素导致的标靶点提取误差较大等)，然后通过反算得到标靶的精确姿态与位置。
本发明的方法主要有两部分，即数据筛选保存以及标靶数据的整体优化。如果车辆位移很小，相关标靶数据的保存没有任何意义，因此需要进行数据的相关筛选与保存，筛选的条件是通过判断实时获取的影像与起始影像之间的角度，进而进行影像的筛选。
有益效果
与现有技术相比，本发明具有如下显著优点：
1、本发明通过对标靶进行整体优化求解，使标靶得到的空间姿态更加合理与准确。更加有利于四轮定位相关参数的解求。
2、本发明通过对标靶的不同时间数据进行筛选，在尽可能获取较多影像数据的同时，去除了影像间位移较小的数据，使得优化结果更加稳定可靠。
3、本发明通过迭代权值自动筛选误差较大的标靶点，尽可能的去除了车辆抖动等因素导致的优化精度太差等，使得位姿求解更加精确。
附图说明
图1为排列后的标靶在相机上成像的点与实际中的标靶上的点的示意图；
图2为标靶的运动示意图；
图3为图2中的1号标靶不动，相机相对位置运动示意图；
图4为获得标靶的空间姿态Ri、空间位置Ti的流程示意图
图5为标靶整体优化的空间位置与姿态解求方法的流程示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步详细的说明。
实施例
如图2所示，对一个标靶运动中形成多个影像序列，其中假设标靶从状态1运动至状态4，相机位置不动，如图3所示，其中车辆在状态2的位置发生了部分晃动，标靶在空间位置姿态上发生改变，需要对其空间位置进行优化，并剔除误差较大的标靶序列，从而实现对于标靶的精确定位定姿。
如图5所示，一种标靶整体优化的空间位置与姿态解求方法，其包括以下步骤：
1)获取标靶的成像点数据，包括标靶的空间姿态Ri、空间位置Ti、成像后的标靶的影像的坐标(u，v)、标靶的实际空间上的坐标值(X，Y)；
2)通过标靶的空间姿态Ri和空间位置Ti，根据空间的几何关系进行标靶—相机公式推导，即可以反求出相机的空间姿态Ri -1和空间位置-Ri -1*Ti，此求解出来的相机空间姿态和空间位置作为最小二乘法平差中相机的初始的空间姿态和位置；
3)根据已知的相机内参矩阵A，标靶的空间姿态矩阵Ri，列向量矩阵Ti和尺度因子s，将步骤2)中的标靶—相机公式进行推导，得到标靶在实际空间的坐标值计算值计算该值与实际空间上的坐标值(X，Y)的差值，即得到残差矩阵L；
4)把步骤2)中的标靶—相机公式进行泰勒展开式，也针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B；
5)通过平差理论，求解x＝(BTB)-1BTL，其中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/25 | 9/25 | 5/25 | 4/25 |
| 平均投影误差 | 0.5 | 1.9 | 1.0 | 0.8 |
如图4所示，进一步的技术方案，所述的步骤1)中获取标靶的空间姿态Ri、空间位置Ti的方法，包括以下步骤：
11)通过四轮定位仪的相机获取标靶的起始影像和实时影像；
12)提取标靶的起始影像和实时影像中的椭圆数据，对椭圆数据进行排列；
13)计算标靶的空间位置T、空间姿态R，包括起始影像的空间位置T0、空间姿态R0和实时影像的空间位置Ti、空间姿态Ri；
14)通过实时影像的空间姿态Ri与起始影像的空间姿态R0，获得起始影像到实时影像的标靶的空间姿态关系：Ri0＝Ri*R0 -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的地方，同时，用ri表示旋转矩阵R的第i列，由公式1简化得公式2：
sm＝HM
H＝A[r1 r2 t]
其中，A为摄像机内参数矩阵，H＝(h11，h12，h13，h21，h22，h23，h31，h32，h33)，可以得到2N个关于h的方程，根据已知的标靶点成像空间坐标(u，v)和标靶点在标靶中的空间位置(X，Y)，通过线性方程求解可求解出H的最优解；
133)相机标定后，利用相机的内参矩阵A和前面所求的单应矩阵H，即可以反求标靶的空间位置T和空间姿态R，得公式3：
其中，λ＝1/||A-1h1||＝1/||A-1h2||，h1，h2，h3为H的列向量，T为平移向量。
述的步骤133)中，计算出的空间姿态R可以进一步优化，具体方法如下：
对R进行奇异值分解，即R＝USVT，其中，S＝diag(σ1，σ2，σ3)，则Q＝UVT为R的最佳估计矩阵。
所述的步骤14)中，旋转矩阵R可以求出对应的旋转角度的求解方法如下：
假设先绕着Y轴旋转角度继而绕着X轴旋转ω，最后绕着Z轴旋转κ，则相应的旋转矩阵R为:
其中a1，a2，a3，b1，b2，b3，c1，c2，c3为相乘后的对应的矩阵的元素；
其中由旋转矩阵R可以求出对应的旋转角度，如下所示:
ω＝-arcsin b3。
κ＝arctan(b1/b2)
进一步的技术方案，所述的步骤2)中，相机的空间姿态Ri -1和空间位置-Ri -1*Ti的计算方法方法，包括以下步骤：
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(x0+δx)≈f(x0)+f(x0)′*δx，其中δx是微小量，省略了其二次项以后的数据项，把步骤2)中的标靶—相机公式进行泰勒展开式，即对公式5泰勒展开式，也可以展开表示成类似的形式，针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B。
为了验证算法的有效性，我们设计了对比试验，试验结果如下表：
表2：
表3：
表4：
表2是正常推车获取的标靶数据结果，表3中的状态2添加了扰动干扰，未加优化获取的标靶数据结果，表4中的状态2添加了扰动干扰，但是利用本发明的算法去除了扰动带来的数据误差的标靶数据结果。对比发现，本发明的算法的结果与正常推车获取的结果精度基本一致，在有扰动干扰的情况下，也可以获取较好的优化结果，算法稳定性强，能满足实际应用。
Claims (7)
1.一种标靶整体优化的空间位置与姿态解求方法，其特征在于，包括以下步骤：
1)获取标靶的成像点数据，包括标靶的空间姿态Ri、空间位置Ti、成像后的标靶的影像的坐标(u，v)、标靶的实际空间上的坐标值(X，Y)；
2)通过标靶的空间姿态Ri和空间位置Ti，根据空间的几何关系进行标靶—相机公式推导，即反求出相机的空间姿态Ri -1和空间位置-Ri -1*Ti；
3)根据已知的相机内参矩阵矩阵A，标靶的空间姿态矩阵Ri，列向量矩阵Ti和尺度因子s，将步骤2)中的标靶—相机公式进行推导，得到标靶在实际空间的坐标值计算值计算该值与实际空间上的坐标值(X，Y)的差值，即得到残差矩阵L；
4)把步骤2)中的标靶—相机公式进行泰勒展开式，也针对公式中的多个变量进行求导，获得的相关导数求解出来的数值，作为误差矩阵B；
5)通过平差理论，求解x＝(BTB)-1BTL，其中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像素，则认为该影像为误差影像，将其进行剔除。
2.根据权利要求1所述的标靶整体优化的空间位置与姿态解求方法，其特征在于，所述的步骤1)中获取标靶的空间姿态Ri、空间位置Ti的方法，包括以下步骤：
11)通过四轮定位仪的相机获取标靶的起始影像和实时影像；
12)提取标靶的起始影像和实时影像中的椭圆数据，对椭圆数据进行排列；
13)计算标靶的空间位置T、空间姿态R，包括起始影像的空间位置T0、空间姿态R0和实时影像的空间位置Ti、空间姿态Ri；
14)通过实时影像的空间姿态Ri与起始影像的空间姿态R0，获得起始影像到实时影像的标靶的空间姿态关系：Ri0＝Ri*R0 -1，通过罗德里格斯公式求解旋转轴，获得旋转的角度；
15)如果旋转的角度满足一定条件，将其所求解的影像的标靶点坐标(u，v)与相对应的实际的空间点坐标(X,Y)、标靶的空间位置Ti和标靶的空间姿态Ri保存，作为下一步的数据。
3.根据权利要求2所述的标靶整体优化的空间位置与姿态解求方法，其特征在于，所述的步骤13)中计算标靶的空间位置T、空间姿态R的方法，包括以下步骤：
131)成像后的标靶的影像的坐标(u，v)与标靶的实际空间上的坐标值(X，Y)之间的对应关系用公式1表示:
其中，s为尺度因子，r1、r2和r3分别为标靶的姿态R的列向量，t为标靶的空间位置T的列向量；
132)标靶在相机上成像的点与实际中的标靶上的某一点唯一对应可通过数学几何的方式，采用单应性矩阵H来联系，对于平面模板来说，假设其位于世界坐标系Z＝0的地方，同时，用ri表示旋转矩阵R的第i列，由公式1简化得公式2：
sm＝HM
H＝A[r1 r2 t]
其中，A为摄像机内参数矩阵，H＝(h11，h12，h13，h21，h22，h23，h31，h32，h33)，得到2N个关于h的方程，根据已知的标靶点成像空间坐标(u，v)和标靶点在标靶中的空间位置(X，Y)，通过线性方程求解可求解出H的最优解；
133)相机标定后，利用相机的内参矩阵A和前面所求的单应矩阵H，即反求标靶的空间位置T和空间姿态R，得公式3：
其中，λ＝1/||A-1h1||＝1/||A-1h2||，h1，h2，h3为H的列向量，T为平移向量。
4.根据权利要求3所述的标靶整体优化的空间位置与姿态解求方法，其特征在于，所述的步骤133)中，计算出的空间姿态R进一步优化，具体方法如下：对R进行奇异值分解，即R＝USVT，其中，S＝diag(σ1，σ2，σ3)，则Q＝UVT为R的最佳估计矩阵。
5.根据权利要求2所述的标靶整体优化的空间位置与姿态解求方法，其特征在于，所述的步骤14)中，旋转矩阵R求出对应的旋转角度的求解方法如下：
假设先绕着Y轴旋转角度继而绕着X轴旋转ω，最后绕着Z轴旋转κ，则相应的旋转矩阵R为:
其中a1，a2，a3，b1，b2，b3，c1，c2，c3为相乘后的对应的矩阵的元素；
其中由旋转矩阵R求出对应的旋转角度，如下所示:
6.根据权利要求1所述的标靶整体优化的空间位置与姿态解求方法，其特征在于，所述的步骤2)中，相机的空间姿态Ri -1和空间位置-Ri -1*Ti的计算方法方法，包括以下步骤：
21)成像后的标靶的影像的坐标(u，v)与标靶的实际空间上的坐标值(X，Y)之间的对应关系用公式1表示:
其中，s为尺度因子，r1、r2和r3分别为标靶的姿态R的列向量，t为标靶的空间位置T的列向量；
22)由公式1，得到推导得到公式4：
23)由公式4，得到推导得到公式5：
24)由公式5，得到推导得到公式6：
即得到标靶实际空间的坐标值与标靶成像的像空间坐标值之间的关系，其中R-1和-R-1T分别代表了相机的空间姿态和空间位置。
7.根据权利要求6所述的标靶整体优化的空间位置与姿态解求方法，其特征在于，所述的步骤3)中，实际空间的坐标值计算值的计算方法是，对公式6进行推导，得到公式7；
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710786093.9A CN107576286B (zh) | 2017-09-04 | 2017-09-04 | 一种标靶整体优化的空间位置与姿态解求方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710786093.9A CN107576286B (zh) | 2017-09-04 | 2017-09-04 | 一种标靶整体优化的空间位置与姿态解求方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107576286A CN107576286A (zh) | 2018-01-12 |
| CN107576286B true CN107576286B (zh) | 2019-06-25 |
Family
ID=61030249
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710786093.9A Active CN107576286B (zh) | 2017-09-04 | 2017-09-04 | 一种标靶整体优化的空间位置与姿态解求方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107576286B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109579831B (zh) * | 2018-11-09 | 2020-05-19 | 西安科技大学 | 矿用悬臂式掘进机可视化辅助导引方法和系统 |
| CN110081841B (zh) * | 2019-05-08 | 2021-07-02 | 上海鼎盛汽车检测设备有限公司 | 3d四轮定位仪目标盘三维坐标的确定方法及系统 |
| CN110570477B (zh) * | 2019-08-28 | 2022-03-11 | 贝壳技术有限公司 | 一种标定相机和旋转轴相对姿态的方法、装置和存储介质 |
| CN112200876B (zh) * | 2020-12-02 | 2021-06-08 | 深圳市爱夫卡科技股份有限公司 | 一种5d四轮定位标定系统的标定方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4854702A (en) * | 1987-12-14 | 1989-08-08 | Hunter Engineering Company | Vehicle wheel alignment apparatus and method of use |
| CN106247932A (zh) * | 2016-07-25 | 2016-12-21 | 天津大学 | 一种基于摄影系统的机器人在线误差补偿装置及方法 |
| CN106352839A (zh) * | 2016-10-14 | 2017-01-25 | 哈尔滨工业大学 | 一种气浮球轴承三维姿态测量方法 |
| CN106969723A (zh) * | 2017-04-21 | 2017-07-21 | 华中科技大学 | 基于低速相机阵列的高速动态物体关键点三维测量方法 |
-
2017
- 2017-09-04 CN CN201710786093.9A patent/CN107576286B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4854702A (en) * | 1987-12-14 | 1989-08-08 | Hunter Engineering Company | Vehicle wheel alignment apparatus and method of use |
| CN106247932A (zh) * | 2016-07-25 | 2016-12-21 | 天津大学 | 一种基于摄影系统的机器人在线误差补偿装置及方法 |
| CN106352839A (zh) * | 2016-10-14 | 2017-01-25 | 哈尔滨工业大学 | 一种气浮球轴承三维姿态测量方法 |
| CN106969723A (zh) * | 2017-04-21 | 2017-07-21 | 华中科技大学 | 基于低速相机阵列的高速动态物体关键点三维测量方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107576286A (zh) | 2018-01-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107576286B (zh) | 一种标靶整体优化的空间位置与姿态解求方法 | |
| CN107255476B (zh) | 一种基于惯性数据和视觉特征的室内定位方法和装置 | |
| CN103745474B (zh) | 基于惯性传感器和摄像机的图像配准方法 | |
| CN106251305B (zh) | 一种基于惯性测量单元imu的实时电子稳像方法 | |
| CN104333675B (zh) | 一种基于球面投影的全景电子稳像方法 | |
| US8964027B2 (en) | Global calibration method with apparatus based on rigid bar for multi-sensor vision | |
| CN106548462B (zh) | 基于薄板样条插值的非线性sar图像几何校正方法 | |
| CN105447888B (zh) | 一种基于有效目标判断的无人机机动目标检测方法 | |
| CN107367229B (zh) | 自由双目立体视觉转轴参数标定方法 | |
| CN109598762A (zh) | 一种高精度双目相机标定方法 | |
| CN104408725B (zh) | 一种基于tld优化算法的目标重捕获系统及方法 | |
| CN110009681A (zh) | 一种基于imu辅助的单目视觉里程计位姿处理方法 | |
| CN110332887A (zh) | 一种基于特征光标点的单目视觉位姿测量系统及方法 | |
| CN109465832A (zh) | 高精度视觉和imu紧融合定位方法与系统 | |
| CN105205858A (zh) | 一种基于单个深度视觉传感器的室内场景三维重建方法 | |
| CN106971408B (zh) | 一种基于时空转换思想的摄像机标定方法 | |
| CN106780628A (zh) | 基于混合畸变模型的高精度摄像机标定方法 | |
| CN103278138A (zh) | 一种复杂结构薄部件三维位置及姿态的测量方法 | |
| CN107560592B (zh) | 一种用于光电跟踪仪联动目标的精确测距方法 | |
| CN107192375B (zh) | 一种基于航拍姿态的无人机多帧图像自适应定位校正方法 | |
| CN103323209A (zh) | 基于双目立体视觉的结构模态参数识别系统 | |
| CN107726975A (zh) | 一种基于视觉拼接测量的误差分析方法 | |
| CN107146239A (zh) | 卫星视频运动目标检测方法及系统 | |
| CN108470356A (zh) | 一种基于双目视觉的目标对象快速测距方法 | |
| CN103129752A (zh) | 一种基于地面导航的光学遥感卫星姿态角误差动态补偿方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A spatial position and attitude solution method for overall optimization of targets Effective date of registration: 20231205 Granted publication date: 20190625 Pledgee: Ma'anshan branch of Bank of China Ltd. Pledgor: ANHUI FCAR ELECTRONIC TECHNOLOGY Co.,Ltd. Registration number: Y2023980069496 |
|
| PE01 | Entry into force of the registration of the contract for pledge of patent right |