视觉信息位姿估计在火箭起飞漂移量中的应用

高 燕，夏 霏

(太原卫星发射中心，山西 太原 030027)

0 引言

在火箭点火起飞至离开发射塔架前，火箭垂直向上飞行，火箭在该阶段由于控制的延迟和超调等原因出现摆动，表现为横向漂移，漂移量就是指火箭垂直起飞过程中相对基准弹道的偏移，主要用于评定火箭的飞行控制性能。目前火箭起飞漂移量主要依靠高速电视摄像机交汇测量实现，该方法对图像判读要求较高，并且测量精度偏低。

位姿估计是视觉测量方法中的基础问题，常用在空间目标交汇对接、飞行器导航与目标定位等方面。李少飞[1]提出基于原始点云对物体进行6D位姿估计的深度学习方法，仅依靠三维点云信息对散乱场景中的物体进行位姿估计。蒋雪源等[2]提出采用动态遍历与预聚类相结合的视觉定位方法，能够有效提高位姿估计的精度。许允喜等[3]提出了扩展正交迭代算法，同时利用点和直线段的特征，定义误差函数，迭代求解其最小值，使得位姿估计误差更低、抗噪性更强、算法更稳定。洪洋等[4]利用矩形的几何特性及空间点、线的共面特征，通过获取一对正交消隐点，确立无穷远点在像平面的投影关系，完成姿态估计。郭军等[5]引入三维重构信息，克服了正交迭代算法的位姿模糊，提高了位姿估计的收敛速度和鲁棒性。李鑫等[6]利用直接最小二乘法实现了高精度的相机位姿估计。凌寒羽等[7]在高精度快速位姿估计算法中，加入改进的自适应加权物方残差目标迭代算法，提高计算精度和稳健性。席志红等[8]提出将不确定性描述矩阵加入到位姿求解等式中，使得求解精度更高，稳定性更强。

多目位姿估计是当前采用较多的方法，其基本原理是利用多台相机对目标进行拍摄，最终获取目标坐标系和相机坐标系之间的位姿转换关系，其求解过程称为透视n点投影问题(Perspective-n-Point，PnP)。

结合火箭起飞漂移量应用实际，提出基于视觉信息的起飞漂移量测量计算方法，利用单幅图像对测量设备进行准确标定，并使用正交迭代算法求解箭体的位置和姿态，试验结果表明该方法有较高的测量精度。

1 视觉信息位姿估计原理

通常使用中心透视投影模型表示相机的成像过程,如图1所示。该模型为一线性模型，假设物体表面的反射光或发射光都经过一个“针孔”点而投影在像平面上，此投影中心或针孔称为光心，满足物点、像点和光心三点共线，并据此建立中心投影的基本方程——共线方程[9]。

图1 中心透视成像模型Fig.1 Central perspective imaging model

图1中，Ow-XwYwZw为世界坐标系;Oc-XcYcZc为摄像机坐标系;Oc为光心。平面O0-uv和平面O1-xy均为成像平面，其中O0-uv以像素为单位建立坐标系，平面O1-xy以物理单位建立坐标系，原点O1为相机光轴与成像平面的交点，其在平面O0-uv的坐标为[u0,v0]，像素在2个方向的物理尺寸为[dx,dy]。

设像点P的像素坐标为[u,v]，图像坐标为[x,y]，物点P在世界坐标系中的坐标为[Xw,Yw,Zw]，在摄像机坐标系中的坐标为[Xc,Yc,Zc]，则根据共线关系，存在：

(1)

式中，fx,fy为横向和纵向等效焦距，表示实际光学焦距与像元的横纵长度之比。

设世界坐标系到摄像机坐标系的坐标转换矩阵和平移向量为{R ,t}，则：

(2)

结合上两式，得到：

(3)

式中，K为内参矩阵，反映相机内部的固有特性，与等效焦距和主点相关。{R,t}为外参矩阵，与相机在世界坐标系中的位置和姿态相关，反映了相机的外部位姿。

