小倾角下空间后方交会算法辅助的SINS/GPS姿态位置解算

夏琳琳，于金鑫，初 妍，朱笔辉

小倾角下空间后方交会算法辅助的SINS/GPS姿态位置解算

夏琳琳1，于金鑫1，初 妍2，朱笔辉1

（1. 东北电力大学 自动化工程学院，吉林 132012；2. 哈尔滨工程大学 计算机科学与技术学院，哈尔滨 150001）

利用航拍图像匹配技术辅助导航系统参数解算，通常要求飞行器姿态角近似为零。提出借用航空单像空间后方交会算法实现小倾角下SINS/GPS组合导航系统的姿态位置估计。推导了载体与航拍镜头固连且横滚角、俯仰角不为零情况下，以SINS输出的姿态角作为参数对航拍图片进行仿射变换的公式。进一步提出用上述仿射变换后的航拍像片参与地图匹配，匹配所得的控制点经空间后方交会解算，结果用以修正组合系统姿态和位置量。仿真试验表明：在静态小倾角情形下，方法能够减小迭代次数，缩短导航解算时间；即使在倾角达到20°，仍可不失效地为载体提供位置姿态信息。模拟飞行器直线平飞动态情形，验证了所提出的辅助算法可获得较好的位置估算精度。

空间后方交会；SINS/GPS；小倾角；地图匹配；仿射变换

在航空航天、舰船及车载系统中，SINS/GPS无疑是组合导航系统中最为经典的组合模式，SINS子系统短期误差较小，但长时间单机工作会出现解算迭代误差。将GPS作为辅助设备引入，可有效抑制误差发散，保证导航解算的精度。但GPS实为带有公共信息特征的无线电导航，卫星信号容易受到外部干扰或阻塞，一旦完全失效，整机系统将退化为纯SINS。视觉导航通常采用被动工作方式，在理论上具有最佳引导柔性[1]，利用影像匹配算法能够实现高精度的姿态位置解算，由此可有效抑制SINS误差[2-3]，确保在无GPS外部量测输入条件下，SINS/影像匹配导航系统仍可长时间高精度工作[4]。传统的影像匹配导航对于飞行器的航拍镜头姿态有较为严格的要求，即镜头主光轴要垂直于地面[5-6]。具体说，要求航空图像与数字地图不存在仿射变换，利用航空图像与数字地图来匹配位置。为达到镜头主光轴垂直于地面要求，或者要求飞行器保持平飞的姿态，或者将航拍镜头上加装其他装置以保证要求。诚然，前者限制了飞行器的姿态，后者不但增加了成本，又不可避免地引入了装置（为保持镜头姿态）误差。本文将探讨如果航拍镜头与飞行器固连，并且在飞行器横滚角和俯仰角不为0，呈现小倾角的情况下，通过单幅航空图像和数字地图的匹配，实现飞行器位置和姿态解算的具体方法。

1 航空单像空间后方交会

利用航拍影像覆盖范围内一定数量的控制点空间坐标与影像坐标，根据共线方程，反求摄影镜头坐标XS,YS,ZS（地理系，摄影测量坐标系）与镜头旋转姿态jS,wS,kS，称为单幅图像的空间后方交会[7]。

1.1坐标系间的几何关系

① 像平面坐标系oxy-：摄影方向与影像平面的交点o称像主点，像平面坐标系原点位于像主点，x轴与载体飞行方向平行，y轴位于影像平面内且与x轴垂直，方向指向x轴左侧，z轴平行于摄影方向，由o点指向摄影镜头中心。

② 像空间坐标系Sxyz-：各个坐标轴与像平面坐标系相平行，方向相同，但是原点位于摄影镜头中心点S。

③ 像空间辅助坐标系SXYZ-：原点位于摄影镜头中心S，X轴指向载体航线方向，Y轴指向左，Z轴指向天方向，其中SXY-平面与地面平行。

④ 摄影测量坐标系A-XAYAZA：以地面某一点A为原点，其余坐标轴与像空间辅助坐标系平行，方向相同。

图1为上述4个坐标系间的几何关系图。

图1 坐标系间几何关系Fig.1 Geometric relationships between coordinates

1.2共线方程

在摄影成像过程中，摄影中心点S、像点o及对应的地面点A三点位于一条直线上，则摄影中心S点的物体空间坐标（XS,YS,ZS）、像点o的像片坐标（x,y）以及对应地面A的物体空间坐标（X,Y,Z）满足式(1)，称为共线方程。

