CN113110556A - 一种基于视觉传感器的无人机位置估计系统及估计方法 - Google Patents

Abstract

本发明公开了一种基于视觉传感器的无人机位置估计系统及估计方法，本发明的系统包括搭载在无人机上的MCU模块以及飞行控制模块；所述MCU模块分别连接摄像头、惯性测量单元、超声波测距传感器、GPS传感器、LCD显示屏、带电可擦可编程只读存储器；所述MCU模块通过通用异步收发传输器与所述飞行控制模块之间实现数据传输；所述MCU模块通过蓝牙与计算机上位机之间实现数据传输；所述惯性测量单元的安装角度平行于无人机的机体，所述摄像头和激光测距传感器的安装角度垂直于无人机的机体。本发明能在制作成本不增加的情况下，兼顾位姿估计的测量精度和计算速率。

Description

一种基于视觉传感器的无人机位置估计系统及估计方法

技术领域

本发明属于控制工程技术领域，特别是实现小型四旋翼无人机的高精度位姿估计，具体涉及一种基于视觉传感器的无人机位置估计系统及估计方法。

背景技术

无人机具有结构紧凑、价格低廉、体积小、质量轻的特点，有着广阔的军用和民用前景，是目前全球科技发展的热点话题。随着无人机的控制技术的发展，无人机的自主定位也就成为了无人机的关键技术之一。为了实现无人机的自主飞行，提高无人机完成任务的能力，自主定位对于无人机发展具有深远的意义。如何实现高精度自主定位，成为了无人机研究的热点话题。

全球定位系统(GPS)是一种基于卫星的全球定位技术，在无人机自主定位方面有着广泛的应用。GPS模块通过接收GPS卫星发送的信息，就能直接得到无人机的当前位置，可以实现无人机的绝对定位。尽管GPS定位技术应用广泛，但其缺陷也十分明显。在使用GPS定位的过程中，由于其更新频率和测量精度都较低，往往不能提供精确的位置信息，估计精度和应用范围就十分有限。因此，改进基于GPS的无人机定位方法，是无人机自主定位技术中非常重要问题。

近年来，基于视觉方案逐渐应用于的无人机领域。基于视觉的无人机定位方案，常常通过使用特征点匹配的方法识别场景之中的特征，实现无人机的定位。由于这种方法计算复杂，而无人机的负载有限，因此使用这种方法，不是安装价格高昂的机载运算单元，就是将图像传输至地面的计算机进行离线计算。在离线传输的过程中，由于无线传输技术的带宽有限，往往会导致较长时间的数据延迟，影响定位的实时性。中国发明专利CN201610634278.3公开了一种无人机定位系统，采用机载摄像头将图像数据传至地面站进行分析和运算，该方法在图像较为复杂的情况下，很难做到理想的实时估计。

以上的定位方案，往往受到传感器性能和计算机运算能力的限制。为了消除离线传输对定位的影响，往往采用价格高昂的微型计算设备，成本较高，使用范围十分有限。对于一个无人机定位系统而言。综合考虑无人机的负载能力、设备成本和定位精度的意义十分重大。

发明内容

为解决上述问题，本发明公开了一种基于视觉传感器的无人机位置估计系统及估计方法，其可靠性好，稳定性高，计算单元具有较高的资源利用率，能在制作成本不增加的情况下，兼顾位姿估计的测量精度和计算速率。

上述的目的通过以下的技术方案实现：

一种基于视觉传感器的无人机位置估计系统，包括搭载在无人机上的MCU模块以及飞行控制模块；所述MCU模块分别连接摄像头、惯性测量单元、超声波测距传感器、GPS传感器、LCD显示屏、带电可擦可编程只读存储器；所述MCU模块通过通用异步收发传输器与所述飞行控制模块之间实现数据传输；所述MCU模块通过蓝牙与计算机上位机之间实现数据传输；所述惯性测量单元的安装角度平行于无人机的机体，所述摄像头和激光测距传感器的安装角度垂直于无人机的机体。

用上述的基于视觉传感器的无人机位置估计系统进行无人机位置估计的方法，该方法包括如下步骤：

S1.更新传感器数据：

S2.MCU模块通过对摄像头采集的图片进行特征点检测和连续图像前后帧获取图像的稀疏光流场；

S3.MCU模块根据惯性测量单元测得的角度信息和测距模块测得的高度信息，对步骤S1中获得的稀疏光流场进行惯性系映射；

