本发明公开一种基于相位相关和光流法的多旋翼速度测量方法，其特征在于：该方法步骤具体如下：步骤一：单目相机内参数离线标定；步骤二：基于相邻图像之间的运动估计；步骤三：基于视觉信息的速度估计；步骤四：基于卡尔曼滤波的速度估计。本发明提出的基于相位相关和光流法的多旋翼速度测量方法，是一种利用多旋翼机载的单目相机和其它传感器进行视觉测量的方法，不依靠额外增加设备，成本低廉，克服地面纹理简单而无法获取特征点的困难，算法计算量小，算法鲁棒性高等优点。
1.一种基于相位相关和光流法的多旋翼速度测量方法，其特征在于：该方法步骤具体如下：步骤一：单目相机内参数离线标定
利用相邻图像数据，来估计全局光流，分为两种情况，一种是图像平移量较大的情况，另一种是图像平移量较小的情况；具体操作如下：S21、对于图像平移量较大的情况
针对相邻帧图像I1,I2，直接利用OpenCv的相位相关函数phaseCorrelate来获取图像之间亚像素级精度的平移量Tu,Tv，即需要的全局光流值；其中采用createHanningWindow函数生成汉宁窗来去除图像的边界效应；
利用LK光流、迭代求精和图像金字塔原理，估计图像仿射运动模型参数，进而估计全局光流；
根据步骤S21在平移量大的情况下得到的全局光流以及步骤S22在平移量小的情况下得到的全局光流，假定pk-1,pk为相邻两帧像素点，其中当前帧像素点pk为中心像素坐标，上一帧对应像素点根据所述两个全局光流获得，即有接着根据标定的相机内参数，得到对应的归一化坐标最后，直接给出基于视觉信息的速度估计
其中，αx为u轴上的尺度因子，αy为v轴上的尺度因子；u0,v0是摄像机光轴与图像平面的交点，称为主点坐标； 为根据视觉直接得到的飞行器速度估计值，hk-1, 为高度传感器的读数， 为三维单位矩阵，e3＝[0 0 1]T，δt＝0.05s为相邻图像时间间隔， 为陀螺仪测得的三维角速度， 直接由公式(7)得出，Rk-1, 为旋转矩阵，直接由多旋翼的欧拉角得出：
其中，θ,φ,ψ分别是多旋翼的俯仰角、滚转角和偏航角；
其中，vk为待测三维速度，zk为飞行器z轴方向的高度值，ba,k为三维加速度偏移；
xk＝Axk-1+uk-1+wk (11)其中， 为系统转移矩阵， 为控制输入， 为系统噪声，表征系统模型的不确定性，它们有如下表达式
其中，ax,ay,az分别为加速度计读数；噪声wk假设为高斯白噪声，其噪声方差阵为为对角阵；
zk＝Hxk+γk (13)其中，观测量 包括步骤三中通过视觉信息得到的水平速度以及高度传感器测得的高度； 为观测转移矩阵， 为观测噪声，表征观测量的不确定性，假设γk为高斯白噪声，其噪声方差阵为 H的表达式如下S42、滤波初始化
x0＝[vc dsonarcosθcosφ 03×1]T (15)其中，vc＝[vx vy vz]T为公式(8)给出的初始视觉速度，高度初值由高度传感器给出，其中dsonar为高度传感器读数，加速度偏移初值设为零；
Pk|k-1＝APk-1|k-1AT+Qk-1 (18)S45、卡尔曼滤波增益更新
Kk＝Pk|k-1HT(HPk|k-1HT+Rk)-1 (19)S46、状态更新校正
Pk|k＝(I7-KkH)Pk|k-1 (21)S48、k＝k+1，返回步骤S43继续运行；
至此，卡尔曼滤波器不断迭代递推，就可以实时估计出飞行器的速度。
2.根据权利要求1所述的一种基于相位相关和光流法的多旋翼速度测量方法，其特征在于：所述步骤S22对于图像平移量较小的情况，估计全局光流的具体过程如下：S221、给定两帧相邻图像，粗略估计出其LK光流；
S222、为进一步提高精度，设计迭代求精算法，根据步骤S221估计出的LK光流值对图像进行扭转操作，然后再进一步求取LK光流，由粗到精，经过三次迭代后确定光流值；
S223、针对简单纹理环境，增加图像金字塔，克服飞行器作快速大运动的情况所带来的跟踪困难。
3.根据权利要求2所述的一种基于相位相关和光流法的多旋翼速度测量方法，其特征在于：所述步骤S221具体过程如下：S2211、针对相邻帧图像I1,I2，分别计算当前图像每个像素(x,y)的灰度相对于时间、空间上的偏导数Iu(x,y),Iv(x,y),It(x,y)如下；为方便以下简写为Iu,Iv,It：其中符号 是卷积操作；
[Iu Iux Iuy Iv Ivx Ivy]a＝-It (3)其中a＝[a1 a2 a3 a4 a5 a6]T为待求解的仿射运动参数；
S2213、根据步骤S2212求解的运动参数，直接给出当前图像中每个像素的光流表达式：
4.根据权利要求2所述的一种基于相位相关和光流法的多旋翼速度测量方法，其特征在于：所述步骤S223具体过程如下：S2231、针对相邻图像I1,I2，分别获取其三层图像金字塔；金字塔图像分辨率成倍数缩小，即金字塔最底层分辨率是图像原始分辨率u_0，v_0；第一层是最底层分辨率的1/2，第二层是最底层分辨率的1/4，第三层是最底层分辨率的1/8；
S2232、针对I1,I2的第三层金字塔图像，根据步骤S222的迭代求精得到光流估计值S2233、利用所述的光流估计值 对图像I1的第二层金字塔图像进行扭转操作，得到的扭转后图像与图像I2的第二层金字塔图像进行迭代求精操作，得到光流估计值S2234、利用所述的光流估计值 对图像I1的第一层金字塔图像进行扭转操作，得到的扭转后图像与图像I2的第一层金字塔图像进行迭代求精操作，得到光流估计值S2235、得到最终全局光流估计
一种基于相位相关和光流法的多旋翼速度测量方法
技术领域
[0001] 本发明涉及一种基于相位相关和光流法的多旋翼速度测量方法，属于视觉测量领域。
背景技术
[0002] 多旋翼是一种依靠旋翼旋转产生升力的无人机。这类无人机通常具有优异的机动性能，可以垂直起降，悬停，灵活转向，因此广泛应用于航拍、遥感、农业、救援、监测、邮递等领域。对于多旋翼来说，快速且准确的获取自身速度能有效提高多旋翼控制的稳定性(提高阻尼)，从而达到更好的悬停和操控效果，因此测速工作为多旋翼更广泛使用起到十分重要的作用。
[0003] 在无GPS环境下，视觉传感器能提供比较精确的速度信息，融合惯性传感器信息可以获得比较精确的速度估计。而目前视觉处理的方法一方面是利用动作捕捉系统或者人工标志点，安装复杂且对环境要求严格；另一方面是利用机载视觉，采用特征点匹配的方法，计算量大，计算耗时，也要求环境中纹理特征丰富。因此，本专利申请提出了一种基于相位相关和光流法的多旋翼速度测量方法，该方法不需要额外增加设备，仅利用多旋翼的机载传感器，如单目下视相机、惯性传感器、高度传感器，来实时测量机体速度，在频域中分析图像信息，抗干扰性强，对环境没有特别要求，且在无GPS信号的未知环境中仍有效果。
发明内容
[0004] 本发明提供了一种基于相位相关和光流法的多旋翼速度测量方法，它是一种融合图像频域信息以及全局光流信息的测速方法。利用视觉信息、惯性传感器信息、高度传感器信息，并结合多旋翼运动学模型，提出了一种基于卡尔曼滤波的融合测速算法。它解决了小型无人机无法安装空速管、在室内等未知复杂环境中无法获取GPS信号进行速度测量，不需要进行特征点匹配带来的耗时的困难，不需要额外增加传感设备，方法简单便捷，计算量小，测量精度高，实用性好。
[0005] 本发明采用的相机模型为线性模型，即针孔成像模型，模型描述如下：
[0006] 如图1所示，空间一点pe在图像平面的成像位置可以用针孔成像模型近似表示，即点pe在图像平面中的投影位置pc，为光心O与空间点pe的连线与图像平面的交点。因而世界坐标系下pe点坐标(xe,ye,ze)T与投影点pc的像素坐标(u,v)T之间的关系可以描述如下[0007]
[0008] 其中αx为u轴上的尺度因子，αy为v轴上的尺度因子，且αx＝f/dx，αy＝f/dy(f为焦距，dx和dy分别为u轴和v轴方向的像素尺寸)；(u0,v0)是摄像机光轴与图像平面的交点，称为主点坐标。αx,αy,u0,v0只与摄像机内部参数有关，称为相机的内参数。 分别为相机坐标系与世界坐标系的旋转矩阵和平移向量，称为摄像机的外部参数。
[0009] 本发明中被测对象为一台多旋翼无人飞行器，要求其机载设备有单目下视相机、高度传感器和惯性传感器。本测速方法是一种实时在线估计方法，在测速之前，需要对相机进行离线标定操作，提前获取相机的内参数。整个在线测速流程见图2，主要包含以下三个部分：
[0010] 1)基于相邻图像之间的运动估计
[0011] 本部分的主要目的是根据相邻两帧图像之间的信息，估计帧间平移量。主要考虑两种情况：图像平移量较大和平移量小。针对图像有较大平移量时，频域上相位偏移明显，可以利用图像傅里叶变换，然后计算出相位偏移，频域上的相位偏移表征了图像之间的空间平移量；对于图像平移量较小的情况，利用LK光流算法；整个过程通过构建图像金字塔来克服大运动情况，通过迭代求精算法来降低噪声影响。
[0012] 2)基于视觉信息的速度估计
[0013] 本部分的主要目的是利用相机模型以及视觉运动约束，根据相邻图像之间的运动情况，估计速度信息。这部分根据视觉数据直接给出多旋翼速度信息。
[0014] 3)基于卡尔曼滤波的速度估计
[0015] 本部分的主要目的是利用卡尔曼滤波算法，融合视觉运动信息、惯性传感器信息以及高度传感器信息，并结合多旋翼运动学模型，估计多旋翼实时的速度信息，利用卡尔曼滤波可以有效地减小噪声影响，且在视觉信息短暂丢失的情况下滤波器仍能发挥作用，保证实时性。
[0016] 本发明提出了一种基于相位相关和光流法的多旋翼速度测量方法，其实现步骤具体如下：
[0017] 步骤一：单目相机内参数离线标定
[0018] 在具体实现过程中，本发明利用二维棋盘格标定板(如图3)作为靶标，固定单目相机，在相机视场内多角度移动靶标，保证整个靶标尽量占满图片。这样就可以得到图片序列(至少13张)，接下来，直接利用MATLAB R2017a自带的相机标定工具箱，根据工具箱界面依次导入图片序列，然后进行标定，获取最终标定结果，我们需要标定的是相机的内参数αx,αy,u0,v0。
[0019] 步骤二：基于相邻图像之间的运动估计
[0020] 本步骤是利用相邻图像数据，来估计全局光流(表征图像中像素的整体运动情况)，分为两种情况，一种是图像平移量较大的情况，另一种是图像平移量较小的情况。具体操作如下：
[0021] S21、对于图像平移量较大的情况
[0022] 针对相邻帧图像I1,I2，直接利用OpenCv的相位相关函数phaseCorrelate来获取图像之间亚像素级精度的平移量Tu,Tv，即需要的全局光流值。其中采用createHanningWindow函数生成汉宁窗来去除图像的边界效应。
[0023] S22、对于图像平移量较小的情况
[0024] 本部分利用LK光流、迭代求精和图像金字塔原理，估计图像仿射运动模型参数，进而估计全局光流。
[0025] S221、给定两帧相邻图像，可以粗略估计出其LK光流，具体过程如下：
[0026] S2211、针对相邻帧图像I1,I2，分别计算当前图像每个像素(x,y)的灰度相对于时间、空间上的偏导数Iu(x,y),Iv(x,y),It(x,y)如下(为方便以下简写为Iu,Iv,It)：
[0027]
[0028] 其中符号 是卷积操作。
[0029] S2212、利用最小二乘法求解方程：
[0030] [Iu Iux Iuy Iv Ivx Ivy]a＝-It (3)
[0031] 其中a＝[a1 a2 a3 a4 a5 a6]T为待求解的仿射运动参数。
[0032] S2213、根据步骤S2212求解的运动参数，直接给出当前图像中每个像素的光流表达式：
[0033]
[0034] S222、为了进一步提高精度，设计迭代求精算法，根据步骤S221估计出的LK光流值对图像进行扭转操作(MATLAB中的warp函数)，然后再进一步求取LK光流，由粗到精，经过三次迭代后确定光流值。具体方法见图4所示。
[0035] S223、针对简单纹理环境，还增加了图像金字塔，来克服飞行器作快速大运动的情况所带来的跟踪困难，最终全局光流的计算方法见图5，具体如下：
[0036] S2231、针对相邻图像I1,I2，分别获取其三层图像金字塔。金字塔图像分辨率成倍数缩小，即金字塔最底层(第0层)分辨率是图像原始分辨率u_0，v_0；第一层是最底层分辨率的1/2，第二层是最底层分辨率的1/4，第三层是最底层分辨率的1/8。
[0037] S2232、针对I1,I2的第三层金字塔图像(分辨率30*40)，根据步骤S222的迭代求精得到光流估计值
[0038] S2233、利用所述的光流估计值 对图像I1的第二层金字塔图像(分辨率60*80)进行扭转操作，得到的扭转后图像与图像I2的第二层金字塔图像(分辨率60*80)进行迭代求精操作，得到光流估计值
[0039] S2234、利用所述的光流估计值 对图像I1的第一层金字塔图像(分辨率120*160)进行扭转操作，得到的扭转后图像与图像I2的第一层金字塔图像(分辨率120*
160)进行迭代求精操作，得到光流估计值
[0040] S2235、得到最终全局光流估计
[0041]
[0042] 步骤三：基于视觉信息的速度估计
[0043] 根据步骤S21在平移量大的情况下得到的全局光流以及步骤S22在平移量小的情况下得到的全局光流，假定pk-1,pk为相邻两帧像素点，其中当前帧像素点pk为中心像素坐标，上一帧对应像素点根据所述两个全局光流获得，即有
[0044]
[0045] 接着根据标定的相机内参数，得到对应的归一化坐标
[0046]
[0047] 最后，直接给出基于视觉信息的速度估计
[0048]
[0049] 其中， 为根据视觉直接得到的飞行器速度估计值， 为高度T
传感器的读数， 为三维单位矩阵，e3＝[0 0 1] ，δt＝0.05s为相邻图像时间间隔，为陀螺仪测得的三维角速度， 直接由公式(7)得出， 为
旋转矩阵，直接由多旋翼的欧拉角得出：
[0050]
[0051] 其中，θ,φ,ψ分别是多旋翼的俯仰角、滚转角和偏航角。
[0052] 步骤四：基于卡尔曼滤波的速度估计
[0053] S41、构建系统的状态变量、过程模型和观测模型。
[0054] 状态变量：
[0055]
[0056] 其中，vk为待测三维速度，zk为飞行器z轴方向的高度值，ba为三维加速度偏移。
[0057] 过程模型：
[0058] xk＝Axk-1+uk-1+wk (11)
[0059] 其中， 为系统转移矩阵， 为控制输入， 为系统噪声，表征系统模型的不确定性，它们有如下表达式
[0060]
[0061] 其中，ax,ay,az分别为加速度计读数。噪声wk假设为高斯白噪声，其噪声方差阵为为对角阵。
[0062] 观测模型：
[0063] zk＝Hxk+vk (13)
[0064] 其中，观测量 包括步骤三中通过视觉信息得到的水平速度以及高度传感器测得的高度。 为观测转移矩阵， 为观测噪声，表征观
测量的不确定性，假设vk为高斯白噪声，其噪声方差阵为 它们的表达式如下[0065]
[0066] S42、滤波初始化
[0067] 令状态初值为：
[0068] x0＝[vc dsonarcosθcosφ 03×1]T (15)
[0069] 其中，vc＝[vx vy vz]T为公式(8)给出的初始视觉速度，高度初值由高度传感器给出，其中dsonar为高度传感器读数，加速度偏移初值设为零。
[0070] 令状态估计误差协方差初值为对角矩阵：
[0071]
[0072] 令k＝0, P0|0＝P0。
[0073] S43、状态一步预测
[0074]
[0075] S44、误差协方差一步预测
[0076] Pk|k-1＝APk-1|k-1AT+Qk-1 (18)
[0077] S45、卡尔曼滤波增益更新
[0078] Kk＝Pk|k-1HT(HPk|k-1HT+Rk)-1 (19)
[0079] S46、状态更新校正
[0080]
[0081] S47、误差协方差更新校正
[0082] Pk|k＝(I7-KkH)Pk|k-1 (21)
[0083] S48、k＝k+1，返回步骤S43继续运行。
[0084] 至此，卡尔曼滤波器不断迭代递推，就可以实时估计出飞行器的速度。
[0085] 本发明提出的基于相位相关和光流法的多旋翼速度测量方法，是一种利用多旋翼机载的单目相机和其它传感器进行视觉测量的方法，不依靠额外增加设备，成本低廉，克服地面纹理简单而无法获取特征点的困难，算法计算量小，算法鲁棒性高等优点。
附图说明
[0086] 图1是相机针孔成像模型示意图；
[0087] 图2是本发明在线测速流程图；
[0088] 图3是相机标定所用的二维棋盘格靶标示意图；
[0089] 图4是迭代求精算法示意图；
[0090] 图5是基于相邻图像进行运动估计流程图
[0091] 图6a、b是本发明方法与Guidance参考方法的水平速度估计值对比图
[0092] 图中符号说明如下：
[0093] 图1中的符号说明：O表示摄像机光心,OI表示图像坐标系原点。(u,v)表示图像坐标系的坐标轴，(Xc,Yc,Zc)表示摄像机坐标系的坐标轴。pe(xe,ye,ze)表示三维点在摄像机坐标系下的坐标,pc(u,v)表示点pe(xe,ye,ze)在图像上的投影。
[0094] 图4中的符号说明：I1,I2表示相邻帧图像，LK表示直接由LK光流计算方法(公式(2)-(4))，WARP表示图像扭转操作，ui,vi,i＝1,2,3表示LK光流，Tu,Tv表示全局光流。
[0095] 图5中的符号说明：I1,I2表示相邻帧图像在不同层下的金字塔图像，W表示图像扭转操作， 表示每一层金字塔图像进行迭代求精运算后的光流，Tu,Tv表示全局光流。
具体实施方式
[0096] 下面结合附图和实施例，对本发明的技术方案做进一步的说明。
[0097] 一种基于相位相关和光流法的多旋翼速度测量方法，其实现步骤具体如下：
[0098] 步骤一：单目相机内参数离线标定
[0099] 在具体实现过程中，本发明利用二维棋盘格标定板(如图3)作为靶标，固定单目相机，在相机视场内多角度移动靶标，保证整个靶标尽量占满图片。这样就可以得到图片序列(至少13张)，接下来，直接利用MATLAB R2017a自带的相机标定工具箱，根据工具箱界面依次导入图片序列，然后进行标定，获取最终标定结果，我们需要标定的是相机的内参数αx,αy,u0,v0。
[0100] 步骤二：基于相邻图像之间的运动估计
[0101] 本步骤是利用相邻图像数据，来估计全局光流(表征图像中像素的整体运动情况)，分为两种情况，一种是图像平移量较大的情况，另一种是图像平移量较小的情况。具体操作如下：
[0102] S21、对于图像平移量较大的情况
[0103] 针对相邻帧图像I1,I2，直接利用OpenCv的相位相关函数phaseCorrelate来获取图像之间亚像素级精度的平移量Tu,Tv，即需要的全局光流值。其中采用createHanningWindow函数生成汉宁窗来去除图像的边界效应。
[0104] S22、对于图像平移量较小的情况
[0105] 本部分利用LK光流、迭代求精和图像金字塔原理，估计图像仿射运动模型参数，进而估计全局光流。
[0106] S221、给定两帧相邻图像，可以粗略估计出其LK光流，具体过程如下：
[0107] S2211、针对相邻帧图像I1,I2，分别计算当前图像每个像素(x,y)的灰度相对于时间、空间上的偏导数Iu(x,y),Iv(x,y),It(x,y)如下(为方便以下简写为Iu,Iv,It)：
[0108]
[0109] 其中符号 是卷积操作。
[0110] S2212、利用最小二乘法求解方程：
[0111] [Iu Iux Iuy Iv Ivx Ivy]a＝-It (3)
[0112] 其中a＝[a1 a2 a3 a4 a5 a6]T为待求解的仿射运动参数。
[0113] S2213、根据步骤S2212求解的运动参数，直接给出当前图像中每个像素的光流表达式：
[0114]
[0115] S222、为了进一步提高精度，设计迭代求精算法，根据步骤S221估计出的LK光流值对图像进行扭转操作(MATLAB中的warp函数)，然后再进一步求取LK光流(即利用公式(2)～公式(4))，由粗到精，经过三次迭代后确定光流值。具体方法见图4所示。
[0116] S223、针对简单纹理环境，还增加了图像金字塔，来克服飞行器作快速大运动的情况所带来的跟踪困难，最终全局光流的计算方法见图5，具体如下：
[0117] S2231、针对相邻图像I1,I2，分别获取其三层图像金字塔。金字塔图像分辨率成倍数缩小，即金字塔最底层(第0层)分辨率是图像原始分辨率u_0，v_0；第一层是最底层分辨率的1/2，第二层是最底层分辨率的1/4，第三层是最底层分辨率的1/8。
[0118] S2232、针对I1,I2的第三层金字塔图像(分辨率30*40)，根据步骤S222的迭代求精得到光流估计值
[0119] S2233、利用所述的光流估计值 对图像I1的第二层金字塔图像(分辨率60*80)进行扭转操作，得到的扭转后图像与图像I2的第二层金字塔图像(分辨率60*80)进行迭代求精操作，得到光流估计值
[0120] S2234、利用所述的光流估计值 对图像I1的第一层金字塔图像(分辨率120*160)进行扭转操作，得到的扭转后图像与图像I2的第一层金字塔图像(分辨率120*
160)进行迭代求精操作，得到光流估计值
[0121] S2235、得到最终全局光流估计
[0122]
[0123] 步骤三：基于视觉信息的速度估计
[0124] 根据步骤S21在平移量大的情况下得到的全局光流以及步骤S22在平移量小的情况下得到的全局光流，假定pk-1,pk为相邻两帧像素点，其中当前帧像素点pk为中心像素坐标，上一帧对应像素点根据所述两个全局光流获得，即有
[0125]
[0126] 接着根据标定的相机内参数，得到对应的归一化坐标
[0127]
[0128] 最后，直接给出基于视觉信息的速度估计
[0129]
[0130] 其中， 为根据视觉直接得到的飞行器速度估计值， 为高度传感器的读数， 为三维单位矩阵，e3＝[0 0 1]T，δt＝0.05s为相邻图像时间间隔，为陀螺仪测得的三维角速度， 直接由公式(7)得出， 为旋
转矩阵，直接由多旋翼的欧拉角得出：
[0131]
[0132] 其中，θ,φ,ψ分别是多旋翼的俯仰角、滚转角和偏航角。
[0133] 步骤四：基于卡尔曼滤波的速度估计
[0134] S41、构建系统的状态变量、过程模型和观测模型。
[0135] 状态变量：
[0136]
[0137] 其中，vk为待测三维速度，zk为飞行器z轴方向的高度值，ba为三维加速度偏移。
[0138] 过程模型：
[0139] xk＝Axk-1+uk-1+wk (11)
[0140] 其中， 为系统转移矩阵， 为控制输入， 为系统噪声，表征系统模型的不确定性，它们有如下表达式
[0141]
[0142] 其中，ax,ay,az分别为加速度计读数。噪声wk假设为高斯白噪声，其噪声方差阵为为对角阵。
[0143] 观测模型：
[0144] zk＝Hxk+vk (13)
[0145] 其中，观测量 包括步骤三中通过视觉信息得到的水平速度以及高度传感器测得的高度。 为观测转移矩阵， 为观测噪声，表征观
测量的不确定性，假设vk为高斯白噪声，其噪声方差阵为 它们的表达式如下[0146]
[0147] S42、滤波初始化
[0148] 令状态初值为：
[0149] x0＝[vc dsonarcosθcosφ03×1]T (15)
[0150] 其中，vc＝[vx vy vz]T为公式(8)给出的初始视觉速度，高度初值由高度传感器给出，其中dsonar为高度传感器读数，加速度偏移初值设为零。
[0151] 令状态估计误差协方差初值为对角矩阵：
[0152]
[0153] 令k＝0, P0|0＝P0。
[0154] S43、状态一步预测
[0155]
[0156] S44、误差协方差一步预测
[0157] Pk|k-1＝APk-1|k-1AT+Qk-1 (18)
[0158] S45、卡尔曼滤波增益更新
[0159] Kk＝Pk|k-1HT(HPk|k-1HT+Rk)-1 (19)
[0160] S46、状态更新校正
[0161]
[0162] S47、误差协方差更新校正
[0163] Pk|k＝(I7-KkH)Pk|k-1 (21)
[0164] S48、k＝k+1，返回步骤S43继续运行。
[0165] 至此，卡尔曼滤波器不断迭代递推，就可以实时估计出飞行器的速度。
[0166] 实施例
[0167] 本发明提出了一种基于相位相关和光流法的多旋翼速度测量方法，并以大疆公司的Matrix 100四轴旋翼飞行器以及Guidance视觉感知模块进行真实实验。
[0168] 首先我们需要进行相机内参数离线标定操作。由于大疆公司提供的Guidance视觉感知模块是五块双目相机和超声波传感模块，我们只需要安装一个下视相机，针对其中左侧相机取图，进行标定操作。我们采用的棋盘格靶标如图3所示，规格为6×9，每个格子的长度为29.5mm。Guidance相机的分辨率设置为320×240像素，标定得到的内参数为：
[0169] αx＝334.4204,αy＝333.4688,u0＝149.4593,v0＝114.9624
[0170] 接着，我们直接按照步骤二到步骤四的操作，对多旋翼飞行速度实时测量，并与Guidance内部直接提供的水平速度值进行对比，结果如图6a、b所示，可以看出，我们提出的测量方法能够准确地估计速度值。同时，我们控制四轴旋翼飞行器飞行一个回环，保证起飞点和降落点相同。我们利用本发明的方法估计的速度以及Guidance自身提供的水平速度，分别对它们积分，得到整个轨迹长度，然后得到起飞点与降落点的水平位置误差值，最终对比结果如表1，为水平位置估计误差值(单位：米)。结果表明，本发明方法能够较好地估计飞行速度值。
[0171]实验场景 本发明方法 Guidance参考方法
室内悬停 0.0744 0.2475
室内飞行 0.0036 0.1701
室外悬停 0.4351 0.2605
室外飞行 0.4351 0.5575
[0172] 表1。