式中：f为航摄仪焦距；(0x,0y)为摄影镜头主光轴与摄影图像的交点；

其中，(,,φωκ)为像空间辅助坐标系SXYZ-旋转至像空间坐标系Sxyz-的角度，其旋转过程为绕Y轴旋转φ角度，再绕新的X轴旋转ω角度，再绕Z轴旋转κ角度。

1.3空间后方交会解算方法

① 获取已知数据，包括平均摄影距离H（即大概航高）、内方位元素(x0,y0,f)，获取控制点的空间坐标(Xt,Yt,Zt)，获取控制点对应的像片点坐标(xt,yt)。

② 确定未知数的初始值，位置初值为

其中，(tiX,tiY)为第i个控制点的坐标。

姿态初值为φ0=ω0=0，κ0可以选取前一时刻航向作为初值。

③ 计算旋转矩阵R，利用步骤②中角度元素近似值(φ0,ω0,κ0)以及公式(2)，求得

④ 根据式(1)逐点计算像点坐标的近似值(x˜,y˜)。

⑤ 逐点计算误差方程式的系数和常数项，组成

误差方程式：

其中，

⑥ 解算法方程，表示为

其中，P为观测值的权矩阵，其反映了观测值的量测精度。对于像点坐标的观测值，一般认为是等精度量测，故一般P为单位阵，则

X=[ΔX,ΔY,ΔZ,Δφ,Δω,Δκ]T即为所求未知数的

ssssss改正数，并与对应的近似值求和，得到未知数的近似值，其公式为

⑦ 对上述所求的未知数改正数与规定限差比较。通常对角度元素的修正数Δφi、Δωi、Δκi（i=1,2,…），通常取限差为0.1' 。当3个改正数小于限差时，停止迭代，并用新的近似值重复③~⑥步，直到满足要求为止。

1.4空间后方交会算法在组合导航中的适用场合

上述算法在组合导航中的应用具有一定的适用场合，如果真实的（φS,ωS）较大，则飞行器可能位于摄影范围正上方外侧，此时位置初值过远。并且算法本身将0°作为角度解算初值，这也使角度初值过远，可能导致迭代不收敛，进而无法求解。故该算法探讨姿态角较小时SINS/GPS组合系统的位置、姿态解算。

2 航空图像在匹配前、后的仿射变换

2.1航拍像片的仿射变换

图2 航拍像片仿射变换关系图Fig.2 Affine transformation of aerial photo

若航拍镜头与飞行器固连，则当横滚角和俯仰角都不为0时，由于所摄航空影像与地图数据库中的影像存在仿射变换，不能直接参与匹配，因此需要对倾斜拍摄的航片进行变换。图2给出了航拍像片仿射变换关系图。假设拍摄地面的范围为P，航拍像片为p，镜头中心为s，主光轴为l，l与p的交点为o，l与P的交点为O。考虑到匹配效果最好时对飞行器姿态的要求是镜头主光轴垂直于地面，因此将倾斜像片p转换为所拍摄范围内正上方拍摄的像片p'，用像片p'匹配。此时假设p'对应的摄影镜头位置为S'，高度与航拍镜头相等，此时l'垂直于地面，垂足为O，这样p'与数据库中影像不存在仿射变换，仅存在旋转与缩放变换。将p'与地图数据库匹配则容易的多，匹配方法则有GA匹配[8]、面特征匹配[9-10]、直线特征匹配[11-12]等方法。像片p变换至p'的过程及推导过程如下：

还不可怕么？试想这些连基本分数运算都不会的青少年，以后如何学习中等数学和科学？又如何进入大学和职场？即使日常生活中如购物、储蓄等事项对他们都会构成挑战！

若摄影镜头在摄影坐标系下坐标为S=(XS,YS,ZS)，这根据镜头中心点-像点三者的共线关系，有

根据公式(8)(9)，有

对于S'的照片p'，由于是垂直拍摄，所以R是单位阵，设点M在p'中坐标为m′=（xm′,ym′ ），有

公式(10)(11)两式相减，有

其中，kfH′=，经整理并展开，令1k=λ，可获得摄影测量坐标系下任意一点经S摄影之后的像片坐标，与虚构的S′摄影之后的像片坐标关系为

