CN113296139B - 一种自适应图像光流与rtk融合测姿方法 - Google Patents

Abstract

本发明公开一种自适应图像光流与RTK融合测姿方法，通过使用自适应卡尔曼滤波融合图像光流测姿数据和RTK测姿数据，有效地抑制图像光流测姿的误差累积，并且使用图像光流测姿***来辅助RTK测姿，增强了***的稳定性及姿态测量精度。通过使用对比度受限的直方图均衡处理方法、基于代价函数的野值剔除方法以及改进的光流运动方程，有效地提升***姿态测量的精度。

Description

一种自适应图像光流与RTK融合测姿方法

技术领域

本发明涉及测姿测向技术领域，具体涉及一种自适应图像光流与RTK融合测姿方法。

背景技术

对载体进行姿态测量是GNSS的重要技术应用之一，该方法可以实时提供载体的姿态信息，运用RTK姿态测量技术，可以求解出载体的航向角、横滚角和俯仰角。虽然RTK测姿***所产生的误差不会随时间积累，但是受限于实际观测环境影响，该方法测量精度较差。虽然图像光流姿态测量***具有很强的自主性，工作时不受外界干扰，但是图像光流姿态测量***采用的是相对测量的方法，存在着误差累积的问题，限制了该方法的运用。

发明内容

本发明所要解决的是传统RTK测姿***精度相比较于精密惯导***还存在一定差距，且不适用于卫星信号遮挡的场合，以及图像光流测姿***在运行过程中存在累积误差，需要实时矫正误差的问题，提出一种自适应图像光流与RTK融合测姿方法。

为解决上述问题，本发明是通过以下技术方案实现的：

一种自适应图像光流与RTK融合测姿方法，包括步骤如下：

步骤1、使用一个具有三个以上GNSS天线的RTK测姿***进行载体姿态解算，获取各个时刻载体的姿态角；其中姿态角包含载体的俯仰角p、横滚角r和航向角y；

步骤2、先从固定在载体上的摄像机获取各个时刻图像；再对各个时刻图像进行预处理后，利用SURF法得到各个时刻图像的特征点；后利用前后时刻图像的特征点的坐标做差得到位移量，并用位移量除以***采样时间得到各个时刻图像的特征点的光流值；

步骤3、在传统光流运动方程的基础上引入平动光流膨胀点，得到改进的光流运动方程；其中改进的光流运动方程为：

式中，ω_r为载体的横滚角速度，ω_p为载体的俯仰角速度，ω_y为载体的航向角速度，(u_E,v_E)为光流膨胀点在图像中的坐标，(u_n,v_n)为图像的第n个特征点的坐标，

为图像的第n个特征点的光流值，f为摄像机的焦距，n＝1,2,...,N，N为图像的特征点的数目；

步骤4、对各个时刻图像的特征点的光流值使用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度；其中姿态角速度包含载体的俯仰角速度ω_p、横滚角速度ω_r以及航向角速度ω_y；

步骤5、使用自适应卡尔曼滤波对步骤1所获得的各个时刻载体的姿态角和步骤4获取的各个时刻载体的姿态角速度进行融合估计，得到各个时刻载体的姿态角估计值。

上述步骤5的具体过程如下：

步骤5.1、构建自适应卡尔曼滤波的滤波模型；滤波模型各个时刻观测向量Y由该时刻载体的俯仰角p、横滚角r、航向角y、俯仰角速度ω_p、横滚角速度ω_r和航向角速度ω_y所组成；滤波模型各个时刻状态向量X由该时刻载体的俯仰角p、横滚角r、航向角y、俯仰角速度ω_p、横滚角速度ω_r、航向角速度ω_y、俯仰角加速度a_p，横滚角加速度a_r和航向角加速度a_y所组成；

步骤5.2、给定初始的俯仰角加速度a_p、初始的横滚角加速度a_r、初始的航向角加速度a_y、初始的观测向量Y₀、初始的状态向量X₀、初始的状态向量误差协方差矩阵P₀、初始的观测误差协方差矩阵R₀、卡尔曼滤波状态转移矩阵A、状态过程协方差矩阵Q和测量矩阵C；并令i＝1；

步骤5.3、根据第i-1时刻状态向量X_i-1和卡尔曼滤波状态转移矩阵A，计算第i时刻预测状态向量

步骤5.4、根据第i-1时刻状态向量误差协方差矩阵P_i-1、状态过程协方差矩阵Q和卡尔曼滤波状态转移矩阵A，计算第i时刻预测状态向量误差协方差矩阵P_i ^-：