在实际的成像系统中，物像之间理想的线性投影关系并不能实现，由于各种因素的影响，成像投影点与投影几何中心存在偏差，称之为光学畸变误差，其存在会影响最终结果的正确性，因此需要建立像差模型对该误差进行修正。相机成像偏差并非只受单一因素影响，是由多种类型畸变综合作用的结果，一般包括径向畸变、切向畸变和薄棱镜畸变[10]。在忽略各种畸变的高阶项后，理论像点和实际像点可表示为：

，

(4)

式中，k1，p1，p2，s1，s2为相差系数，已知相差系数可以对图像进行有效修正，从而保证测量结果的正确性。由于相差系数只与相机本身的成像性质有关，所以相差系数可以认为是相机内部参数的一部分。

综上所述，相机的成像过程如图2所示，某点在世界坐标系中的坐标经过刚体坐标变换，得到该点在相机中的坐标，经过透视投影，得到理想图像坐标，相差修正后得到真实图像坐标，最后经过处理得到该点的像素坐标。简单的说，已知物点P对应像点的像素坐标[u，v]，及相机的内参系数fx，fy，u0，v0，外参矩阵R，t，相差系数k1，p1，p2，s1，s2，可以求得该点在世界坐标系中的坐标。

图2 相机成像过程示意图Fig.2 Schematic diagram of camera imaging process

2 相机标定

由上一节可知，利用视觉信息进行物体的位姿估计，需要先知道相机的内外参数及相差系数，相机参数的获取通过相机标定实现，其标定的准确程度将会直接影响最终测量精度。

鉴于三维坐标维度信息充足、测量方便和求解效率高的优点，采用三维标定参照物的方式进行标定测量。在测量空间中布置一系列非共面靶标，要尽可能充满整个视场，在线性运算基础上利用归一化的求解方式进行多点迭代运算，通过单张图像实现内外参数的求解。即利用已知靶标的图像坐标及对应的世界坐标估计相机内外参数的初值，与镜头畸变参数一起进行非线性优化搜索，估计出所有参数的最优解[11]。

若引入投影矩阵：

(5)

则式(3)可以描述为关于像点的显示表达：

(6)

可将方程组(6)写成矩阵形式：

(7)

式中，m中未知量的个数是11，已知标定点的二维像素坐标和三维世界坐标，要求解式(7)，那么标定点的数量只要保证N≥6即可。由于方程的个数大于未知量的个数，因此该方程组为超定方程组，利用最小二乘法进行求解，其结果为：

m=(ATA)-1ATU。

(8)

上述过程中没有考虑图像畸变和噪声的影响，只能获得粗略的内外参数结果。为了提高精度，以式(8)的计算结果为初值，采用LM(Levenberg-Marquardt)算法进行迭代优化，构造目标函数为：

(9)

3 多相机正交迭代算法

在计算过程中需要建立火箭的箭体坐标系Om-XmYmZm来反映箭体上所有点的信息，原点Om位于箭体尾部中心，Ym轴与箭体纵轴重合指向火箭头部，Xm轴在箭体纵对称面内，垂直于Ym轴，在火箭平飞时指向上方，Zm轴与Xm、Ym构成右手坐标系。火箭在飞行过程中，其上各点坐标在箭体坐标系中的位置不变，可以用来描述空间点对箭体的位置关系和火箭姿态。通过箭体坐标系到世界坐标系的转换矩阵和平移向量{Rmw,tmw}，可以将箭体上各点在箭体坐标系下的位置转换为世界坐标系下的位置：

(10)

将式(10)带入式(3)可以得到：

(11)

{Rmw,tmw}反映了火箭在世界坐标系中的位姿。由式(11)可知，在已知箭体上多个目标点的箭体坐标(Xm ,Ym,Zm)、相机拍摄的对应目标点的图像坐标(u,v)及相机的内外参数K,Rwc,twc的条件下，可以对{Rmw,tmw}进行求解。

控制点pi在多摄像机系统坐标系的坐标qi=Rpi+t；

物方残差矢量在多摄像机系统坐标系下表示为：

(12)

多摄像机系统的位姿估计问题可以转化为最小化下式的物方残差平方和：

(13)

步骤1：对于第k次迭代，给定R的值，则t存在最优解：