再经过插值及其它图像处理方法，获得像片p'。

注意到，无论采用哪种方法，对原始图像p应用公式(12)进行变换所得的“垂直拍摄”的像片p'都与地图数据库中的图像只存在旋转与缩放变换，便于对像片p'进行匹配。

2.2原像片控制点坐标的求取

经过仿射变换的图像与数据库地图进行匹配之后，可以得到像片的特征点和直线在虚拟像片p'中的坐标以及其在大地坐标下（摄影测量坐标系）的位置。得到特征点和直线之后，可以筛选道路交点、拐点、特殊建筑物等的位置坐标和像片坐标作为控制点。假设某一控制点m在p'中的位置为(xm′,ym′)，则根据公式(12)即可求取控制点在原始航拍像片中的位置，而该控制点在摄影测量坐标系下的物体坐标则通过图像匹配和对地图数据库的查询获得，因此在选取多个控制点之后，满足进行空间后方交会解算的条件。

2.3摄影坐标系与导航坐标系的转换

假设像片坐标系下m点坐标为(xm,ym)，则该点在像空间坐标系下为(xm,ym,-f)，则在像空间辅助坐标系下坐标为R[xmym-f]T；b（载体）坐标系取右前上，则同时该点在b系下坐标因为[-ymxm-f ]T，即，则在n系下坐标为

再根据n系下坐标轴与空间像辅助坐标系下存在x、y轴互换之后，载体（像片）y轴取反方向的变换关系，故有

约掉m点，则有

或者

由此可知，完成航空单像空间后方交会解算后，可通过公式(16)修正姿态阵。

3 空间后方交会辅助下的组合导航系统参数解算方法

在组合导航系统中，陀螺仪的短期精度良好，长期运行则精度变差，需要抑制误差的发散。注意到，空间后方交会解算可以提供部分导航参数，故考虑利用此算法的输出来修正陀螺仪的输出。

这种辅助解算方法需要多个控制点在地理（摄影测量）系下的坐标和像片坐标系下对应的像点坐标。为了获得这两组坐标，首先需要获取航拍镜头的姿态，从而对单幅航空相片进行仿射变换，文中2.1部分已实现了该仿射变换公式的推导；其次需要用仿射变换后的图像与地图数据库的图像匹配，从而获得控制点的两组坐标。对于图像匹配部分，理论上，仿射变换后的图像存在由陀螺仪的漂移所带来的计算误差，但是鉴于陀螺仪的短期误差较小，并且根据文献[8]~[12]的研究结果，当待匹配图像与数据库图像存在较小的偏差时，采用文献中的方法仍能够较准确的获取控制点坐标，确保了空间后方交会解算辅助算法不会受到限制。

考虑到本文研究的假设，即航拍镜头与载体相固联，可以借助陀螺仪短期精度良好这一优势，将陀螺仪解算的姿态角度作为仿射变换中求解R矩阵的参数；另一方面，也可将空间后方交会求解的角度值作为SINS输出的修正值之一。图3给出了空间后方交会算法的辅助过程流程图。

图3 空间后方交会算法辅助组合导航流程图Fig.3 Flowchart of space resection aided algorithm for integrated navigation system

4 仿真试验与分析

4.1单幅相片下的静态仿真

假设图像匹配过程中的匹配误差可以忽略，模拟横滚角、俯仰角不同情况下地面控制点在图像中的位置，并对此时解算得出的姿态角叠加误差，叠差之后的姿态阵为，并将叠加误差后的姿态角经过式(16)计算后解出的(φ,ω,κ)作为空间后方交会解算的初始角度，并根据解算结果再经式(16)求取的姿态角，与叠差之前的仿真姿态角对比；再模拟当没有其它传感器为空间后方交会提供初值的情况下的解算过程，与前一种方法的迭代次数对比。焦距f取0.1 m，飞行器正下方的地面点取为g系原点，飞行器高度为2000 m，模拟GPS提供的位置误差为50 m，并将叠差之后的位置作为空间后方交会解算的位置初始值，不同姿态下的地面控制点在摄影测量坐标系下的坐标点见于表1。取摄影测量坐标系原点为空间像辅助坐标系正下方的地面点，不同姿态角下的地面控制点的像片坐标见于表2，采用两种方法获得的数据对比结果见于表3。