P_i ^-＝AP_i-1A+Q

步骤5.5、判断当前时刻i是否达到设定的时刻数M：

如果当前时刻i未达到设定的时刻数M，则第i时刻观测误差协方差矩阵R_i沿用第i-1时刻观测误差协方差矩阵R_i-1，即令R_i＝R_i-1；

如果当前时刻i达到设定的时刻数M，则根据第i时刻的前M个时刻预测状态向量、第i时刻的前M个时刻观测向量、第i-1时刻状态向量误差协方差矩阵P_i-1和测量矩阵C，计算第i时刻观测误差协方差矩阵R_i：

其中，M为设定值，

为第i-j时刻状态向量，Y_i-j为第i-j时刻观测向量；

步骤5.6、根据第i时刻预测状态向量误差协方差矩阵P_i ^-、第i时刻观测误差协方差矩阵R_i和测量矩阵C，计算第i时刻卡尔曼滤波增益矩阵K_i：

K_i＝P_i ^-C^T(CP_i ^-C^T+R_i)^-1

步骤5.7、根据第i时刻预测状态向量

第i时刻观测向量Y_i、第i时刻卡尔曼滤波增益矩阵K_i和测量矩阵C，计算第i时刻状态向量X_i，并将第i时刻状态向量X_i作为姿态角估计值输出：

步骤5.8、利用第i时刻预测状态向量误差协方差矩阵P_i ^-、第i时刻卡尔曼滤波增益矩阵K_i、测量矩阵C和9×9的单位矩阵I_9×9，更新第i时刻状态向量误差协方差矩阵P_i；

P_i＝(I_9×9-K_iC)P_i ^-

步骤5.9、令i＝i+1，并返回步骤5.3。

上述步骤5.1中，初始的俯仰角加速度a_p、初始的横滚角加速度a_r和初始的航向角加速度a_y均为0。

上述步骤5.2中，

初始的状态向量误差协方差矩阵P₀为：

P₀＝I_9×9

初始的观测误差协方差矩阵R₀为：

卡尔曼滤波状态转移矩阵A为：

状态过程协方差矩阵Q为：

测量矩阵C为：

C＝[I_6×6 0_6×3]

其中，△t为***采样时间，I_9×9为9×9的单位矩阵，I_6×6为6×6的单位矩阵，I_3×3为3×3的单位矩阵，0_6×3为6×3的全0矩阵。

作为改进，上述步骤4之后，步骤5之前，还进一步包括如下步骤：

步骤①、基于步骤4所得到的光流膨胀点坐标和载体的姿态角速度，使用代价函数计算各个时刻图像的特征点的残差值J_n；

步骤②、将步骤①所得残差值J_n超过设定的残差值阈值的图像的特征点进行初步剔除；

步骤③、对各个时刻图像初步剔除剩下的特征点用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度；

步骤④、基于步骤③所得到的光流膨胀点坐标和载体的姿态角速度，使用代价函数重新计算各个时刻图像初步剔除剩下的特征点的残差值J_n；

步骤⑤、将步骤④所得残差值J_n较大的预定比例的图像初步剔除剩下的特征点进行再次剔除；

步骤⑥、对各个时刻图像再次剔除剩下的特征点用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度；

步骤⑦、利用步骤⑥所得的载体的姿态角速度替换步骤4所得的载体的姿态角速度。

上述代价函数为：

其中，J_n为图像的第n个特征点所对应的残差值，ω_r为载体的横滚角速度，ω_p为载体的俯仰角速度，ω_y为载体的航向角速度，(u_E,v_E)为光流膨胀点在图像中的坐标，(u_n,v_n)为图像的第n个特征点的坐标，

为图像的第n个特征点的光流值，f为摄像机的焦距，n＝1,2,3...为图像的特征点的数目。

上述步骤1中，对图像进行预处理的方法为图像高斯滤波和对比度受限的直方图均衡处理。

与现有技术相比，本发明通过使用自适应卡尔曼滤波融合图像光流测姿数据和RTK测姿数据，有效地抑制图像光流测姿的误差累积，并且使用图像光流测姿***来辅助RTK测姿，增强了***的稳定性及姿态测量精度。通过使用对比度受限的直方图均衡处理方法、基于代价函数的野值剔除方法以及改进的光流运动方程，有效地提升***姿态测量的精度。

附图说明

图1为一种自适应图像光流与RTK融合测姿方法的流程图。

图2是光流膨胀点示意图。

