的协方差矩阵：
[0069]
卿
[0070] 3、基于单应关系的运动估计
[0071] 无人机运动参数化2、t2和m与相邻的两帖图像之间的单应矩阵满足关系式
[007^
仍
[0073] 其中，Ru是将第一帖图像相机坐标系中的向量变换到第二帖图像相机坐标系中向 量的旋转矩阵，t2为第二帖图像相对于第一帖图像的相机坐标系的相对位移，m是第一帖图 像相机坐标系中垂直于图像平面的单位向量(取沿相机向外的方向为正），1为单位矩阵，A 为相机的标定矩阵，山是第一帖图像相机位置到图像平面之间的距离。
[0074] 某干奋择估A解，h式可Pi亲示为：
[0075]
(8)
[0076] 因此，已知相邻两帖图像之间的单应关系化2、相机标定矩阵A和第一帖图像相机位 置到图像平面之间的距离di时，由（9)式可W由无人机捕获的图像序列对无人机运动参数 Ri2、t2和ni进行求解。
[0077] 4、更新图像拼接数据库
[0078] 每当UAV采集到一帖新的图像，需要对拼接数据库进行更新，该过程包括：
[0079] 1)计算当前图像与前一帖图像之间的单应矩阵及其协方差；
[0080] 2)将该单应矩阵乘W拼接数据库内之前所有的单应矩阵，即计算组合单应矩阵， 从而得到该图像在拼接中相对参考帖的位置；
[0081] 3)将该图像与前一帖图像之间的单应矩阵（图像i与前一帖图像的单应矩阵记作 H(i-m)、与参考帖图像Xi的组合单应矩阵、单应矩阵的协方差矩阵及相对参考帖的位置等 信息插入到拼接数据库中。
[0082] 5、基于扩展卡尔曼滤波的位置估计
[0083] 结合当前图像与图像拼接数据库检测航迹是否闭环，即通过计算当前图像与拼接 数据库中单帖或多帖图像的重叠来检测航迹闭环的终点，运里根据马氏距离来检测一幅图 像中屯、与估计位置的距离是否在一个经验值范围内。若未检测到航迹闭环的终点则跳过此 步骤，若检测到航迹闭环的终点即图像序列的第i帖图像和第j帖图像重叠时，启动特征匹 配过程校准重叠图像Ii与图像。，并基于扩展卡尔曼滤波进行位置参数的鲁棒估计，其原理 框图见图1。
[0084] 在时刻t，图像Ii在拼接过程中的估计位置记作氣，若检测到其与图像。有重叠， 后者的估计位置记作，则由。到Ii的变换关系可W定义为：
[0085]
(10)
[0086] 由于单应性估计存在不准确性，校准过程中的误差不可避免，即变换图像r Qi根 据1?,,变换后的结果）和图像。之间存在估计误差。考虑拼接过程中的不确定性，在下式约 束条件下，启动邸F滤波器对图像间的单应性估计进行更新：
[0087]
(1巧
[0088] a)建立动态模型
[0089] 状态方程和量测方程分别建立如下：
[0090] X(k)=f(x(k-l))+w 化-1) (12)
[0091] z(k)=g(x(k-l))+n(k) (13)
[0092] 其中，状态X由其均值和协方差矩阵P表示。w(k-l)和n(k)分别是零均值高斯的过 程噪声和量测噪声，Q和R为其各自的协方差矩阵。矩阵.
分别为状态 方程和量测方程关于状态的雅可比行列式。
[0093] b)计算协方差矩阵
[0094] 假设。和Ii之间共包含n+1张图片，而校准重叠图像的单应矩阵为田1，协方差矩阵 为.为简单起见，假定j = 0 (即参考帖），i = n。
[0095] 先验状态向量由图像Ii，…，In关于Io的n个转换关系式组成：
[0096] X-=[Xl，X2，...，Xn]T=&〇l，Xl?hl2，...，Xn-l?h(n-l)n]T(14)
[0097] 其中，分别是Xi和出J的向量形式。状态可W通过迭代得到，若Xi表示k = j+l =1时刻的状态，则X2表示k=j+2 = 2时刻的状态，并且可W写成Xi的函数，W此类推。
[009引 C)计算状态向量
[0099] 根据如下预测方程逐步完成对n个状态的预测：
[0100] Xi = Xi-I ? h(i-i)i (15)
[0101] 易得上式关于状态和的雅可比行列式，分别记作A和W，则Xi的协方差矩阵 为：
[0102] (化)
[0103] d)计算卡尔曼增益
[0104] 将状态向量按序排列W便用量测Hon和C；。，对其进行更新。的状态Xn表示从当前 图像n到闭环参考帖0的变换，则有h〇n = Xn，并有如下量测方程：
[010引 h0n = Gx= [09X(9n)，l9X9]X (17)
[0106] 根据邸曰经典方程式计算卡尔曼增益。
[0107] e)更新状态向量和协方差矩阵
[0108] 当检测到闭环终点时，利用校正信息对状态向量及其协方差矩阵进行更新，状态 向量按下式完成更新：
[0109] x = x-+K 化血-Gx-) (18)
[0110] 式(15)的矩阵形式如下：
[0111] H(i-m = (Xi-i)-iXi (19)
[0112] 通过计算该式的雅克比行列式可W对协方差矩阵进行更新。
[0113] 综上，即可修正闭环内所有图像的位置，完成对闭环内所有相邻两帖图像间单应 关系的更新，并将更新后的信息插入到图像拼接数据库中，修正无人机累积位置估计误差， 从而实现无人机位置的精确估计。算法在计算过程中不需要获得所有相邻两帖图像间的真 实运动，仅需考虑误差累积，即可消除漂移，在降低位置估计算法复杂度的同时，可W提高 位置估计精度。
【主权项】
1. 一种基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法，其特征在于步骤如 下： 步骤1、对无人机捕获的图像序列中相邻的两帧图像进行单应矩阵的鲁棒估计:首先提 取相邻的两帧图像的特征点，然后进行匹配： 1) 当匹配点被追踪的成功率超过65%时，采用全单应模型，利用LMeDS剔除误匹配点， 并采用Μ估计器计算单应矩阵； 2) 当匹配点被追踪的成功率在40%到65%之间时，采用仿射单应模型，并利用松弛Μ估 计器估算单应矩阵； 3) 当匹配点被追踪的成功率低于40%时，采用欧式单应模型，并利用最小二乘计算单 应矩阵； 步骤2、计算单应矩阵估计的协方差： 给定一个具有η对匹配点的集合Sm= {{mi，n/ i}，{m2，n/ 2}，…，{mn，n/ η}}，其中，mi和n/ i 分别表示两个匹配点的像素位置，则描述不确定性的协方差矩阵Ch的计算过程为： 1) 计算从m到π/的单应性变换的雅克比矩阵J; 2) 计算每对匹配点匹配误差的协方差，g卩m' 之间残差的协方差，假设各 对匹配点的匹配误差之间不相关，则总协方差^Cm = …; 3) 计算单应估计的协方差矩阵，即= (JTC^J)-1; 步骤3、基于单应关系的运动估计： 无人机运动参数与相邻的两帧图像之间的单应矩阵满足关系式 ，其中R12是将第一帧图像相机坐标系中的向量变换到第二帧图像 相机坐标系中向量的旋转矩阵，t2是第二帧图像相对于第一帧图像的相机坐标系的相对位 移，m为第一帧图像相机坐标系中垂直于图像平面的单位向量（取沿相机向外的方向为 正），1为单位矩阵，A为相机标定矩阵，cU是第一帧图像相机位置到图像平面之间的距离。基 于奇异值分解，关系式Ηι2=ΑΚ12(Ι-?^站)#可以表示为滅^ 已知H12、A和cU时，根据该关系式可以由无人机捕获的图像序列对无人机运动参数1?12、^和 m进行求解； 步骤4:更新图像拼接数据库： 每当UAV采集到一帧新的图像，图像拼接数据库的更新参数包括： 1) 当前图像与前一帧图像之间的单应矩阵及其协方差； 2) 将该单应矩阵乘以拼接数据库内之前所有的单应矩阵，即计算组合单应矩阵，更新 该图像在拼接中相对参考帧的位置； 3) 将该图像与前一帧图像之间的单应矩阵H(1-m、与参考帧图像组合单应矩阵、单 应矩阵的协方差矩阵及相对参考帧的位置信息插入到拼接数据库中； 步骤5:基于扩展卡尔曼滤波的位置估计： 若未检测到当前图像与图像拼接数据库航迹形成闭环则跳过此步骤，继续采集下一帧 图像，重复步骤1; 若检测到航迹闭环的终点即图像序列的第i帧图像和第j帧图像重叠时，启动特征匹配 过程校准重叠图像与图像I』，更新位置估计，其步骤包括： 1) 建立动态模型:状态方程为x(k) = f (x(k-l))+w(k-l)，量测方程为z(k)=g(x(k-l)) +n(k)，w(k-l)和n(k)分别是零均值高斯的过程噪声和量测噪声，Q和R为其各自的协方差矩 阵，记發7&和= 分别为状态方程和量测方程关于状态的雅可比行列式； 2) 计算协方差矩阵:设I4PL·之间共包含n+1张图片，而校准重叠图像的单应矩阵为H#， 协方差矩阵为（\..,假定」=〇，1=]1，则有先验状态向量厂=|^1，12，~1 11]1'=[11()1，11， hi2，."，xn-1 · h(n-ι)η]τ，状态可以通过迭代得到; 3) 计算状态向量:根据预测方程Xl = Xl-i · h(1-m逐步完成对η个状态的预测，易得该式 关于状态和11(1-1)1的雅可比行列式Α和Wd〗 Xl的协方差矩阵表示为 4) 计算卡尔曼增益：将状态向量按序排列，取h〇n = Gx= [〇9X(9n)，l9X9]X，根据卡尔曼滤 波经典方程式计算卡尔曼增益； 5) 更新状态向量和协方差矩阵:状态向量按照式X = x>K (h〇n-Gx_)进行更新，协方差矩 阵通过求解式雅克比行列式获得； 6) 修正闭环内所有图像的位置:完成对闭环内所有相邻两帧图像间单应关系的更新， 并将更新后的信息插入到图像拼接数据库中，从而实现无人机位置的精确估计； 继续采集下一帧图像，重复步骤1，直至飞行结束。
【专利摘要】本发明涉及一种基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法，利用在线图像拼接来消除无人机位置估计的累积误差，并实时构建环境地图，可以有效提高无人机位置参数估计的精确性。该方法利用在线图像拼接消除无人机位置估计的累积误差，基于图像帧间单应矩阵鲁棒估计方法，并考虑图像间单应关系和不确定性，实现对无人机位置参数的精确估计，并利用扩展卡尔曼滤波对无人机位置估计结果进行预测与更新，从而显著提高无人机的位置估计精度。
【IPC分类】G06T7/00, G06T7/20, G06T3/40, G01C11/06
【公开号】CN105719314
【申请号】CN201610066069
【发明人】潘泉, 靳珍璐, 赵春晖, 魏妍妍, 王荣志
【申请人】西北工业大学
【公开日】2016年6月29日
【申请日】2016年1月30日