一种自适应估计景象匹配与惯导安装误差的方法技术

本发明专利技术属于视觉导航技术领域，具体涉及一种自适应估计景象匹配与惯导安装误差的方法；分析相机与惯导的安装误差对景象匹配定位精度造成的影响，本发明专利技术基于景象匹配的基本原理，并进行误差建模；将惯导系统输出的位置信息、景象匹配输出的位置信息之差作为卡尔曼滤波器的输入量，卡尔曼滤波器对安装误差进行估计，输出安装误差的估计量。在景象匹配的运算过程中利用安装误差进行位置校正，实现高精度的景象匹配定位。本发明专利技术无需事先进行人工标定，利用卡尔曼滤波器自适应估计景象匹配与惯导的安装误差，即可实现高精度的景象匹配定位；本发明专利技术尤其适用于解决高空应用环境下的景象匹配高精度应用需求。

A method of adaptive estimation of scene matching and inertial navigation error

【技术实现步骤摘要】
一种自适应估计景象匹配与惯导安装误差的方法
本专利技术属于视觉导航
，具体涉及一种自适应估计景象匹配与惯导安装误差的方法。
技术介绍
目前，下视景象匹配导航定位技术是巡航飞行器实现导航误差修正的关键技术，美国的BGM-109“战斧”巡航导弹便采用数学景象匹配与区域相关(DSMAC)导航方法作为末端制导方式。传统的惯性/景象匹配组合导航修正方法是利用景象匹配定位结果，对惯性导航位置信息进行重置，实现对惯性导航误差的校正。但是该方法修正后惯性导航精度定位保持时间短。惯性导航利用景象匹配定位结果进行位置误差修正，随虽可以提高短时导航定位精度，但因惯性速度、姿态和器件误差未进行校正，惯导系统定位精度会迅速恢复到景象匹配前的精度水平。此外，传统的景象匹配技术只能简单地利用惯导输出的姿态角进行坐标转换，这将导致随着飞行高度的增高，景象匹配的定位精度将随之下降。传统景象匹配因飞行高度有限而且采用简单位置匹配技术，并没有对姿态安装误差进行自适应估计。
技术实现思路
针对上述现有技术，本专利技术的目的在于提供一种自适应估计景象匹配与惯导安装误差的方法，目的在于提高景象匹配在高空环境中的定位精度。为了达到上述目的，本专利技术采用以下技术方案。本专利技术的目的在于提供一种自适应估计景象匹配与惯导安装误差的方法，包括以下步骤：步骤一、景象匹配定位，景象匹配定位采用归一化积相关算法；相关性运算的计算公式为：式中：CC(u,v)为模板图像与基准图像的相关度；(x,y)为模板图像中的坐标点；(u,v)为基准图像中的坐标点；T(x,y)为模板图像在点(x,y)处的灰度值，范围为0到255；为模板图像在基准图像(u,v)处的均值；I(x+u,y+v)为基准图像点(u,v)处，与模板图像相对应位置处的灰度值；为基准图像(u,v)处与模板图像相同区域的均值；步骤二、误差建模，获取图像时刻惯导输出的滚转、航向和俯仰角信息为依据，对景象匹配输出位置进行修正，修正公式如下所示：式中：Lnew、λnew表示经过姿态校正后的景象匹配输出位置信息，单位为弧度；Lold、λold表示原始的景象匹配输出位置信息，单位为弧度；表示由航向角组成的变换矩阵；KL、Kλ表示与景象匹配输出的纬度、经度姿态补偿系数；γ、θ表示惯导输出的滚转角、俯仰角，单位为弧度；步骤三、卡尔曼滤波器估计；将惯导系统输出的位置信息、景象匹配输出的位置信息之差作为卡尔曼滤波器的输入量，卡尔曼滤波器对安装误差进行估计，输出安装误差的估计量；在景象匹配的运算过程中利用输出安装误差的估计量进行位置校正，实现高精度的景象匹配定位。进一步在步骤二、误差建模中，当惯导与相机存在安装误差Δγ和Δθ时，将导致对应的纬度误差ΔL和经度误差Δλ，则式(2)可表示成下式：式中：Lnew、λnew表示经过姿态校正后的景象匹配输出位置信息，单位为弧度；Lold、λold表示原始的景象匹配输出位置信息，单位为弧度；表示由航向角组成的变换矩阵；KL、Kλ表示与景象匹配输出的纬度、经度姿态补偿系数；γ、θ表示惯导输出的滚转、俯仰角，单位为弧度；ΔL表示纬度误差，单位为弧度；表示Δλ经度误差，单位为弧度；Δγ表示惯导与相机存在滚动角安装误差，单位为弧度；Δθ表示惯导与相机存在俯仰角安装误差，单位为弧度。进一步在步骤二、误差建模中，当滚转、俯仰角均为小量，通过泰勒展开，公式(3)可化简为下式：式中：ΔL表示纬度误差，单位为弧度；表示Δλ经度误差，单位为弧度；表示由航向角组成的变换矩阵；KL、Kλ表示与景象匹配输出的纬度、经度姿态补偿系数；Δγ表示惯导与相机存在滚动角安装误差，单位为弧度；Δθ表示惯导与相机存在俯仰角安装误差，单位为弧度。进一步所述步骤三、卡尔曼滤波器估计包括以下步骤：1)误差模型；涉及的误差主要包括3个方面：一是惯导的导航参数解算误差；二是惯导的惯性器件自身误差和其受环境影响而引起的误差；三是景象匹配的误差；采用间接法滤波，系统状态方程为上述三种误差的方程，共选取19个系统状态量，则状态量方程为：式中：X为误差状态量；δVn,δVu,δVe分别表示捷联惯导系统北向、天向、东向的速度误差，单位为米每秒；δL,δh,δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；φn,φu,φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角，单位为弧度；分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；εx,εy,εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；δLdsmac,δλdsmac分别表示景象匹配纬度、经度误差，单位为弧度；δγ,δθ分别表示景象匹配与惯导的滚动、俯仰安装误差，单位为弧度；系统状态方程为：式中：为X的倒数；X为误差状态量；W为系统白噪声；F和G为系数矩阵；滤波器量测方程形式如下：Z＝HX+V(7)式中：Z为量测量；H为量测矩阵；X为误差状态量；V为量测噪声；量测值Z为惯导系统和景象匹配分别给出的速度的差值，实际上是两者误差的差值：式中，Limu、λimu表示惯导输出的纬度、经度，单位为弧度；Ldsmac、λdsmac表示景象匹配输出的纬度、经度，单位为弧度；V为量测噪声，考虑为白噪声；结合式(6)、(7)、(8)计算得到量测矩阵H；2)卡尔曼滤波模型：根据惯性/景象匹配系统方程和量测方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：式中：Tn为导航周期，NTn为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵；I为单位阵；状态一步预测：状态估计：滤波增益矩阵：一步预测误差方差阵：估计误差方差阵：Pk＝[I-KkHk]Pk,k-1(14)式中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵。本专利技术实施例提供的技术方案带来的有益效果是：本专利技术一种自适应估计景象匹配与惯导安装误差的方法，基于景象匹配的基本原理，分析相机与惯导的安装误差对景象匹配定位误差的影响机理，利用泰勒展开将对应的方程线性化，结合卡尔曼滤波技术实时估计出安装误差，能够大幅度提高景象匹配的定位精度。本专利技术无需事先进行人工标定，利用卡尔曼滤波器自适应估计景象匹配与惯导的安装误差，即可实现高精度的景象匹配定位；本专利技术尤其适用于解决高空应用环境下的景象匹配高精度应用需求。附图说明图1是本专利技术一种自适应估计景象匹配与惯导安装误差的方法示意图；图2是典型相关性分布可视化图；图3是姿态角导致景象匹配输出位置误差示意图；图4是本专利技术实施例机载试验的安装误差估计曲线示意图。具体实施方式下面结合具体实施方式和附图对本专利技术一种自适应估计景象匹配与惯导安装误差的方法作详细说明。分析相机与惯导的安装误差对景象匹配定位精度造成的影响，并进行误差建模；将惯导系统输出的位置信息、景象匹配输出的位置信息之差作为卡尔曼滤波器的输入量，卡尔曼滤波器对安装误差进行估计，输出安装误差的估计量。在景象匹配的运算过程中利用安装误差进行位置校正，实本文档来自技高网...