图3是基于代价函数的野值剔除的流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实例，对本发明进一步详细说明。

参见图1，一种自适应图像光流与RTK融合测姿方法，其具体包括步骤如下：

步骤1：使用一个具有三个以上GNSS天线的RTK测姿***进行载体姿态解算，获取各个时刻载体的姿态角，该姿态角包含载体的横滚角r、俯仰角p和航向角y。

步骤2：先从固定在载体上的摄像机获取各个时刻的图像；再对各个时刻的图像进行预处理后，利用SURF(Speed Up Robust Features，加速求解的鲁棒特征点)法得到各个时刻的图像的特征点；后利用前后时刻的图像的特征点的坐标做差得到位移量，并用位移量除以***采样时间得到各个时刻的图像的特征点的光流值。

摄像头固定安装在载体上，并实时采集载体前方的图像。采用的图像预处理方法为：图像高斯滤波和对比度受限的直方图均衡处理。通过高斯滤波消除图像中的噪声，并通过对比度受限的直方图均衡处理对图像的对比度进行调整，使得亮度可以更好地在直方图上分布，增强局部的对比度改善阴影部分的亮度、凸出图像纹理，有效地改善了图像光流的求解精度。

步骤3：在传统光流运动方程的基础上引入平动光流膨胀点，得到改进的光流运动方程。

通过相机成像原理可建立的表示所拍摄的图像与相机运动之间关系的传统光流运动方程为：

其中，ω_r为载体的横滚角速度，ω_p为载体的俯仰角速度，ω_y为载体的航向角速度，(v_x,v_y,v_z)为载体三轴的线速度；Z_n为第n个特征点距离摄像机光心的距离，(u_n,v_n)为图像的第n个特征点的坐标，

为图像的第n个特征点的光流值，f为摄像机的焦距，，n＝1,2,...,N，N为图像中使用SURF法所检测到的特征点的数目。

为了降低姿态估计难度、提高姿态估计精度，本发明在传统光流运动方程中引入了平动光流膨胀点实现对光流平动分量的简化，进而减少整体的未知数个数。假设载体只进行平移运动且没有任何的旋转运动，此时摄像机中的图像表现为以某一个光流值为0的点为中心向四周膨胀，这个点在图像中的坐标可表示为(u_E,v_E)，并称为光流膨胀点，流膨胀点如图2所示。

此时光流膨胀点坐标有以下关系：

将公式(2)带入传统的光流运动方程(1)即可获获得所述改进的光流运动方程为：

其中，ω_r为载体的横滚角速度，ω_p为载体的俯仰角速度，ω_y为载体的航向角速度，(u_E,v_E)为光流膨胀点在图像中的坐标，(u_n,v_n)为图像的第n个特征点的坐标，

为图像的第n个特征点的光流值，f为摄像机的焦距，n＝1,2,3...为图像的特征点的数目。

步骤4：对各个时刻图像的特征点的光流值使用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度；其中姿态角速度包含载体的俯仰角速度ω_p、横滚角速度ω_r以及航向角速度ω_y。

步骤5：对各个时刻图像的特征点使用基于代价函数的野值剔除法剔除野值，如图3所示。

因为图像的特征点的光流值中存在不可避免的误差会影响运动参数的估计，所以载体姿态信息初步估计后的残差值J_n不为0，且误差较大的光流值的残差值J_n会比误差小的光流值大，因此可以通过剔除残差值J_n较大的光流值来改善估计精度。

步骤5.1：基于步骤4所得到的光流膨胀点坐标和载体的姿态角速度，使用代价函数计算各个时刻图像的特征点的残差值J_n。

使用一个代价函数作为图像特征点的评判标准，代价函数由改进光流运动方程推导而来，其公式如下：

其中，J_n为图像的第n个特征点所对应的残差值，ω_r为载体的横滚角速度，ω_p为载体的俯仰角速度，ω_y为载体的航向角速度，(u_E,v_E)为光流膨胀点在图像中的坐标，(u_n,v_n)为图像的第n个特征点的坐标，

为图像的第n个特征点的光流值，f为摄像机的焦距，n＝1,2,3...为图像的特征点的数目。

步骤5.2：将步骤5.1所得残差值J_n超过设定的残差值阈值的图像的特征点进行初步剔除(示例中选择的残差值阈值为5)。

步骤5.3：对各个时刻图像初步剔除剩下的特征点用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度。

步骤5.4：基于步骤5.3所得到的光流膨胀点坐标和载体的姿态角速度，使用代价函数重新计算各个时刻图像初步剔除剩下的特征点的残差值J_n。