从表3可以看出，在忽略图像匹配误差的情况下，仅以控制点作为空间后方交会解算可给出解载体在小姿态角（分别为0°及10°）情况下的位置和姿态。但是，由于没有其它导航传感器赋予解算初值，迭代次数较多，较大倾角情况下（20°）也会发生迭代错误。相比之下，本文提出用其它导航传感器提供空间后方交会解算初值，在载体存在较小姿态角的情形下，能够减小迭代次数，有效地缩短了导航数据的解算时间。特别是与前者相比，后者姿态和位置的导航解算更接近真实值。当倾角为0°时，姿态角和位置的解算结果都在数量级上很大程度地优于传统后方解算方法，并且在较大倾角条件下，仍然能够正确提供载体的位置和姿态信息。

表1 不同姿态角下的地面控制点的坐标Tab.1 Coordinates of ground control points under different attitudes m

表2 不同姿态角下的地面控制点的像片坐标Tab.2 Photo coordinates of ground control points under different attitudes m

表3 角度误差1°，位置误差50 m下，不同方法初始迭代下的解算结果Tab.3 Estimate results observed from two initial iteration (attitude & heading error=1°, position error=50 m)

4.2动态仿真

模拟飞行器保持匀速直线平飞的状态，以GPS/SINS松耦合状态下的解算作为算例：假设GPS提供的位置误差为50 m，空间后方交会算法提供的位置误差为10 m，角度误差1°，将空间后方交会解算结果作为SINS前一时刻输出的修正值，经修正后的SINS输出再与GPS进行松耦合解算。图4为位置误差估计曲线。实验结果表明，动态情况下本文提出的空间后方交会算法对位置的估算精度优于GPS单机工作模式，确保了陀螺仪的误差输出被抑制于一定范围内，动态小倾角下（模拟平飞状态）的位置解算可满足导航系统的精度要求。

图4 位置误差估算结果Fig.4 Estimated results for position error

5 结 论

本文面向SINS/GPS导航参数解算问题，引入视觉导航影像匹配技术，突破传统算法需飞行器的航拍镜头姿态主光轴要垂直于地面的限制，实现姿态小倾角下基于航空单像空间后方交会的组合系统位置、姿态估计。推导了载体与航拍镜头固连且水平姿态角不为零情况下，以SINS输出的姿态角作为参数对航拍图片进行仿射变换的公式，并给出该技术用于组合系统数据互补的具体流程。在角度误差为1°、位置误差为50 m条件下，分别进行姿态倾角为0°、10°（小倾角）和20°（大倾角）的仿真试验，对比仅以控制点进行后方交汇，以及姿态角作为迭代初值后再后方交汇两种方法的参数估计结果，结果表明后者在小倾角下的导航解算更接近真实值，且迭代次数少于前者，性能更优。相比之下，传统方法在大倾角下会出现迭代错误，通用性受到限制。诚然，本文的研究没有考虑到大倾角情况下地平线将进入航拍像片的情况，同时还需要适当考虑图像匹配误差和由初始姿态阵求解p'所带来的误差问题，故如何完善大倾角下的算法将是后续的研究方向。

（References）：

[1] 唐超颖, 杨忠, 沈春林. 视觉导航技术综述[C]//2007年江苏省自动化学会学术年会论文集. 2007: 96-104.

Tang Chao-ying, Yang Zhong, Shen Chun-lin. A survey of visual navigation[C]//Proceedings of Academic Conference on Automation Association of Jiangsu Province. 2007: 96-104.

[2] 王龙, 董新民, 张宗麟. 紧耦合INS/视觉相对位姿测量方法[J]. 中国惯性技术学报, 2011, 19(6): 686-691.

Wang Long, Dong Xin-min, Zhang Zong-lin. Relative pose measurement based on tightly-coupled INS/vision[J]. Journal of Chinese Inertial Technology, 2011, 19(6): 686-691.

[3] Sazdovsk V, Kitanov A, Petrovic I. Implicit observation model for vision aided inertial navigation of aerial vehicles using single camera vector observations[J]. Aerospace Science and Technology, 2015, 40: 33-46.

[4] Courbon J, Mezouar Y, Guenard N, Martinet P. Visionbased navigation of unmanned aerial vehicles[J]. Control Engineering Practice, 2010, 18: 789-799.