【技术保护点】
一种自适应估计景象匹配与惯导安装误差的方法，其特征在于，该方法包括以下步骤：步骤一、景象匹配定位，景象匹配定位采用归一化积相关算法；相关性运算的计算公式为：

【技术特征摘要】
1.一种自适应估计景象匹配与惯导安装误差的方法，其特征在于，该方法包括以下步骤：步骤一、景象匹配定位，景象匹配定位采用归一化积相关算法；相关性运算的计算公式为：式中：CC(u,v)为模板图像与基准图像的相关度；(x,y)为模板图像中的坐标点；(u,v)为基准图像中的坐标点；T(x,y)为模板图像在点(x,y)处的灰度值，范围为0到255；为模板图像在基准图像(u,v)处的均值；I(x+u,y+v)为基准图像点(u,v)处，与模板图像相对应位置处的灰度值；为基准图像(u,v)处与模板图像相同区域的均值；步骤二、误差建模，获取图像时刻惯导输出的滚转、航向和俯仰角信息为依据，对景象匹配输出位置进行修正，修正公式如下所示：式中：Lnew、λnew表示经过姿态校正后的景象匹配输出位置信息，单位为弧度；Lold、λold表示原始的景象匹配输出位置信息，单位为弧度；表示由航向角组成的变换矩阵；KL、Kλ表示与景象匹配输出的纬度、经度姿态补偿系数；γ、θ表示惯导输出的滚转角、俯仰角，单位为弧度；步骤三、卡尔曼滤波器估计；将惯导系统输出的位置信息、景象匹配输出的位置信息之差作为卡尔曼滤波器的输入量，卡尔曼滤波器对安装误差进行估计，输出安装误差的估计量；在景象匹配的运算过程中利用输出安装误差的估计量进行位置校正，实现高精度的景象匹配定位。2.根据权利要求1所述的一种自适应估计景象匹配与惯导安装误差的方法，其特征在于，在步骤二、误差建模中，当惯导与相机存在安装误差Δγ和Δθ时，将导致对应的纬度误差ΔL和经度误差Δλ，则式(2)可表示成下式：式中：Lnew、λnew表示经过姿态校正后的景象匹配输出位置信息，单位为弧度；Lold、λold表示原始的景象匹配输出位置信息，单位为弧度；表示由航向角组成的变换矩阵；KL、Kλ表示与景象匹配输出的纬度、经度姿态补偿系数；γ、θ表示惯导输出的滚转、俯仰角，单位为弧度；ΔL表示纬度误差，单位为弧度；表示Δλ经度误差，单位为弧度；Δγ表示惯导与相机存在滚动角安装误差，单位为弧度；Δθ表示惯导与相机存在俯仰角安装误差，单位为弧度。3.根据权利要求2所述的一种自适应估计景象匹配与惯导安装误差的方法，其特征在于，在步骤二、误差建模中，当滚转、俯仰角均为小量，通过泰勒展开，公式(3)可化简为下式：式中：ΔL表示纬度误差，单位为弧度；表示Δλ经度误差，单位为弧度；表示由航向角组成的变换矩阵；KL、Kλ表示与景象匹配输出的纬度、经度姿态补偿系数；Δγ表示惯导与相机存在滚动角安装误差，单位为弧度；Δθ表示惯导与相机存在俯仰角安装误差，单位为弧度。4.根据权利要求1或3所述的一种自适应估计景象匹配与惯导安装误差的方法，其特征在于，所述步骤三、卡尔曼滤波器估计包括以下步骤：1)误差模型；涉及的误差主要包括3个方面：一是惯导的导航参数解算误差；二是惯导的惯性器件自身误差和其受环境影响而引起的误差；三是景象匹配的误差；采用间接法滤波，系统状态方程为上述三种误差的方程，共选取19个系统状态量，则状态量方程为：

【专利技术属性】
技术研发人员：孙伟，郭元江，李海军，徐海刚，李群，刘冲，郑辛，裴玉锋，原润，钟润伍，
申请(专利权)人：北京自动化控制设备研究所，
类型：发明
国别省市：北京,11