步骤5.5：将步骤5.4所得残差值J_n较大的预定比例的图像初步剔除剩下的特征点进行再次剔除(示例中选择的预定比例为10％)。

步骤6：对经过步骤5剔除野值后的各个时刻图像的特征点的光流值使用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度，该姿态角速度包含载体的俯仰角速度ω_p、横滚角速度ω_r以及航向角速度ω_y。

步骤7：使用自适应卡尔曼滤波对步骤1所获得的各个时刻载体的姿态角和步骤6获取的各个时刻载体的姿态角速度进行融合估计，得到各个时刻载体的姿态角估计值。

自适应卡尔曼滤波是指在利用***实时的测量数据进行卡尔曼滤波的同时，不断用滤波的结果去判断***自身的各项状态是否发生变化，并对自身滤波模型的参数和噪声特性进行实时的修正，以便减小卡尔曼滤波的模型误差、提高滤波结果的精度。在常规卡尔曼滤波中，表示滤波运动模型误差状态过程协方差矩阵Q和观测向量误差协方差矩阵R是固定的，在***自身运动模型出现偏差或***观测环境发生变化时会对最终滤波结果产生较大的影响。由于RTK测姿***容易因为楼房、树木对GNSS卫星信号的遮挡而影响测姿精度；对于图像光流测姿***中，其对载体角速度的测量结果也容易受到光照变化的影响。因此本发明使用了自适应卡尔曼滤波，利用***实时观测值和卡尔曼滤波预测值来实时修正观测向量误差协方差矩阵R。

令初始时刻为第0时刻，令当前时刻为第i时刻，令当前时刻的前一时刻为第i-1时刻，步骤7的具体过程如下：

步骤7.1：构建自适应卡尔曼滤波的滤波模型。

滤波模型各个时刻观测向量Y由步骤1所获取的和步骤6所获取的各个时刻载体的俯仰角p、横滚角r、航向角y、俯仰角速度ω_p、横滚角速度ω_r和航向角速度ω_y所组成。

Y＝[p r y ω_p ω_r ω_y]^T (5)

滤波模型各个时刻状态向量X由各个时刻载体的俯仰角p、横滚角r、航向角y、俯仰角速度ω_p、横滚角速度ω_r、航向角速度ω_y、俯仰角加速度a_p，横滚角加速度a_r和航向角加速度a_y所组成。

X＝[p r y ω_p ω_r ω_y a_p a_r a_y]^T (6)

步骤7.2：初始化：初始的俯仰角加速度a_p、初始的横滚角加速度a_r、初始的航向角加速度a_y、初始的观测向量Y₀、初始的状态向量X₀、初始的状态向量误差协方差矩阵P₀、初始的观测误差协方差矩阵R₀、卡尔曼滤波状态转移矩阵A、状态过程协方差矩阵Q和测量矩阵C。

由于初始的俯仰角加速度a_p、初始的横滚角加速度a_r和初始的航向角加速度a_y均为0。因此初始的观测向量Y₀和初始的状态向量X₀均可根据第0时刻的姿态角和姿态角速度能够确定。

初始的状态向量误差协方差矩阵P₀为：

P₀＝I_9×9 (7)

初始的观测误差协方差矩阵R₀为：

卡尔曼滤波状态转移矩阵A为：

状态过程协方差矩阵Q为：

测量矩阵C为：

C＝[I_6×6 0_6×3] (11)

其中，△t为***采样时间，I_9×9为9×9的单位矩阵，I_6×6为6×6的单位矩阵，I_3×3为3×3的单位矩阵，0_6×3为6×3的全0矩阵。

步骤7.3、根据第i-1时刻状态向量X_i-1和卡尔曼滤波状态转移矩阵A，计算第i时刻预测状态向量

步骤7.4、根据第i-1时刻状态向量误差协方差矩阵P_i-1、状态过程协方差矩阵Q和卡尔曼滤波状态转移矩阵A，计算第i时刻预测状态向量误差协方差矩阵P_i ^-：

P_i ^-＝AP_i-1A+Q

步骤7.5、判断当前时刻i是否达到设定的时刻数M：

如果当前时刻i未达到设定的时刻数M，则第i时刻观测误差协方差矩阵R_i沿用第i-1时刻观测误差协方差矩阵R_i-1，即令R_i＝R_i-1；