tk=t(Rk)=

(14)

步骤3：根据误差函数则有：

(15)

(16)

可以得到：Rk+1=argmin(R)tr(RTM(Rk))。

对M(Rk)进行奇异值分解SVD，记为M(Rk)=UDTT，其中D=diag(σ1,σ2,...,σn)，得到式(16)问题的最优解为：

Rk+1=UVT。

(17)

步骤4：判断迭代矩阵的收敛性，设定一个阈值，当目标函数小于这个阈值，或者目标函数的相对变化量小到一定程度，或者达到了预设的迭代次数上限，迭代停止，输出位姿参数R和t，否则返回步骤1重新计算。

4 测量结果计算与分析

4.1 仿真实验

仿真实验在Matlab环境下运行，依据透视投影原理将物体投影到摄像机平面，由于摄像机误差和特征点提取误差的存在，为模拟真实性，在图像特征点中加入噪声。

本实验中，摄像机内部参数为：等效焦距为(2 200，2 200)，主点为(1 024 pixel，1 024 pixel)，像面大小为(2 048 pixel×2 048 pixel)，取3个摄像机组成多摄像机系统。定义目标物尺寸为[-10,10]×[-10,10]×[-10,10]的立方体，在其表面均匀分布6个点作为标志点，3台相机获取的总标识点数为12个，3台相机布站在与参考点坐标系原点的距离为100，且光轴指向原点的位置，目标的标志点投影到归一化图像平面产生图像点，在图像点中加入均值为零，标准方差为1 pixel的高斯噪声，随机仿真500次，统计误差均值。旋转矩阵的误差计算公式为ER=arccos((tr(RTRg)-1)/2)，平移量的误差计算公式为Et=‖t-tg‖/‖tg‖·100。

算法随着噪声、点数、摄像机个数增加的位姿估计误差如图4所示。从图4中可以看出，随着加入的噪声级别、点数和相机个数的不断增加，旋转矩阵和平移量的误差变化逐渐减小，可以看出算法有效且精度较高。

(a) 旋转矩阵误差随噪声级变化图

(b) 旋转矩阵误差随点数变化图

(c) 旋转矩阵误差随相机数变化图

(d) 平移量误差随噪声级变化图

(e) 平移量误差随点数变化图

(f) 平移量误差随相机数变化图

4.2 真实实验

将上述方法应用在实际情况中，其解算过程为：① 对高速电视测量系统进行标定，方法为在设备视场内选取已知点作为标志点，测量标志点的大地坐标，每个视场内至少需要6个标志点，且标志点最好均匀分布在整个视场。对拍摄的单幅图像中的标志点进行判读，结合测站和发射点的大地坐标，对设备内外参数进行精确标校，得到设备相机的等效焦距，光心在测量坐标系中的位置，相机坐标系到发射坐标系转换矩阵等参数；② 将发射坐标系作为这个多摄像机系统的摄像坐标系，以火箭中心为原点建立箭体坐标系，依据火箭外形尺寸计算火箭上所有测量点在箭体坐标系中的坐标；③ 对各相机每个图像中的测量点进行判读，并用判读结果进行逐点多目正交迭代解算，获得各测量时刻箭体坐标系到摄像坐标系的转换矩阵，从而解算出各测量点的漂移量参数。

将起飞前火箭中心的漂移量位置参数与实测值进行比对，统计其在各方向上的总误差均在厘米级，在一定程度上可以反映火箭起飞漂移量的测量精度。

5 结束语

利用摄像机视觉测量的结果，将单幅图像中多个已知空间位置的特征点进行标定，从而获得多台摄像机的内外参数，在此基础上，对多摄像机拍摄的全部图像序列统一表达，再把图像上特征点的物方残差作为目标函数，最后使用多目正交迭代算法求解各时刻箭体的位置和姿态，得到箭体各位置的漂移量。仿真和实际测量结果都表明，该方法是有效、精确的。但在解算过程中发现，由于火箭起飞过程中存在运动模糊现象，以及火箭起飞时会引起相机抖动，都给解算结果带来误差。基于此，下一步可以进一步分析误差模型，寻求适应性更强的解法。