[发明专利]一种自适应估计景象匹配与惯导安装误差的方法

权利要求书

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)处与模板图像相同区域的均值；

步骤二、误差建模，获取图像时刻惯导输出的滚转、航向和俯仰角信息为依据，对景象匹配输出位置进行修正，修正公式如下所示：

式中：L_new、λ_new表示经过姿态校正后的景象匹配输出位置信息，单位为弧度；L_old、λ_old表示原始的景象匹配输出位置信息，单位为弧度；表示由航向角组成的变换矩阵；K_L、K_λ表示与景象匹配输出的纬度、经度姿态补偿系数；γ、θ表示惯导输出的滚转角、俯仰角，单位为弧度；

步骤三、卡尔曼滤波器估计；将惯导系统输出的位置信息、景象匹配输出的位置信息之差作为卡尔曼滤波器的输入量，卡尔曼滤波器对安装误差进行估计，输出安装误差的估计量；

在景象匹配的运算过程中利用输出安装误差的估计量进行位置校正，实现高精度的景象匹配定位。

2.根据权利要求1所述的一种自适应估计景象匹配与惯导安装误差的方法，其特征在于，在步骤二、误差建模中，当惯导与相机存在安装误差Δγ和Δθ时，将导致对应的纬度误差ΔL和经度误差Δλ，则式(2)可表示成下式：

式中：L_new、λ_new表示经过姿态校正后的景象匹配输出位置信息，单位为弧度；L_old、λ_old表示原始的景象匹配输出位置信息，单位为弧度；表示由航向角组成的变换矩阵；K_L、K_λ表示与景象匹配输出的纬度、经度姿态补偿系数；γ、θ表示惯导输出的滚转角、俯仰角，单位为弧度；ΔL表示纬度误差，单位为弧度；表示Δλ经度误差，单位为弧度；Δγ表示惯导与相机存在滚转角安装误差，单位为弧度；Δθ表示惯导与相机存在俯仰角安装误差，单位为弧度。

3.根据权利要求2所述的一种自适应估计景象匹配与惯导安装误差的方法，其特征在于，在步骤二、误差建模中，当滚转角、俯仰角均为小量，通过泰勒展开，公式(3)可化简为下式：

式中：ΔL表示纬度误差，单位为弧度；表示Δλ经度误差，单位为弧度；表示由航向角组成的变换矩阵；K_L、K_λ表示与景象匹配输出的纬度、经度姿态补偿系数；Δγ表示惯导与相机存在滚转角安装误差，单位为弧度；Δθ表示惯导与相机存在俯仰角安装误差，单位为弧度。

4.根据权利要求1或3所述的一种自适应估计景象匹配与惯导安装误差的方法，其特征在于，所述步骤三、卡尔曼滤波器估计包括以下步骤：

1)误差模型；

涉及的误差主要包括3个方面：

一是惯导的导航参数解算误差；

二是惯导的惯性器件自身误差和其受环境影响而引起的误差；

三是景象匹配的误差；

采用间接法滤波，系统状态方程为上述三种误差的方程，共选取19个系统状态量，则状态量方程为：

式中：

X为误差状态量；

δV_n，δV_u，δV_e分别表示捷联惯导系统北向、天向、东向的速度误差，单位为米每秒；

δL，δh，δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；

分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角，单位为弧度；

分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；

ε_x,ε_y,ε_z分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；

δL_dsmac，δλ_dsmac分别表示景象匹配纬度、经度误差，单位为弧度；

δγ，δθ分别表示景象匹配与惯导的滚转、俯仰安装误差，单位为弧度；

系统状态方程为：

式中：为X的倒数；X为误差状态量；W为系统白噪声；F和G为系数矩阵；

滤波器量测方程形式如下：

Z＝HX+V (7)

式中：Z为量测值；H为量测矩阵；X为误差状态量；V为量测噪声；

量测值Z为惯导系统和景象匹配分别给出的位置的差值，实际上是两者误差的差值：

式中，L_imu、λ_imu表示惯导输出的纬度、经度，单位为弧度；L_dsmac、λ_dsmac表示景象匹配输出的纬度、经度，单位为弧度；V为量测噪声，考虑为白噪声；

结合式(6)、(7)、(8)计算得到量测矩阵H；

2)卡尔曼滤波模型：

根据惯性/景象匹配系统方程和量测方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：

式中：T_n为导航周期，NT_n为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵；I为单位阵；

状态一步预测：

状态估计：

滤波增益矩阵：

一步预测误差方差阵：

估计误差方差阵：

P_k＝[I-K_kH_k]P_k,k-1 (14)

式中，为一步状态预测值，为状态估计矩阵，Φ_k,k-1为状态一步转移矩阵，H_k为量测矩阵，Z_k为量测量，K_k为滤波增益矩阵，R_k为观测噪声阵，P_k,k-1为一步预测误差方差阵，P_k为估计误差方差阵，Γ_k,k-1为系统噪声驱动阵，Q_k-1为系统噪声阵。