如果当前时刻i达到设定的时刻数M，则根据第i时刻的前M个时刻预测状态向量、第i时刻的前M个时刻观测向量、第i-1时刻状态向量误差协方差矩阵P_i-1和测量矩阵C，计算第i时刻观测误差协方差矩阵R_i：

其中，M为设定值(示例中选择的M的值为10)，

为第i-j时刻状态向量，Y_i-j为第i-j时刻观测向量。

步骤7.6、根据第i时刻预测状态向量误差协方差矩阵P_i ^-、第i时刻观测误差协方差矩阵R_i和测量矩阵C，计算第i时刻卡尔曼滤波增益矩阵K_i：

K_i＝P_i ^-C^T(CP_i ^-C^T+R_i)^-1

步骤7.7、根据第i时刻预测状态向量

第i时刻观测向量Y_i、第i时刻卡尔曼滤波增益矩阵K_i和测量矩阵C，计算第i时刻状态向量X_i，并将第i时刻状态向量X_i作为姿态角估计值输出：

步骤7.8、利用第i时刻预测状态向量误差协方差矩阵P_i ^-、第i时刻卡尔曼滤波增益矩阵K_i、测量矩阵C和9×9的单位矩阵I_9×9，更新第i时刻状态向量误差协方差矩阵P_i：

P_i＝(I_9×9-K_iC)P_i ^-

步骤7.9、令i＝i+1，并返回步骤7.3。

需要说明的是，尽管以上本发明所述的实施例是说明性的，但这并非是对本发明的限制，因此本发明并不局限于上述具体实施方式中。在不脱离本发明原理的情况下，凡是本领域技术人员在本发明的启示下获得的其它实施方式，均视为在本发明的保护之内。

Claims (6)

1.一种自适应图像光流与RTK融合测姿方法，其特征是，包括步骤如下：

步骤1、使用一个具有三个以上GNSS天线的RTK测姿***进行载体姿态解算，获取各个时刻载体的姿态角；其中姿态角包含载体的俯仰角p、横滚角r和航向角y；

步骤2、先从固定在载体上的摄像机获取各个时刻图像；再对各个时刻图像进行预处理后，利用SURF法得到各个时刻图像的特征点；后利用前后时刻图像的特征点的坐标做差得到位移量，并用位移量除以***采样时间得到各个时刻图像的特征点的光流值；

步骤3、在传统光流运动方程的基础上引入平动光流膨胀点，得到改进的光流运动方程；其中改进的光流运动方程为：

式中，ω_r为载体的横滚角速度，ω_p为载体的俯仰角速度，ω_y为载体的航向角速度，(u_E,v_E)为光流膨胀点在图像中的坐标，(u_n,v_n)为图像的第n个特征点的坐标，

为图像的第n个特征点的光流值，f为摄像机的焦距，n＝1,2,...,N，N为图像的特征点的数目；

步骤4、对各个时刻图像的特征点的光流值使用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度；其中姿态角速度包含载体的俯仰角速度ω_p、横滚角速度ω_r以及航向角速度ω_y；

步骤5、使用自适应卡尔曼滤波对步骤1所获得的各个时刻载体的姿态角和步骤4获取的各个时刻载体的姿态角速度进行融合估计，得到各个时刻载体的姿态角估计值；其具体过程如下：

步骤5.1、构建自适应卡尔曼滤波的滤波模型；滤波模型各个时刻观测向量Y由该时刻载体的俯仰角p、横滚角r、航向角y、俯仰角速度ω_p、横滚角速度ω_r和航向角速度ω_y所组成；滤波模型各个时刻状态向量X由该时刻载体的俯仰角p、横滚角r、航向角y、俯仰角速度ω_p、横滚角速度ω_r、航向角速度ω_y、俯仰角加速度a_p，横滚角加速度a_r和航向角加速度a_y所组成；

步骤5.2、给定初始的俯仰角加速度a_p、初始的横滚角加速度a_r、初始的航向角加速度a_y、初始的观测向量Y₀、初始的状态向量X₀、初始的状态向量误差协方差矩阵P₀、初始的观测误差协方差矩阵R₀、卡尔曼滤波状态转移矩阵A、状态过程协方差矩阵Q和测量矩阵C；并令i＝1；

步骤5.3、根据第i-1时刻状态向量X_i-1和卡尔曼滤波状态转移矩阵A，计算第i时刻预测状态向量

步骤5.4、根据第i-1时刻状态向量误差协方差矩阵P_i-1、状态过程协方差矩阵Q和卡尔曼滤波状态转移矩阵A，计算第i时刻预测状态向量误差协方差矩阵P_i ^-：

