基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法
【技术领域】
[0001] 本发明属于无人机位置估计方法，设及一种基于单应性估计和扩展卡尔曼滤波的 无人机位置估计（A Homography Estimaiton and Extended Kalman Filter-based Localization Method for Unmanned Aerial Vehicle,肥-EKF-LM)方法，该方法能够消除 无人机位置估计的累积误差，并实时构建环境地图，实现无人机位置参数的精确估计功能。
【背景技术】
[0002] 在无人机视觉导航领域，无人机位置估计与环境构图至关重要，无人机位置参数 的估计是实现自主导航的核屯、，研究可靠、高精度的视觉位置估计方法是提高无人机位置 参数估计精度的重要手段，具有重要的理论意义和应用价值。
[0003] 在线图像拼接技术，当无人机对某一区域重访时，认为其航迹形成闭环，并利用该 区域的位置信息在扩展卡尔曼滤波器化Xtended Kalman FiIter,邸F)的框架下检测并消 除无人机位置估计的累积漂移，从而为无人机提供精确的位置信息。一般情况下，计算图像 之间的单应关系，可W对无人机的位置进行实时估计，然而，随着时间增加，传统的静态图 像拼接方法对位置估计的累积误差会不可避免的增大，位置参数估计结果并不准确。需要 考虑拼接过程中的不确定性因素，能够完成在线图像拼接，在无人机位置估计过程中保持 一定的可靠性。与传统方法相比，基于在线图像拼接的位置估计技术在位置估计精度和可 靠性方面具有较大优势。
【发明内容】
[0004] 要解决的技术问题
[0005] 为了避免现有技术的不足之处，本发明提出一种基于单应性估计和扩展卡尔曼滤 波的无人机位置估计方法。
[0006] 技术方案
[0007] -种基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法，其特征在于步骤 如下：
[000引步骤1、对无人机捕获的图像序列中相邻的两帖图像进行单应矩阵的鲁棒估计:首 先提取相邻的两帖图像的特征点，然后进行匹配：
[0009] 1)当匹配点被追踪的成功率超过65%时，采用全单应模型，利用LMeDS剔除误匹配 点，并采用M估计器计算单应矩阵；
[0010] 2)当匹配点被追踪的成功率在40%到65%之间时，采用仿射单应模型，并利用松 弛M估计器估算单应矩阵；
[0011] 3)当匹配点被追踪的成功率低于40%时，采用欧式单应模型，并利用最小二乘计 算单应矩阵；
[0012] 步骤2、计算单应矩阵估计的协方差：
[001；3]给定一个具有n对匹配点的集合Sm= {{mi，n/ i}，{m2，n/ 2}，…，{mn，n/n}}，其中，Hii和 n/I分别表示两个匹配点的像素位置，则描述不确定性的协方差矩阵Ch的计算过程为：
[0014] 1)计算从m到心的单应性变换的雅克比矩阵J;
[001引2)计算每对匹配点匹配误差的协方差即m' 1和放;二撤之间残差的协方差，假 设各对匹配点的匹配误差之间不相关，则总协方差巧
[0016] 3)计算单应估计的协方差矩阵，良[
[0017] 步骤3、基于单应关系的运动估计：
[0018] 韦人化坛动参数Ri2、t2和ni与相邻的两帖图像之间的单应矩阵满足关系式
，其中Ru是将第一帖图像相机坐标系中的向量变换到第二帖图像 相机坐标系中向量的旋转矩阵，t2是第二帖图像相对于第一帖图像的相机坐标系的相对位 移，m为第一帖图像相机坐标系中垂直于图像平面的单位向量（取沿相机向外的方向为 正），I为单位矩阵，A为相机标定矩阵，山是第一帖图像相机位置到图像平面之间的距离。基 于奇异值分解，关系式
巧W表示为
已知出2、A和山时，根据该关系式可W由无人机捕获的图像序列对无人机运动参数Rl2、t2和 m进行求解；
[0019] 步骤4:更新图像拼接数据库：
[0020] 每当UAV采集到一帖新的图像，图像拼接数据库的更新参数包括：
[0021] 1)当前图像与前一帖图像之间的单应矩阵及其协方差；
[0022] 2)将该单应矩阵乘W拼接数据库内之前所有的单应矩阵，即计算组合单应矩阵， 更新该图像在拼接中相对参考帖的位置；
[0023] 3)将该图像与前一帖图像之间的单应矩阵与参考帖图像Xi的组合单应矩 阵、单应矩阵的协方差矩阵及相对参考帖的位置信息插入到拼接数据库中；
[0024] 步骤5:基于扩展卡尔曼滤波的位置估计：
[0025] 若未检测到当前图像与图像拼接数据库航迹形成闭环则跳过此步骤，继续采集下 一帖图像，重复步骤1;
[0026] 若检测到航迹闭环的终点即图像序列的第i帖图像和第j帖图像重叠时，启动特征 匹配过程校准重叠图像Ii与图像。，更新位置估计，其步骤包括：
[0027] 1)建立动态模型：状态方程为X化）=f(x(k-l))+w(k-l)，量测方程为z(k)=g(x 化-l))+n化），w化-1)和n化)分别是零均值高斯的过程噪声和量测噪声，Q和R为其各自的协 方差矩阵，记
分别为状态方程和量测方程关于状态的雅可比行列 式；
[0028] 2)计算协方差矩阵:设。和Ii之间共包含n+1张图片，而校准重叠图像的单应矩阵 为町1，协方差矩阵为（\;,假定^' = 0，1=11，则有先验状态向量义-=[山^，。'，^^=比01， Xl ? hl2，…，Xn-I ? h(n-l)n]T，状态可W通过迭代得到；
[0029] 3)计算状态向量:根据预测方程Xi = Xi-I ? h(i-i)i逐步完成对n个状态的预测，易得 该式关于状态和h ( 1 - 1 ) 1的雅可比行列式A和W，则X 1的协方差矩阵表示为
[0030] 4)计算卡尔曼增益:将状态向量按序排列，取h日n = Gx=[09X(9n)，I9X9]X，根据卡尔 曼滤波经典方程式计算卡尔曼增益；
[003。 5)更新状态向量和协方差矩阵:状态向量按照式x = x>K(h0n-G。进行更新，协方 差矩阵通过求解式恥-1)1 =化-1厂吃的雅克比行列式获得；
[0032 ] 6)修正闭环内所有图像的位置:完成对闭环内所有相邻两帖图像间单应关系的更 新，并将更新后的信息插入到图像拼接数据库中，从而实现无人机位置的精确估计；
[0033] 继续采集下一帖图像，重复步骤1，直至飞行结束。
[0034] 有益效果
[0035] 本发明提出的一种基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法，利 用在线图像拼接来消除无人机位置估计的累积误差，并实时构建环境地图，可W有效提高 无人机位置参数估计的精确性。该方法利用在线图像拼接消除无人机位置估计的累积误 差，基于图像帖间单应矩阵鲁棒估计方法，并考虑图像间单应关系和不确定性，实现对无人 机位置参数的精确估计，并利用扩展卡尔曼滤波对无人机位置估计结果进行预测与更新， 从而显著提高无人机的位置估计精度。
【附图说明】
[0036] 图1为原理框图
【具体实施方式】
[0037] 现结合实施例、附图对本发明作进一步描述：
[0038] 本发明主要包括两个核屯、部分:A、单应矩阵的估计及估计不确定性的描述。B、基 于扩展卡尔曼滤波的位置估计。其实现步骤如下：
[0039] 第1步，单应矩阵的鲁棒估计；
[0040] 第2步，单应矩阵的估计不确定性的描述；
[0041] 第3步，基于单应关系的运动估计；
[0042] 第4步，更新图像拼接数据库；
[0043] 第5步，基于扩展卡尔曼滤波的位置估计。
[0044] 具体实施例流程如图1所示。
[0045] 1、单应矩阵的鲁棒估计
[0046] 在齐次坐标中，任何单应关系都可W表示为如下可逆线性变换：
[0047]
(!)
[004引其中，H为单应矩阵，[x，y，U哺巧哀皆分别为同一点在两个不同投影空间内的坐 标向量。
[0049]无人机捕获的图像序列中相邻的两帖图像互相匹配的特征点之间不仅存在相机 运动引起的特征点位置偏移，还存在由监视区域非平面引起的不确定性。因此，单应性估计 过程一般可W分为剔除误匹配点和估计单应矩阵两个部分。其中，剔除误匹配点通常采用 最小中值算法(LMeDS)或随机采样一致性(RANSAC)方法，对于单应性拟合问题，LMeDS具有 更优的性能;估计单应矩阵时通常利用M估计器。
[0050] 匹配点较少时，单应性估计的结果并不理想，为了得到单应矩阵的鲁棒估计，可W 自适应地降低单应模型的复杂度，采用欧式、仿射和全单应矩阵等形式，其自由度分别为4、 6、8d
[0051] 根据点追踪器对匹配点集被成功追踪的比例，确定应当采用的单应模型、误匹配 点剔除方法及单应矩阵估计方法。W下给出的比例阔值是通过对数百张航空图片处理后得 到的经验值。
[0052] 1)全单应模型：该模型适用于匹配点被追踪的成功率超过65 %的情况，采用LMeDS 剔除误匹配点，并利用M估计器计算单应矩阵；
[0053] 2)仿射单应模型：如果匹配点被追踪的成功率在40%到65%之间，需要采用松弛M 估计器估算单应矩阵；
[0054] 3)欧式单应模型：如果匹配点被追踪的成功率低于40%，即数据集受干扰严重或 者数据量过小，则利用最小二乘计算单应矩阵。
[0055] 2、单应矩阵的估计不确定性的描述
[0056] 计算出单应矩阵后，由协方差矩阵衡量估计精度。（1)中的单应矩阵H可W写成如 下向量：
[0057] h=比 11 hi2...h33]T (2)
[005引则其协方差矩阵Ch是一个9X9的矩阵，对角线元素为变量的方差，上、下=角矩阵 元素为互协方差。
[0059] 给定一个具有n对匹配点的集合Sm:
[0060]
(1)
[0061] 其中，mi = [Xi，yi]T和n/I = 分别表示两个匹配点的像素位置。则描述不确 定性的协方差矩阵Ch的计算步骤为：
[0062] 1)计算从m到n/的单应性变换h的雅克比矩阵J，其中mi=[Xi，yi]T和n/ i=[x/ i，y ^1]满足：
[0063]
[0064] (4)
[00 化]
[0066] 2)计算每对匹配点匹配误差的协方差运里通过计算m'l和成;:::撤之间残差 的协方差并将其作为相应的匹配误差协方差。假设各对匹配点的匹配误差之间不相关，贝U 可得到如下对角矩阵形式的总协方差Cm:
[0067]
(5)
[0068] 3)计算单应性估计