[5] Sun Xiu-hong, Chen W, Osterberg J, et al. A GIS-portable airborne multisensor imaging system[C]//Geoscience and Remote Sensing Symposium. Vancouver, Canada, 2011: 981-984.

[6] Gu Duoyu, Zhu Chengfei, Guo Jiang, Li Shuxiao, Chang Hongxing. Vision-aided UAV navigation using GIS data [C]//2010 IEEE Conference on Vehicular Electronics and Safety. Qingdao, China, 2010: 78-82.

[7] 张红良, 郭鹏宇, 李壮. 一种视觉辅助的惯性导航系统动基座初始对准方法[J]. 中国惯性技术学报, 2014, 22(4): 469-473.

Zhang Hong-liang, Guo Peng-yu, Li Zhuang. Vision aided alignment method for inertial navigation system on moving base[J]. Journal of Chinese Inertial Technology, 2014, 22(4): 469-473.

[8] Niu Rui-qing, Wu Xue-ling, Yao Deng-kui, et al. Susceptibility assessment of landslides triggered by the Lushan Earthquake[J]. IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing, 2014, 7(9): 3979-3992.

[9] 龙腾飞, 焦伟利, 王威. 基于面特征的遥感图像几何校正模型[J]. 测绘学报, 2013, 42(4):540-545.

Chen Gang, Tang Biao, Zuo Zheng-rong. Multiple subimage matching algorithm based on correlation plane feature[J]. Infrared Technology, 2012, 34(4): 229-236.

[10] 陈刚, 汤彪, 左峥嵘. 基于相关面特征的多子区异谱图像匹配算法[J]. 红外技术, 2012, 34(4): 229-236.

Chen Gang, Tang Biao, Zuo Zheng-rong. Multiple subimage matching algorithm based on correlation plane feature. Infrared Technology, 2012, 34(4): 229-236.

[11] Da-ni Joo, Sang-chan Moon, Min-Woo Kim, et al. A study about curve extraction and lane departure determination of linear curved road[C]//2014 14thInternational Conference on Control, Automation and System. Gyeonggi-do, South Korea, 2014: 720-725.

[12] 邵振峰, 陈敏. 尺度、旋转以及亮度稳健的高分辨率影像直线特征匹配[J]. 光学精密工程, 2013, 21(3): 790-798.

Shao Zhen-feng, Chen Min. Line-based matching for high-resolution images with robustness for scale, rotation and illumination[J]. Optics and Precision Engineering, 2013, 21(3): 790-798.

Attitude and position calculations for SINS/GPS aided by space resection of aeronautic single-image under small inclination angles

XIA Lin-lin1, YU Jin-xin1, CHU Yan2, ZHU Bi-hui1
(1. School of Automation Engineering, Northeast Dianli University, Jilin 132012, China; 2. College of Computer Science and Technology, Harbin Engineering University, Harbin 150001, China)

When it comes to the issue of parameter estimates for navigation aided by aerial images matching technology, a requirement should be satisfied that the attitudes for the aerocrafts approximately equal to zero. In this work, the space resection algorithm based on aeronautic single-image is invoked, realizing the attitude and position estimates for the SINS/GPS with small inclination angle. Suppose the aerocraft is connected with the aerial camera and the rolls and pitches are not zeros, the attitudes derived from SINS are adopted as the parameters, which are then applied to deduce the affine transformation formula of the aeronautic images. In sequence, it is proposed that the affine transformed aeronautic images above could be involved in the map matching, and the space resection results, which are obtained from the matched control points, could be used to correct the attitude and position of the integrated navigation systems. The simulation results show that proposed method leads to reduce the number of iterations, and shorten the estimation time for navigation with static small inclination angle as well. Even if the inclination angle summed up to 20°, the derived attitude and position are also suitable to be provided to the aerocrafts without any failure. Moreover, the dynamic simulation related to the flat flying is carried out, testifying the aided solution has contributed to the better estimation precision of the navigation parameters.

space resection; SINS/GPS; small inclination angle; map matching; affine transformation

U666.1

A

1005-6734(2015)03-0350-06

10.13695/j.cnki.12-1222/o3.2015.03.013

2015-01-14；

2015-05-27

吉林省科技厅青年科研基金项目（20130522171JH）；黑龙江省留学归国人员科学基金（LC2015025）

夏琳琳（1980—），女，博士，副教授，从事组合导航、机器人技术研究。E-mail：xiall521@mail.edu.edu.cn