S4.去除图像的稀疏光流场在惯性系中对应向量的旋转运动量，得到平移运动量；

S5.根据平移运动量和GPS传感器得到的位置信息，使用卡尔曼滤波，实现精确的位置估计。

进一步地，步骤S1的具体方法是：在无人机飞行过程中，系统将以惯性测量单元—超声波测距传感器—摄像头—GPS传感器的顺序对每一个时刻的传感器数据进行查询，以保证使用当前使用的传感器数据时最新的；当传感器未更新时，系统则使用上一时刻的传感器数据，并对时间进行记录，得到每个传感器的采样时间。

进一步地，步骤S2的具体方法是：

S21.使用FAST角点检测方法检测当前帧图像的特征点，具体方法是：

选取方法为

其中J_i，J_p分别为第i个点和中心点p的灰度值。当中心点领域内与中心点灰度差大于阈值ε_p的点的数量N大于12个时，即选取为特征点；

S22.使用Lucas-Kanade方法计算当前帧图像每个特征点附近的光流向量，同一图像上所有特征点附近的对应光流向量构成稀疏光流场，具体方法是：

在特征点附近，利用前后两帧图片，根据u＝G^-1b求解特征点的位移u＝[u，v]^T，其中

其中P_x，P_y为中心点在图片上的坐标，ω_x、ω_y分别为x和y方向的积分窗口大小，一般选取为3-5个像素，、J_x、J_y、J_t分别为图片对x，y方向以及时间t的偏导数。

进一步地，步骤S4的具体方法是：

三维运动转换与位置平移量分解方法为，使用惯性测量单元IMU测得无人机的偏航角的角速度w_x，使用激光测距传感器测得无人机的高度Z_p ^c，根据

求得无人机位移在图像中的平移分量

其中X_p ^c，Y_p ^c为平移分量的横坐标和纵坐标，f为摄像头焦距，

为位移在图像中的旋转分量。

进一步地，步骤S5的具体方法是：

基于卡尔曼滤波器的图像运动量和GPS获取的位置信号滤波算法，消除运动累加导致的累计误差，具体估计方法为：

其中，

是基于k-1时刻估计都k时刻的位置，ΔT是采样时间，K_g为卡尔曼增益矩阵，W为一个常矩阵，其更新率为

其中P，Q，R为系统状态、测量噪声和过程噪声的协方差矩阵，(·)^k|k-1为基于k-1时刻对k时刻变量(·)的估计值，(·)^k|k为基于k时刻对k时刻变量(·)的估计值，(·)^k-1|k-1为基于k-1时刻对k-1时刻变量(·)的估计值，

分别为位置X和偏移量b的估计值，为了实现无偏估计，定义误差补偿量

对于偏移量b，其偏移矫正量更新率为

其中

l为迭代步长。

本发明的有益效果是：

1.本发明的硬件系统使用基于嵌入式的集成电路模块，可直接用于无人机实时位姿估计，不需要将数据传递回地面。其可靠性好，稳定性高，计算单元具有较高的资源利用率，能在制作成本不增加的情况下，兼顾位姿估计的测量精度和计算速率。

2.本发明使用基于视觉的无人机定位系统，能弥补GPS模块在精度和更新频率上的缺陷，得到性能更高的估计结果。不同于传统方案，本发明通过利用视觉传感器采样，对数字图像进行处理，得到无人机的位移信息；再通过使用卡尔曼滤波算法，与GPS模块的测量结果进行融合，降低视觉传感器方案引起的累计误差。

3.本发明的测量频率可以达到30HZ，并且通过进行图像的在线运算，实现了位置信息的实时估计，有效避免了传输延迟。

4.本发明使用卡尔曼滤波实现无人机位置信息的融合，可以有效降低速度累加过程中的累积误差，从而实现较高精度的位置估计，其精度可达5cm。

附图说明

图1是本发明的系统的硬件结构框图；

图2是本发明的传感器更新时序；

图3是本发明的传感器结构及算法逻辑框图。

图4是本发明的FAST特征检测的阈值对比点位置关系图。