P_i ^-＝AP_i-1A+Q

步骤5.5、判断当前时刻i是否达到设定的时刻数M：

如果当前时刻i未达到设定的时刻数M，则第i时刻观测误差协方差矩阵R_i沿用第i-1时刻观测误差协方差矩阵R_i-1，即令R_i＝R_i-1；

如果当前时刻i达到设定的时刻数M，则根据第i时刻的前M个时刻预测状态向量、第i时刻的前M个时刻观测向量、第i-1时刻状态向量误差协方差矩阵P_i-1和测量矩阵C，计算第i时刻观测误差协方差矩阵R_i：

其中，M为设定值，

为第i-j时刻状态向量，Y_i-j为第i-j时刻观测向量；

步骤5.6、根据第i时刻预测状态向量误差协方差矩阵P_i ^-、第i时刻观测误差协方差矩阵R_i和测量矩阵C，计算第i时刻卡尔曼滤波增益矩阵K_i：

K_i＝P_i ^-C^T(CP_i ^-C^T+R_i)^-1

步骤5.7、根据第i时刻预测状态向量

第i时刻观测向量Y_i、第i时刻卡尔曼滤波增益矩阵K_i和测量矩阵C，计算第i时刻状态向量X_i，并将第i时刻状态向量X_i作为姿态角估计值输出：

步骤5.8、利用第i时刻预测状态向量误差协方差矩阵P_i ^-、第i时刻卡尔曼滤波增益矩阵K_i、测量矩阵C和9×9的单位矩阵I_9×9，更新第i时刻状态向量误差协方差矩阵P_i；

P_i＝(I_9×9-K_iC)P_i ^-

步骤5.9、令i＝i+1，并返回步骤5.3。

2.根据权利要求1所述的一种自适应图像光流与RTK融合测姿方法，其特征是，步骤5.1中，初始的俯仰角加速度a_p、初始的横滚角加速度a_r和初始的航向角加速度a_y均为0。

3.根据权利要求1所述的一种自适应图像光流与RTK融合测姿方法，其特征是，步骤5.2中，

初始的状态向量误差协方差矩阵P₀为：

P₀＝I_9×9

初始的观测误差协方差矩阵R₀为：

卡尔曼滤波状态转移矩阵A为：

状态过程协方差矩阵Q为：

测量矩阵C为：

C＝[I_6×6 0_6×3]

其中，Δt为***采样时间，I_9×9为9×9的单位矩阵，I_6×6为6×6的单位矩阵，I_3×3为3×3的单位矩阵，0_6×3为6×3的全0矩阵。

4.根据权利要求1所述的一种自适应图像光流与RTK融合测姿方法，其特征是，步骤4之后，步骤5之前，还进一步包括如下步骤：

步骤①、基于步骤4所得到的光流膨胀点坐标和载体的姿态角速度，使用代价函数计算各个时刻图像的特征点的残差值J_n；

步骤②、将步骤①所得残差值J_n超过设定的残差值阈值的图像的特征点进行初步剔除；

步骤③、对各个时刻图像初步剔除剩下的特征点用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度；

步骤④、基于步骤③所得到的光流膨胀点坐标和载体的姿态角速度，使用代价函数重新计算各个时刻图像初步剔除剩下的特征点的残差值J_n；

步骤⑤、将步骤④所得残差值J_n较大的预定比例的图像初步剔除剩下的特征点进行再次剔除；

步骤⑥、对各个时刻图像再次剔除剩下的特征点用改进的光流运动方程，并运用非线性最小二乘法求解各个时刻图像的光流膨胀点坐标和载体的姿态角速度；

步骤⑦、利用步骤⑥所得的载体的姿态角速度替换步骤4所得的载体的姿态角速度。

5.根据权利要求4所述的一种自适应图像光流与RTK融合测姿方法，其特征是，代价函数为：

其中，J_n为图像的第n个特征点所对应的残差值，ω_r为载体的横滚角速度，ω_p为载体的俯仰角速度，ω_y为载体的航向角速度，(u_E,v_E)为光流膨胀点在图像中的坐标，(u_n,v_n)为图像的第n个特征点的坐标，

为图像的第n个特征点的光流值，f为摄像机的焦距，n＝1,2,3...为图像的特征点的数目。

6.根据权利要求1所述的一种自适应图像光流与RTK融合测姿方法，其特征是，步骤1中，对图像进行预处理的方法为图像高斯滤波和对比度受限的直方图均衡处理。

Images