[发明专利]一种基于特征点匹配和光流法的多旋翼速度测量方法

摘要

本发明公开一种基于特征点匹配和光流法的多旋翼速度测量方法，该方法步骤具体如下：步骤一：单目相机内参数离线标定；步骤二：基于相邻图像之间的运动估计；步骤三：基于视觉信息的速度估计；步骤四：基于卡尔曼滤波的速度估计。本发明方法是一种利用多旋翼机载的单目相机和其它传感器进行视觉测量的方法，不依靠额外增加设备，成本低廉，克服地面纹理简单而无法获取特征点的困难，算法鲁棒性高等优点。

主权项

1.一种基于特征点匹配和光流法的多旋翼速度测量方法，其特征在于：该方法步骤具体如下：步骤一：单目相机内参数离线标定步骤二：基于相邻图像之间的运动估计利用相邻图像数据，来估计全局光流，分为两种情况，一种是地面有丰富纹理，另一种是地面只有简单纹理；具体操作如下：S21、对于丰富纹理的地面针对相邻帧图像I₁,I₂，首先分别提取特征角点并计算其FREAK描述子，接着利用最近邻原则在两帧图像中匹配特征角点，最后利用RANSAC算法剔除野点，最终得到N对匹配点对则丰富纹理地面的全局光流可以计算如下：其中，函数median()是MATLAB中用来获取给定数组的中位数；S22、对于简单纹理的地面利用LK光流、迭代求精和图像金字塔原理，估计图像仿射运动模型参数，进而估计全局光流；步骤三：基于视觉信息的速度估计根据步骤S21在丰富纹理地面得到的全局光流以及步骤S22在简单纹理地面最终得到的全局光流，假定p_k‑1,p_k为相邻两帧像素点，其中当前帧像素点p_k为中心像素坐标，上一帧对应像素点根据所述两个全局光流获得，即有接着根据标定的相机内参数，得到对应的归一化坐标最后，直接给出基于视觉信息的速度估计其中，为根据视觉直接得到的飞行器速度估计值，为高度传感器的读数，为三维单位矩阵，e₃＝[0 0 1]^T，δt＝0.05s为相邻图像时间间隔，为陀螺仪测得的三维角速度，直接由公式(8)得出，为旋转矩阵，直接由多旋翼的欧拉角得出：其中，θ,φ,ψ分别是多旋翼的俯仰角、滚转角和偏航角；步骤四：基于卡尔曼滤波的速度估计S41、构建系统的状态变量、过程模型和观测模型；状态变量：其中，v_k为待测三维速度，z_k为飞行器z轴方向的高度值，b_a为三维加速度偏移；过程模型：x_k＝Ax_k‑1+u_k‑1+w_k              (12)其中，为系统转移矩阵，为控制输入，为系统噪声，表征系统模型的不确定性，它们有如下表达式其中，a_x,a_y,a_z分别为加速度计读数；噪声w_k假设为高斯白噪声，其噪声方差阵为为对角阵；观测模型：z_k＝Hx_k+v_k             (14)其中，观测量包括步骤三中通过视觉信息得到的水平速度以及高度传感器测得的高度；为观测转移矩阵，为观测噪声，表征观测量的不确定性，假设v_k为高斯白噪声，其噪声方差阵为它们的表达式如下S42、滤波初始化令状态初值为：x₀＝[v_c d_sonarcosθcosφ 0_3×1]^T          (16)其中，v_c＝[v_x v_y v_z]^T为公式(9)给出的初始视觉速度，高度初值由高度传感器给出，其中d_sonar为高度传感器读数，加速度偏移初值设为零；令状态估计误差协方差初值为对角矩阵：令k＝0,P_0|0＝P₀；S43、状态一步预测S44、误差协方差一步预测P_k|k‑1＝AP_k‑1|k‑1A^T+Q_k‑1            (19)S45、卡尔曼滤波增益更新K_k＝P_k|k‑1H^T(HP_k|k‑1H^T+R_k)^‑1       (20)S46、状态更新校正S47、误差协方差更新校正P_k|k＝(I₇‑K_kH)P_k|k‑1         (22)S48、k＝k+1，返回步骤S43继续运行；至此，卡尔曼滤波器不断迭代递推，就可以实时估计出飞行器的速度。