图5是本发明的图像处理算法流程图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图1所示，本发明的一种基于视觉传感器的无人机位置估计系统，包括搭载在无人机上的MCU模块以及飞行控制模块；所述MCU模块分别连接摄像头、惯性测量单元、超声波测距传感器、GPS传感器、LCD显示屏、带电可擦可编程只读存储器；所述MCU模块通过通用异步收发传输器与所述飞行控制模块之间实现数据传输；所述MCU模块通过蓝牙与计算机上位机之间实现数据传输；所述惯性测量单元的安装角度平行于无人机的机体，所述摄像头和激光测距传感器的安装角度垂直于无人机的机体。本实施例中，使用STM32F407芯片作为MCU模块，搭载摄像头采用图像传感器MT9V022、惯性测量单元采用MPU6050、超声波测距传感器采用MB1043、GPS传感器采用BU-353S4，用于定位模块的测量和在线运算。LCD显示屏用于数据显示和EEPROM(带电可擦可编程只读存储器)存储标定信息；通过UART(通用异步收发传输器)与飞行控制模块相连接，并通过蓝牙连接PC计算机；

用上述的基于视觉传感器的无人机位置估计系统进行无人机位置估计的方法，该方法包括如下步骤：

S1.更新传感器数据：

S2.MCU模块通过对摄像头采集的图片进行特征点检测和连续图像前后帧获取图像的稀疏光流场；

S3.MCU模块根据惯性测量单元测得的角度信息和测距模块测得的高度信息，对步骤S1中获得的稀疏光流场进行惯性系映射；使用惯性测量单元中的陀螺仪测量飞行过程中机体坐标系下三个正交方向上的角速度，用作欧拉角过程估计；使用加速度计和磁力计测量惯性坐标系下的加速度和偏航角，用作欧拉角测量估计，通过使用卡尔曼滤波对两组角度信息进行融合，求得无人机的欧拉角，得到无人机的姿态信息。

S4.去除图像的稀疏光流场在惯性系中对应向量的旋转运动量，得到平移运动量；

S5.根据平移运动量和GPS传感器得到的位置信息，使用卡尔曼滤波，实现精确的位置估计。

进一步地，步骤S1的具体方法是：在无人机飞行过程中，系统将以惯性测量单元—超声波测距传感器—摄像头—GPS传感器的顺序对每一个时刻的传感器数据进行查询，以保证使用当前使用的传感器数据时最新的；当传感器未更新时，系统则使用上一时刻的传感器数据，并对时间进行记录，得到每个传感器的采样时间。

进一步地，步骤S2的具体方法是：

S21.使用FAST角点检测方法检测当前帧图像的特征点，具体方法是：

选取方法为

其中J_i，J_p分别为第i个点和中心点p的灰度值。当中心点领域内与中心点灰度差大于阈值ε_p的点的数量N大于12个时，即选取为特征点；

S22.使用Lucas-Kanade方法计算当前帧图像每个特征点附近的光流向量，同一图像上所有特征点附近的对应光流向量构成稀疏光流场，具体方法是：

在特征点附近，利用前后两帧图片，根据u＝G^-1b求解特征点的位移u＝[u，v]^T，其中

其中p_x，p_y为中心点在图片上的坐标，ω_x、ω_y分别为x和y方向的积分窗口大小，一般选取为3-5个像素，、J_x、J_y、J_t分别为图片对x，y方向以及时间t的偏导数。

进一步地，步骤S4的具体方法是：

三维运动转换与位置平移量分解方法为，使用惯性测量单元IMU测得无人机的偏航角的角速度w_x，使用激光测距传感器测得无人机的高度Z_p ^c，根据

求得无人机位移在图像中的平移分量

其中X_p ^c，Y_p ^c为平移分量的横坐标和纵坐标，f为摄像头焦距，

为位移在图像中的旋转分量。

进一步地，步骤S5的具体方法是：

基于卡尔曼滤波器的图像运动量和GPS获取的位置信号滤波算法，消除运动累加导致的累计误差，具体估计方法为：

其中，

是基于k-1时刻估计都k时刻的位置，ΔT是采样时间，K_g为卡尔曼增益矩阵，W为一个常矩阵，其更新率为

其中P，Q，R为系统状态、测量噪声和过程噪声的协方差矩阵，(·)^k|k-1为基于k-1时刻对k时刻变量(·)的估计值，(·)^k|k为基于k时刻对k时刻变量(·)的估计值，(·)^k-1|k-1为基于k-1时刻对k-1时刻变量(·)的估计值，

