[发明专利]基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法

摘要

本发明涉及一种基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法，利用在线图像拼接来消除无人机位置估计的累积误差，并实时构建环境地图，可以有效提高无人机位置参数估计的精确性。该方法利用在线图像拼接消除无人机位置估计的累积误差，基于图像帧间单应矩阵鲁棒估计方法，并考虑图像间单应关系和不确定性，实现对无人机位置参数的精确估计，并利用扩展卡尔曼滤波对无人机位置估计结果进行预测与更新，从而显著提高无人机的位置估计精度。

主权项

一种基于单应性估计和扩展卡尔曼滤波的无人机位置估计方法，其特征在于步骤如下：步骤1、对无人机捕获的图像序列中相邻的两帧图像进行单应矩阵的鲁棒估计：首先提取相邻的两帧图像的特征点，然后进行匹配：1)当匹配点被追踪的成功率超过65％时，采用全单应模型，利用LMeDS剔除误匹配点，并采用M估计器计算单应矩阵；2)当匹配点被追踪的成功率在40％到65％之间时，采用仿射单应模型，并利用松弛M估计器估算单应矩阵；3)当匹配点被追踪的成功率低于40％时，采用欧式单应模型，并利用最小二乘计算单应矩阵；步骤2、计算单应矩阵估计的协方差：给定一个具有n对匹配点的集合S_m＝{{m₁，m′₁},{m₂，m′₂},…,{m_n，m′_n}}，其中，m_i和m′_i分别表示两个匹配点的像素位置，则描述不确定性的协方差矩阵C_h的计算过程为：1)计算从m到m′的单应性变换的雅克比矩阵J；2)计算每对匹配点匹配误差的协方差即m′_i和之间残差的协方差，假设各对匹配点的匹配误差之间不相关，则总协方差为3)计算单应估计的协方差矩阵，即步骤3、基于单应关系的运动估计：无人机运动参数R₁₂、t₂和n₁与相邻的两帧图像之间的单应矩阵满足关系式其中R₁₂是将第一帧图像相机坐标系中的向量变换到第二帧图像相机坐标系中向量的旋转矩阵，t₂是第二帧图像相对于第一帧图像的相机坐标系的相对位移，n₁为第一帧图像相机坐标系中垂直于图像平面的单位向量(取沿相机向外的方向为正)，I为单位矩阵，A为相机标定矩阵，d₁是第一帧图像相机位置到图像平面之间的距离。基于奇异值分解，关系式可以表示为已知H₁₂、A和d₁时，根据该关系式可以由无人机捕获的图像序列对无人机运动参数R₁₂、t₂和n₁进行求解；步骤4：更新图像拼接数据库：每当UAV采集到一帧新的图像，图像拼接数据库的更新参数包括：1)当前图像与前一帧图像之间的单应矩阵及其协方差；2)将该单应矩阵乘以拼接数据库内之前所有的单应矩阵，即计算组合单应矩阵，更新该图像在拼接中相对参考帧的位置；3)将该图像与前一帧图像之间的单应矩阵H_(i‑1)i、与参考帧图像X_i的组合单应矩阵、单应矩阵的协方差矩阵及相对参考帧的位置信息插入到拼接数据库中；步骤5：基于扩展卡尔曼滤波的位置估计：若未检测到当前图像与图像拼接数据库航迹形成闭环则跳过此步骤，继续采集下一帧图像，重复步骤1；若检测到航迹闭环的终点即图像序列的第i帧图像和第j帧图像重叠时，启动特征匹配过程校准重叠图像I_i与图像I_j，更新位置估计，其步骤包括：1)建立动态模型：状态方程为x(k)＝f(x(k‑1))+w(k‑1)，量测方程为z(k)＝g(x(k‑1))+n(k)，w(k‑1)和n(k)分别是零均值高斯的过程噪声和量测噪声，Q和R为其各自的协方差矩阵，记和分别为状态方程和量测方程关于状态的雅可比行列式；2)计算协方差矩阵：设I_j和I_i之间共包含n+1张图片，而校准重叠图像的单应矩阵为H_ji，协方差矩阵为假定j＝0，i＝n，则有先验状态向量x^‑＝[x₁，x₂，…x_n]^T＝[h₀₁，x₁，h₁₂，…，x_n‑1·h_(n‑1)n]^T，状态可以通过迭代得到；3)计算状态向量：根据预测方程x_i＝x_i‑1·h_(i‑1)i逐步完成对n个状态的预测，易得该式关于状态和h_(i‑1)i的雅可比行列式A和W，则x_i的协方差矩阵表示为$C_{x_i}={\mathrm{AC}}_{x_{i-1}}{A}^T+{\mathrm{WC}}_{h_{(i-1)i}}{W}^T;$4)计算卡尔曼增益：将状态向量按序排列，取h_0n＝Gx＝[0_9×(9n)，I_9×9]x，根据卡尔曼滤波经典方程式计算卡尔曼增益；5)更新状态向量和协方差矩阵：状态向量按照式x＝x^‑+K(h_0n‑Gx^‑)进行更新，协方差矩阵通过求解式H_(i‑1)i＝(X_i‑1)^‑1X_i的雅克比行列式获得；6)修正闭环内所有图像的位置：完成对闭环内所有相邻两帧图像间单应关系的更新，并将更新后的信息插入到图像拼接数据库中，从而实现无人机位置的精确估计；继续采集下一帧图像，重复步骤1，直至飞行结束。