CN107966147A - 一种大机动情况下景象匹配的方法 - Google Patents
一种大机动情况下景象匹配的方法 Download PDFInfo
- Publication number
- CN107966147A CN107966147A CN201610915460.6A CN201610915460A CN107966147A CN 107966147 A CN107966147 A CN 107966147A CN 201610915460 A CN201610915460 A CN 201610915460A CN 107966147 A CN107966147 A CN 107966147A
- Authority
- CN
- China
- Prior art keywords
- msub
- mrow
- mtd
- mtr
- error
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
本发明属于视觉导航技术领域，具体公开了一种大机动情况下景象匹配的方法。基于景象匹配的基本原理，分析载体在进行大机动情况下对景象匹配定位误差的影响，并进行误差建模；利用相关信息进行图像畸变校正，以及定位误差的初步校正；再将惯导系统输出的位置信息、景象匹配输出的位置信息之差作为卡尔曼滤波器的输入量，卡尔曼滤波器对景象匹配的各项误差进行估计，输出景象匹配各项误差的估计量。在景象匹配的运算过程中利用估计出的各项误差进行修正，实现高精度的景象匹配定位。
Description
技术领域
本发明属于视觉导航技术领域，具体公开了一种大机动情况下景象匹配的方法。
背景技术
在载体大机动情况下，传统的景象匹配技术由于图像畸变、图像中心与载体中心不一致等因素，会引起极大的定位误差。为提高景象匹配在该情况下的定位精度，利用惯性导航系统的角度信息以及卡尔曼滤波技术等途径提升景象匹配的定位精度。
发明内容
本发明的目的在于提供一种大机动情况下景象匹配的方法，其能够对景象匹配的各项误差进行估计。
本发明的技术方案如下：
一种大机动情况下景象匹配的方法，该方法包括如下步骤：
1)景象匹配定位
利用下式确定衡量实时图在基准图坐标为(u,v)处的相关性变量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)处与模板图像相同区域的均值；
2)确定景象匹配位置误差ΔLdsmac、Δλdsmac
其中，ΔLdsmac表示角度变化导致的景象匹配输出纬度误差、Δλdsmac表示角度变化导致的景象匹配输出经度误差，单位为弧度；
表示由航向角组成的变换矩阵；
KL、Kλ分别表示与景象匹配输出的纬度、经度姿态补偿系数；
γ、θ分别表示惯导输出的滚转、俯仰角，单位为弧度；
3)图像投影畸变校正
确定原图像上的点P和校正后图像上的点P′关系如下式
其中，P和P′的齐次坐标分别为P＝[p1 p2 p3]T和P′＝[p1′ p′ 2p3′]T；Ha是非奇异矩阵；
确定点P和P′的非齐次坐标关系如下式
其中，Ha矩阵可根据载体的滚转、俯仰、航向角度、相机内部参数、相机与景物的距离等信息计算，属于现有的公知技术。
4)估计景象匹配与惯导安装误差状态向量
a)确定误差模型
选取17个系统状态组成系统状态向量X，17个系统状态分别为：
δVn，δVu，δVe，δL，δh，δλ，φn，φu，φe，εx,εy,εz,δLdsmac,δλdsmac
其中：
δVn,δVu,δVe分别表示捷联惯导系统北向、天向、东向的速度误差，单位为米每秒；
δL,δh,δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；
φn,φu,φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角，单位为弧度；
分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；
εx,εy,εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；
δLdsmac,δλdsmac分别表示景象匹配纬度、经度误差，单位为弧度。
确定系统状态方程如下：
其中X(t)为上述17个状态；W(t)为系统白噪声；F(t)和G(t)为系数矩阵，根据误差方程求取，属于公知技术；
b)确定量测方程矩阵
利用下式确定量测值Z
式中，Limu、λimu分别表示惯导输出的纬度、经度，单位为弧度；
Ldsmac、λdsmac分别表示景象匹配输出的纬度、经度，单位为弧度；
量测值Z分别带入下式，解方程，得出量测方程矩阵H
Z＝HX+V
其中，V为量测噪声，考虑为白噪声。
c)卡尔曼滤波模型预估计得出景象匹配与惯导误差向量的最优估计值。
在上述的一种大机动情况下景象匹配的方法中，还包括步骤5)利用估计出的误差补偿景象匹配输出的位置信息，达到提高景象匹配定位精度的目的。
在上述的一种大机动情况下景象匹配的方法中，所述的步骤4)中卡尔曼滤波过程如下：
计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：
式中：
Tn为导航周期，NTn为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵；
状态一步预测
状态估计
滤波增益矩阵
一步预测误差方差阵
估计误差方差阵
Pk＝[I-KkHk]Pk,k-1
其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵。
本发明的显著效果在于：本方法基于景象匹配的基本原理，分析载体在进行大机动情况下对景象匹配定位误差的影响，并进行误差建模；利用相关信息进行图像畸变校正，以及定位误差的初步校正；再将惯导系统输出的位置信息、景象匹配输出的位置信息之差作为卡尔曼滤波器的输入量，卡尔曼滤波器对景象匹配的各项误差进行估计，输出景象匹配各项误差的估计量。在景象匹配的运算过程中利用估计出的各项误差进行修正，实现高精度的景象匹配定位。
附图说明
图1为姿态角导致景象匹配输出位置误差；
图2为本方法的流程图。
具体实施方式
下面结合附图及具体实施例对本发明作进一步详细说明。
如图2所示的流程图，其具体步骤如下：
1.景象匹配定位
本方法所涉及的景象匹配定位采用归一化积相关算法，其本质是一种两幅图像间的相似程度度量准则。匹配算法的一般过程是，模板图像在基准图像中从起始位置(基准图左上角)逐行扫过整个基准图；在每一个位置将模板图像中每个像素点与它所对应基准图区域像素点进行相关性运算，将结果记录在相应的位置处；选取相关性较高的若干个匹配峰值点，作为候选匹配像素点。
相关性运算的计算公式为
式中，
(x,y)为模板图像中的坐标点；(u,v)为基准图像中的坐标点；
T(x,y)为模板图像在点(x,y)处的灰度值，范围为0到255；
为模板图像在基准图像(u,v)处的均值；
I(x+u,y+v)为基准图像点(u,v)处，与模板图像相对应位置处的灰度值；
为基准图像(u,v)处与模板图像相同区域的均值。
2.景象匹配位置误差分析
相机获取图像时刻，若载体滚转角、航向角和俯仰角非零，则将导致图像采集设备获取的图像中心与载体质心在地面上的投影的位置不一致，将给带来较大的景象匹配定位误差，其原因如图1所示。
当存在小角度的滚转和俯仰时，会导致相机获取图像中心与飞行载体质心在地面投影位置产生偏移，若此时航向也发生变化，将会导致相机获取的图像中心位置以某一固定的半径旋转，旋转的角度与航向角保持一致。
针对以上分析，载体三个方向的任一角度都会导致景象匹配的位置误差，计算公式如下所示。
式中，
ΔLdsmac、Δλdsmac表示角度变化导致的景象匹配输出位置误差，单位为弧度；
表示由航向角组成的变换矩阵；
KL、Kλ表示与景象匹配输出的纬度、经度姿态补偿系数；
γ、θ表示惯导输出的滚转、俯仰角，单位为弧度。
3.图像畸变校正
景象匹配中图像发生畸变的原因主要有“透镜畸变”和“投影畸变”，相机的镜头将引起所谓的“透镜畸变”，即径向畸变和切向畸变，而用于景象匹配的相机需要事先进行相机标定以消除“透镜畸变”，故“透镜畸变”不属于本专利的范畴；“投影畸变”产生的核心原因是因为这个世界是三维的，而图像只有二维的尺度，所以相机在将三维的景象转换为二维图像的过程中会丢失一定的信息，载体发生大机动将导致相机拍摄的景象中每个点到相机镜头的距离不一致，引起了所谓的“投影畸变”，下面进行“投影畸变”校正。
设原图像上的点P和校正后图像上的点P′在三维空间中为同一点，P和P′的齐次坐标分别为P＝[p1 p2 p3]T和P′＝[p1′ p′2 p3′]T，则P和P′的关系如下式所示：
记为：
P′＝Ha·P
Ha是非奇异矩阵，用点的非齐次坐标代替点的齐次坐标，设点P和P′的非齐次坐标为(x,y)和(x′,y′)，则二维投影变换的关系可表示为：
Ha矩阵可根据载体的滚转、俯仰、航向角度、相机内部参数、相机与景物的距离等信息计算。
4.卡尔曼滤波器估计
卡尔曼滤波实质上是一种递推线性最小方差滤波方法，它不要求储存过去的量测值，只要根据当时的量测值和前一时刻的估计，就可以实时地计算出所需信号的估计。
1)误差模型
本专利所设计的估计景象匹配与惯导安装误差的方法中，需要涉及的误差主要包括3个方面：一是惯导的导航参数解算误差；二是惯导的惯性器件自身误差和其受环境影响而引起的误差；三是景象匹配的误差。采用间接法滤波，系统状态方程就是各误差方程，选取17个系统状态组成系统状态向量X，17个系统状态分别为：
δVn，δVu，δVe，δL，δh，δλ，φn，φu，φe，εx,εy,εz,δLdsmac,δλdsmac。
其中：
δVn,δVu,δVe分别表示捷联惯导系统北向、天向、东向的速度误差，单位为米每秒；
δL,δh,δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；
φn,φu,φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角，单位为弧度；
分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；
εx,εy,εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；
δLdsmac,δλdsmac分别表示景象匹配纬度、经度误差，单位为弧度。
系统状态方程为：
式中：X(t)为上述17个状态；W(t)为系统白噪声；系数矩阵F(t)和G(t)根据误差方程求取。
滤波器量测方程形式如下：
Z＝HX+V
量测值Z为惯导系统和景象匹配分别给出的速度的差值，实际上是两者误差的差值：
式中，Limu、λimu表示惯导输出的纬度、经度，单位为弧度；
Ldsmac、λdsmac表示景象匹配输出的纬度、经度，单位为弧度；
V为量测噪声，考虑为白噪声。
H阵由式上述式子和系统状态变量的排列顺序确定，不再赘述。
2)卡尔曼滤波模型
建立上述误差模型后，选用卡尔曼滤波方法作为参数辨识方法，卡尔曼滤波方程采用文献《卡尔曼滤波和组合导航原理》(第一版，秦永元等编著)中的形式，具体公式如下：
根据惯性/景象匹配系统方程和量测方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：
式中：
Tn为导航周期，NTn为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵。
状态一步预测
状态估计
滤波增益矩阵
一步预测误差方差阵
估计误差方差阵
Pk＝[I-KkHk]Pk,k-1
其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵。
5估计并补偿误差
根据卡尔曼滤波最优估计结果可得到景象匹配与惯导误差的最优估计值，利用估计出的误差补偿景象匹配输出的位置信息，达到提高景象匹配定位精度的目的。
Claims (3)
1.一种大机动情况下景象匹配的方法，其特征在于，该方法包括如下步骤：
1)景象匹配定位
利用下式确定衡量实时图在基准图坐标为(u,v)处的相关性变量CC(u,v)
<mrow>
<mi>C</mi>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>u</mi>
<mo>,</mo>
<mi>v</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>&Sigma;</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
</mrow>
</msub>
<mo>&lsqb;</mo>
<mi>T</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mover>
<mi>T</mi>
<mo>&OverBar;</mo>
</mover>
<mrow>
<mi>u</mi>
<mo>,</mo>
<mi>v</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mi>I</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>+</mo>
<mi>u</mi>
<mo>,</mo>
<mi>y</mi>
<mo>+</mo>
<mi>v</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mover>
<mi>I</mi>
<mo>&OverBar;</mo>
</mover>
<mrow>
<mi>u</mi>
<mo>,</mo>
<mi>v</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
<msqrt>
<mrow>
<msub>
<mi>&Sigma;</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
</mrow>
</msub>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mi>T</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mover>
<mi>T</mi>
<mo>&OverBar;</mo>
</mover>
<mrow>
<mi>u</mi>
<mo>,</mo>
<mi>v</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mn>2</mn>
</msup>
<msub>
<mi>&Sigma;</mi>
<mrow>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
</mrow>
</msub>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mi>I</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>+</mo>
<mi>u</mi>
<mo>,</mo>
<mi>y</mi>
<mo>+</mo>
<mi>v</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mover>
<mi>I</mi>
<mo>&OverBar;</mo>
</mover>
<mrow>
<mi>u</mi>
<mo>,</mo>
<mi>v</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
</mfrac>
</mrow>
其中，(x,y)为模板图像中的坐标点；(u,v)为基准图像中的坐标点；
T(x,y)为模板图像在点(x,y)处的灰度值，范围为0到255；
为模板图像在基准图像(u,v)处的均值；
I(x+u,y+v)为基准图像点(u,v)处，与模板图像相对应位置处的灰度值；
为基准图像(u,v)处与模板图像相同区域的均值；
2)确定景象匹配位置误差ΔLdsmac、Δλdsmac
其中，ΔLdsmac表示角度变化导致的景象匹配输出纬度误差、Δλdsmac表示角度变化导致的景象匹配输出经度误差，单位为弧度；
表示由航向角组成的变换矩阵；
KL、Kλ分别表示与景象匹配输出的纬度、经度姿态补偿系数；
γ、θ分别表示惯导输出的滚转、俯仰角，单位为弧度；
3)图像投影畸变校正
确定原图像上的点P和校正后图像上的点P′关系如下式
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>p</mi>
<mn>1</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>p</mi>
<mn>2</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>p</mi>
<mn>3</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>4</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>5</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mn>6</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>7</mn>
</msub>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>p</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>p</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>p</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<msub>
<mi>H</mi>
<mi>a</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>4</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>5</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mn>6</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>h</mi>
<mn>7</mn>
</msub>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中，P和P′的齐次坐标分别为P＝[p1 p2 p3]T和P′＝[p′1 p′2 p′3]T；Ha是非奇异矩阵；
确定点P和P′的非齐次坐标关系如下式
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<mi>x</mi>
<mo>&prime;</mo>
</msup>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>h</mi>
<mn>0</mn>
</msub>
<mi>x</mi>
<mo>+</mo>
<msub>
<mi>h</mi>
<mn>1</mn>
</msub>
<mi>y</mi>
<mo>+</mo>
<msub>
<mi>h</mi>
<mn>2</mn>
</msub>
</mrow>
<mrow>
<msub>
<mi>h</mi>
<mn>6</mn>
</msub>
<mi>x</mi>
<mo>+</mo>
<msub>
<mi>h</mi>
<mn>7</mn>
</msub>
<mi>y</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mi>y</mi>
<mo>&prime;</mo>
</msup>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>h</mi>
<mn>3</mn>
</msub>
<mi>x</mi>
<mo>+</mo>
<msub>
<mi>h</mi>
<mn>4</mn>
</msub>
<mi>y</mi>
<mo>+</mo>
<msub>
<mi>h</mi>
<mn>5</mn>
</msub>
</mrow>
<mrow>
<msub>
<mi>h</mi>
<mn>6</mn>
</msub>
<mi>x</mi>
<mo>+</mo>
<msub>
<mi>h</mi>
<mn>7</mn>
</msub>
<mi>y</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
4)估计景象匹配与惯导安装误差状态向量
a)确定误差模型
选取17个系统状态组成系统状态向量X，17个系统状态分别为：
<mrow>
<msub>
<mi>&delta;V</mi>
<mi>n</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&delta;V</mi>
<mi>u</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&delta;V</mi>
<mi>e</mi>
</msub>
<mo>,</mo>
<mi>&delta;</mi>
<mi>L</mi>
<mo>,</mo>
<mi>&delta;</mi>
<mi>h</mi>
<mo>,</mo>
<mi>&delta;</mi>
<mi>&lambda;</mi>
<mo>,</mo>
<msub>
<mi>&phi;</mi>
<mi>n</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&phi;</mi>
<mi>u</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&phi;</mi>
<mi>e</mi>
</msub>
<mo>,</mo>
<msub>
<mo>&dtri;</mo>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mo>&dtri;</mo>
<mi>y</mi>
</msub>
<mo>,</mo>
<msub>
<mo>&dtri;</mo>
<mi>z</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&epsiv;</mi>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&epsiv;</mi>
<mi>y</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&epsiv;</mi>
<mi>z</mi>
</msub>
<mo>,</mo>
<msub>
<mi>&delta;L</mi>
<mrow>
<mi>d</mi>
<mi>s</mi>
<mi>m</mi>
<mi>a</mi>
<mi>c</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>&delta;&lambda;</mi>
<mrow>
<mi>d</mi>
<mi>s</mi>
<mi>m</mi>
<mi>a</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
其中：
δVn,δVu,δVe分别表示捷联惯导系统北向、天向、东向的速度误差，单位为米每秒；
δL,δh,δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；
φn,φu,φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角，单位为弧度；
分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；
εx,εy,εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；
δLdsmac,δλdsmac分别表示景象匹配纬度、经度误差，单位为弧度。
确定系统状态方程如下：
<mrow>
<mover>
<mi>X</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>F</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mi>X</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>G</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mi>W</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
其中X(t)为上述17个状态；W(t)为系统白噪声；F(t)和G(t)为系数矩阵；
b)确定量测方程矩阵
利用下式确定量测值Z
<mrow>
<mi>Z</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>L</mi>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>L</mi>
<mrow>
<mi>d</mi>
<mi>s</mi>
<mi>m</mi>
<mi>a</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&lambda;</mi>
<mrow>
<mi>i</mi>
<mi>m</mi>
<mi>u</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>&lambda;</mi>
<mrow>
<mi>d</mi>
<mi>s</mi>
<mi>m</mi>
<mi>a</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
式中，Limu、λimu分别表示惯导输出的纬度、经度，单位为弧度；
Ldsmac、λdsmac分别表示景象匹配输出的纬度、经度，单位为弧度；
量测值Z分别带入下式，解方程，得出量测方程矩阵H
<mrow>
<mi>Z</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<mi>L</mi>
<mo>-</mo>
<mi>&delta;</mi>
<msub>
<mi>L</mi>
<mrow>
<mi>d</mi>
<mi>s</mi>
<mi>m</mi>
<mi>a</mi>
<mi>c</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<mi>&lambda;</mi>
<mo>-</mo>
<msub>
<mi>&delta;&lambda;</mi>
<mrow>
<mi>d</mi>
<mi>s</mi>
<mi>m</mi>
<mi>a</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mi>V</mi>
</mrow>
Z＝HX+V
其中，V为量测噪声，考虑为白噪声。
c)卡尔曼滤波模型预估计得出景象匹配与惯导误差向量的最优估计值。
2.如权利要求1所述的一种大机动情况下景象匹配的方法，其特征在于，还包括步骤5)利用估计出的误差补偿景象匹配输出的位置信息，达到提高景象匹配定位精度的目的。
3.如权利要求1所述的一种大机动情况下景象匹配的方法，其特征在于，所述的步骤4)中卡尔曼滤波过程如下：
计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：
<mrow>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mi>I</mi>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>n</mi>
</msub>
<msub>
<mi>F</mi>
<msub>
<mi>T</mi>
<mi>n</mi>
</msub>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>n</mi>
</msub>
<msub>
<mi>F</mi>
<mrow>
<mn>2</mn>
<msub>
<mi>T</mi>
<mi>n</mi>
</msub>
</mrow>
</msub>
<mo>+</mo>
<mo>...</mo>
<mo>...</mo>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>n</mi>
</msub>
<msub>
<mi>F</mi>
<mrow>
<msub>
<mi>NT</mi>
<mi>n</mi>
</msub>
</mrow>
</msub>
<mo>=</mo>
<mi>I</mi>
<mo>+</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>N</mi>
</munderover>
<msub>
<mi>T</mi>
<mi>n</mi>
</msub>
<msub>
<mi>F</mi>
<mrow>
<msub>
<mi>iT</mi>
<mi>n</mi>
</msub>
</mrow>
</msub>
</mrow>
式中：
Tn为导航周期，NTn为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵；
状态一步预测
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
状态估计
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mo>&lsqb;</mo>
<msub>
<mi>Z</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&rsqb;</mo>
</mrow>
滤波增益矩阵
<mrow>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>H</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msup>
<mrow>
<mo>&lsqb;</mo>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>H</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>R</mi>
<mi>k</mi>
</msub>
<mo>&rsqb;</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
</mrow>
一步预测误差方差阵
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>Q</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
</mrow>
估计误差方差阵
Pk＝[I-KkHk]Pk,k-1
其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610915460.6A CN107966147B (zh) | 2016-10-20 | 2016-10-20 | 一种大机动情况下景象匹配的方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610915460.6A CN107966147B (zh) | 2016-10-20 | 2016-10-20 | 一种大机动情况下景象匹配的方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107966147A true CN107966147A (zh) | 2018-04-27 |
| CN107966147B CN107966147B (zh) | 2021-02-05 |
Family
ID=61996488
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610915460.6A Active CN107966147B (zh) | 2016-10-20 | 2016-10-20 | 一种大机动情况下景象匹配的方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107966147B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101532841A (zh) * | 2008-12-30 | 2009-09-16 | 华中科技大学 | 基于地标捕获跟踪的飞行器导航定位方法 |
| CN102538781A (zh) * | 2011-12-14 | 2012-07-04 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
| EP2372656A3 (en) * | 2010-03-04 | 2012-10-17 | Honeywell International Inc. | Method and apparatus for vision aided navigation using image registration |
| CN103093193A (zh) * | 2012-12-28 | 2013-05-08 | 中国航天时代电子公司 | 一种空地图像制导武器目标识别方法 |
| CN102506867B (zh) * | 2011-11-21 | 2014-07-30 | 清华大学 | 基于Harris角点匹配的SINS/SMANS组合导航方法及系统 |
| CN104006708A (zh) * | 2014-05-30 | 2014-08-27 | 河南科技大学 | 一种基于景象匹配的地面目标间接定位方法 |
| CN104138661A (zh) * | 2014-03-27 | 2014-11-12 | 北京太阳光影影视科技有限公司 | 一种巨型幕多人射击互动景物定位方法 |
| CN104330075A (zh) * | 2014-10-20 | 2015-02-04 | 中北大学 | 栅格化极坐标系目标定位方法 |
| CN104809720A (zh) * | 2015-04-08 | 2015-07-29 | 西北工业大学 | 基于小交叉视场的两相机目标关联方法 |
| CN105574886A (zh) * | 2016-01-28 | 2016-05-11 | 多拉维（深圳）技术有限公司 | 手持多目相机高精度标定方法 |
| CN103954283B (zh) * | 2014-04-01 | 2016-08-31 | 西北工业大学 | 基于景象匹配/视觉里程的惯性组合导航方法 |
-
2016
- 2016-10-20 CN CN201610915460.6A patent/CN107966147B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101532841A (zh) * | 2008-12-30 | 2009-09-16 | 华中科技大学 | 基于地标捕获跟踪的飞行器导航定位方法 |
| EP2372656A3 (en) * | 2010-03-04 | 2012-10-17 | Honeywell International Inc. | Method and apparatus for vision aided navigation using image registration |
| CN102506867B (zh) * | 2011-11-21 | 2014-07-30 | 清华大学 | 基于Harris角点匹配的SINS/SMANS组合导航方法及系统 |
| CN102538781A (zh) * | 2011-12-14 | 2012-07-04 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
| CN103093193A (zh) * | 2012-12-28 | 2013-05-08 | 中国航天时代电子公司 | 一种空地图像制导武器目标识别方法 |
| CN104138661A (zh) * | 2014-03-27 | 2014-11-12 | 北京太阳光影影视科技有限公司 | 一种巨型幕多人射击互动景物定位方法 |
| CN103954283B (zh) * | 2014-04-01 | 2016-08-31 | 西北工业大学 | 基于景象匹配/视觉里程的惯性组合导航方法 |
| CN104006708A (zh) * | 2014-05-30 | 2014-08-27 | 河南科技大学 | 一种基于景象匹配的地面目标间接定位方法 |
| CN104330075A (zh) * | 2014-10-20 | 2015-02-04 | 中北大学 | 栅格化极坐标系目标定位方法 |
| CN104809720A (zh) * | 2015-04-08 | 2015-07-29 | 西北工业大学 | 基于小交叉视场的两相机目标关联方法 |
| CN105574886A (zh) * | 2016-01-28 | 2016-05-11 | 多拉维（深圳）技术有限公司 | 手持多目相机高精度标定方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107966147B (zh) | 2021-02-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
| CN103411609B (zh) | 一种基于在线构图的飞行器返航路线规划方法 | |
| CN105241445B (zh) | 一种基于智能移动终端的室内导航数据获取方法及系统 | |
| CN105371840A (zh) | 一种惯性/视觉里程计/激光雷达的组合导航方法 | |
| JP4448187B2 (ja) | 映像の幾何補正方法およびその装置 | |
| WO2012118207A1 (ja) | 局所地図生成装置、局所地図生成システム、グローバル地図生成装置、グローバル地図生成システム、及びプログラム | |
| CN110221328A (zh) | 一种组合导航方法和装置 | |
| CN106960174A (zh) | 高分影像激光雷达高程控制点提取及其辅助定位方法 | |
| CN114216454B (zh) | 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| JP6060642B2 (ja) | 自己位置推定装置 | |
| CN105352509A (zh) | 地理信息时空约束下的无人机运动目标跟踪与定位方法 | |
| CN111024072B (zh) | 一种基于深度学习的卫星地图辅助导航定位方法 | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN113447949B (zh) | 一种基于激光雷达和先验地图的实时定位系统及方法 | |
| JP2008175786A (ja) | 移動体位置検出方法および移動体位置検出装置 | |
| CN114136315B (zh) | 一种基于单目视觉辅助惯性组合导航方法及系统 | |
| CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| CN114693754B (zh) | 一种基于单目视觉惯导融合的无人机自主定位方法与系统 | |
| JP6589410B2 (ja) | 地図生成装置及びプログラム | |
| CN113639722B (zh) | 连续激光扫描配准辅助惯性定位定姿方法 | |
| JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
| CN111189446B (zh) | 一种基于无线电的组合导航方法 | |
| KR102506411B1 (ko) | 차량의 위치 및 자세 추정 방법, 장치 및 이를 위한 기록매체 | |
| CN107966147A (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 |