分别为位置X和偏移量b的估计值，为了实现无偏估计，定义误差补偿量

对于偏移量b，其偏移矫正量更新率为

其中

l为迭代步长。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。

Claims (6)

1.一种基于视觉传感器的无人机位置估计系统，其特征在于，包括搭载在无人机上的MCU模块以及飞行控制模块；所述MCU模块分别连接摄像头、惯性测量单元、超声波测距传感器、GPS传感器、LCD显示屏、带电可擦可编程只读存储器；所述MCU模块通过通用异步收发传输器与所述飞行控制模块之间实现数据传输；所述MCU模块通过蓝牙与计算机上位机之间实现数据传输；所述惯性测量单元的安装角度平行于无人机的机体，所述摄像头和激光测距传感器的安装角度垂直于无人机的机体。

2.一种用上述的基于视觉传感器的无人机位置估计系统进行无人机位置估计的方法，其特征在于，该方法包括如下步骤：

S1.更新传感器数据：

S2.MCU模块通过对摄像头采集的图片进行特征点检测和连续图像前后帧获取图像的稀疏光流场；

S3.MCU模块根据惯性测量单元测得的角度信息和测距模块测得的高度信息，对步骤S1中获得的稀疏光流场进行惯性系映射；

S4.去除图像的稀疏光流场在惯性系中对应向量的旋转运动量，得到平移运动量；

S5.根据平移运动量和GPS传感器得到的位置信息，使用卡尔曼滤波，实现精确的位置估计。

3.根据权利要求2所述的基于视觉传感器的无人机位置估计方法，其特征在于，步骤S1的具体方法是：在无人机飞行过程中，系统将以惯性测量单元—超声波测距传感器—摄像头—GPS传感器的顺序对每一个时刻的传感器数据进行查询，以保证使用当前使用的传感器数据时最新的；当传感器未更新时，系统则使用上一时刻的传感器数据，并对时间进行记录，得到每个传感器的采样时间。

4.根据权利要求2所述的基于视觉传感器的无人机位置估计方法，其特征在于，步骤S2的具体方法是：

S21.使用FAST角点检测方法检测当前帧图像的特征点，具体方法是：

选取方法为

其中J_i，J_p分别为第i个点和中心点p的灰度值。当中心点领域内与中心点灰度差大于阈值ε_p的点的数量N大于12个时，即选取为特征点；

S22.使用Lucas-Kanade方法计算当前帧图像每个特征点附近的光流向量，同一图像上所有特征点附近的对应光流向量构成稀疏光流场，具体方法是：

在特征点附近，利用前后两帧图片，根据u＝G^-1b求解特征点的位移u＝[u，v]^T，其中

其中p_x，p_y为中心点在图片上的坐标，ω_x、ω_y分别为x和y方向的积分窗口大小，一般选取为3-5个像素，J_x、J_y、J_t分别为图片对x，y方向以及时间t的偏导数。

5.根据权利要求2所述的基于视觉传感器的无人机位置估计方法，其特征在于，步骤S4的具体方法是：

三维运动转换与位置平移量分解方法为，使用惯性测量单元IMU测得无人机的偏航角的角速度w_x，使用激光测距传感器测得无人机的高度Z_p ^c，根据

求得无人机位移在图像中的平移分量

其中X_p ^c，Y_p ^c分别为平移分量的横坐标和纵坐标，f为摄像头焦距；

为位移在图像中的旋转分量。

6.根据权利要求2所述的基于视觉传感器的无人机位置估计方法，其特征在于，步骤S5的具体方法是：

基于卡尔曼滤波器的图像运动量和GPS获取的位置信号滤波算法，消除运动累加导致的累计误差，具体估计方法为：

其中，

是基于k-1时刻估计都k时刻的位置，ΔT是采样时间，K_g为卡尔曼增益矩阵，W为一个常矩阵，其更新率为

其中P，Q，R为系统状态、测量噪声和过程噪声的协方差矩阵，(·)^k|k-1为基于k-1时刻对k时刻变量(·)的估计值，(·)^k|k为基于k时刻对k时刻变量(·)的估计值，(·)^k-1|k-1为基于k-1时刻对k-1时刻变量(·)的估计值，

分别为位置X和偏移量b的估计值，为了实现无偏估计，定义误差补偿量

对于偏移量b，其偏移矫正量更新率为

其中

为迭代步长。

Images