CN110222581B - 一种基于双目相机的四旋翼无人机视觉目标跟踪方法 - Google Patents

Abstract

本发明公开了一种基于双目相机的四旋翼无人机视觉目标跟踪方法，通过目标识别算法检测出跟踪目标，再使用基于相关滤波的视觉追踪算法，完成对目标在像素意义上的位置追踪与尺度追踪，并根据追踪效果判断是否跟踪丢失需要启动重定位程序，或需要对长期跟踪器进行更新；然后，根据跟踪框选定的图像区域，使用LK光流法进行四旋翼无人机与追踪目标相对距离的计算，经过坐标转换后使用卡尔曼滤波器实现对追踪目标的全局状态估计；最后根据估计的目标全局位置、速度状态，设计无人机外环控制系统的状态量，实现无人机对追踪目标无延时的稳定跟踪。

Description

一种基于双目相机的四旋翼无人机视觉目标跟踪方法

技术领域

本发明属于四旋翼无人机技术领域，更为具体地讲，涉及一种基于双目相机的四旋翼无人机视觉目标跟踪方法。

背景技术

在四旋翼无人机技术领域，视觉目标追踪无人机具有很好的实用价值。在军事侦察领域，通过在无人机上使用目标识别与跟踪技术，可对打击目标进行持续锁定监视，估计目标的位置及运动状态信息，从而引导己方进行精确火力打击；航空摄影方面，小到消费级市场上普通消费者游玩时对于跟拍的巨大需求，大到行业应用市场中，影视拍摄时的运动特写镜头，这些功能以前都需要经验丰富的无人机操作员来完成，操作门槛高且成功率低。因此研究能够自主跟随进行特殊视角拍摄的无人机系统具有重要意义。

现有类似的无人机目标跟踪系统主要有两种实现方法，第一种要求跟踪目标携带GPS或其他可与无人机通信的电子信标，无人机通过接受信标的定位信息完成对目标的跟踪，这种方法在应用上显然具有很大的局限性；第二种方法则是利用视觉追踪算法对跟踪目标进行跟踪，但使用的追踪算法多为传统跟踪算法，跟踪能力差，不具备长期跟踪能力，此外，很多类似系统为了得到无人机与跟踪目标之间的相对距离及运动速度等状态信息，需要提前知道目标的形状大小，或者以无人机的飞行高度作为状态估计条件之一，这使得其只能跟踪已知目标或者地面运动的目标。这些情况影响了视觉目标跟踪无人机的实用性，应用场景具有很大的局限性。

基于此，本发明设计实现了一套实用的基于视觉的四旋翼无人机目标跟踪系统，该系统具有普适性，在复杂环境下，能够对目标进行识别与状态估计，实现四旋翼无人机对空中及地面目标在没有任何先验知识(如：形状大小)情况下平稳快速的追踪，并且在目标在视野范围内丢失时具有重新发现并定位目标的能力。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于双目相机的四旋翼无人机视觉目标跟踪方法，通过跟踪目标在双目相机中的成像视差以及无人机自身的定位信息对跟踪目标进行状态估计，进而控制无人机的飞行，实现无人机对跟踪目标平稳快速的跟随。

为实现上述发明目的，本发明一种基于双目相机的四旋翼无人机视觉目标跟踪方法，其特征在于，包括以下步骤：

(1)、选取跟踪目标

地面站计算机将左相机传回的图像进行实时显示，再通过地面站计算机上运行的目标检测算法在当前帧图像中检测并框出跟踪目标，显示出目标类别，然后用鼠标单击检测到的跟踪目标框区域，框选出跟踪目标；如果目标检测算法未检测出跟踪目标，则通过手动画出跟踪目标框，然后再用鼠标单击跟踪目标框选出跟踪目标；

(2)、训练滤波器并更新

(2.1)、在跟踪目标框以及周围区域，提取包含31维FHOG特征与1维的灰度特征，整个提取区域的大小的长宽为跟踪目标框长宽的p倍，整个提取区域记为S；

(2.2)、分别利用提取区域S训练位置追踪滤波器和外观滤波器；

(2.3)、在下一帧图像到来时，利用上一帧训练好的位置追踪滤波器寻找相关性响应最大的位置，记为P；然后以P为中心，利用尺度滤波器对目标框区域进行缩放，得到新一帧图像上的跟踪目标框；

(2.4)、在新一帧图像上计算平均峰值相关能量APCE值，然后比较APCE与预设的跟踪丢失阈值的大小，如果APCE值低于跟踪丢失阈值时，则判断为跟踪丢失，进入步骤(2.5)，否则，跳转至步骤(2.6)；

(2.5)、将四旋翼无人机切换到定点悬停模式，再使用SVM算法对跟踪目标进行重定位，直到重新确定跟踪目标，然后返回步骤(2.3)；

(2.6)、对位置追踪滤波器和尺度滤波器进行更新；同时判断APCE值是否大于外观滤波器的更新阈值，如果大于，则对外观滤波器进行更新，否则不更新外观滤波器；待三个滤波器处理完成后进入步骤(3)；

(3)、估计跟踪目标状态

(3.1)、提取跟踪目标框内的ORB特征点，并记录下这些特征点的位置；再使用带金字塔的LK光流法，寻找同一时刻右相机图像中对应的点，并计算每个点对应的视差值；

(3.2)、按照视差值对右相机图像中对应的点进行升序排序，再剔除视差大小处于前1/3与后1/3的点，将处于中间1/3的点的视差值取平均，作为视差结果；

(3.3)、通过双目测距原理和三角化方法，利用视差结果计算出跟踪目标的深度d以及跟踪目标在相机坐标系下的位置坐标值(x_c,y_c,z_c)；

(3.4)、将GPS数据、IMU数据和超声波数据通过四旋翼无人机自带的位姿估计程序处理，得到四旋翼无人机在地理坐标系下的位置(X_ch,Y_ch,Z_ch)和姿态(

θ，ψ)；

(3.5)、将d、x_c,y_c,z_c、(X_ch,Y_ch,Z_ch)和(

θ，ψ)输入至卡尔曼滤波算法，估计出跟踪目标在地理坐标系下的实际位置(x,y,z)和实际速度

(4)、控制四旋翼无人机对跟踪目标进行平行跟随飞行

(4.1)、根据实际位置(x,y,z)计算四旋翼无人机的实际位置与期望位置的差值，再将该差值作为外环全局位置PID控制器的输入，同时将实际速度

作为PID的控制量，那么，全局位置PID控制器的输出为：

其中，C_x,C_y,C_z分别为在x,y,z方向上外环全局位置PID控制器的输出，d_xexp,d_{y exp},d_{z exp}分别表示四旋翼无人机与跟踪目标在x,y,z方向上的期望距离，p_x,p_y,p_z分别表示在x,y,z方向上外环全局位置PID控制器的比例参数；

(4.2)、保持跟踪目标处在图像中心，将跟踪目标在图像中的中心位置的像素坐标与图像实际的中心位置的像素坐标的差值作为yaw角PID控制器的输入，同时对输出C_yaw＝p_yaw·(u_x-u_c)进行限幅，p_yaw表示yaw角PID控制器的比例参数，u_x表示跟踪目标框中心像素点的横向像素坐标值，u_c表示图像实际中心像素点的横向像素坐标；

(4.3)、根据外环全局位置PID控制器的输出(C_x,C_y,C_z)，计算出四旋翼无人机期望全局速度

根据YAW角控制器的输出C_yaw，计算出期望的YAW角速度

将期望全局速度和期望YAW角速度与实际的全局速度和实际YAW角速度的差值作为内环速度PID控制器的输入，通过内环速度PID调节后传递给姿态控制器，姿态控制器控制电机的转速，完成四旋翼无人机视觉目标跟踪。

本发明的发明目的是这样实现的：

本发明基于双目相机的四旋翼无人机视觉目标跟踪方法，通过目标识别算法检测出跟踪目标，再使用基于相关滤波的视觉追踪算法，完成对目标在像素意义上的位置追踪与尺度追踪，并根据追踪效果判断是否跟踪丢失需要启动重定位程序，或需要对长期跟踪器进行更新；然后，根据跟踪框选定的图像区域，使用LK光流法进行四旋翼无人机与追踪目标相对距离的计算，经过坐标转换后使用卡尔曼滤波器实现对追踪目标的全局状态估计；最后根据估计的目标全局位置、速度状态，设计无人机外环控制系统的状态量，实现无人机对追踪目标无延时的稳定跟踪。

同时，本发明基于双目相机的四旋翼无人机视觉目标跟踪方法还具有以下有益效果：

(1)、本发明使用目标检测算法辅助进行跟踪目标的选取，能识别常见感兴趣物体种类，对于移动的物体可以快捷准确的实现初始跟踪框的选取。

(2)、本发明使用的视觉追踪算法传统的相关滤波类算法基础上新增外观滤波器和SVM重定位策略，在保证目标追踪实时性的基础上增加了对目标追踪的精度和进行追踪目标重定位的能力，为追踪目标的状态估计提供了基本的保障。

(3)、在状态估计算法方面，计算视差采用LK光流法，计算速度快，同时剔除视差过大与过小的值，保证了视差计算的精度与稳定性；对追踪目标进行运动学模型和测量模型建模，使用扩展卡尔曼滤波器完成对追踪目标的位置滤波与速度估计，得到目标精度高、延时小的位置和速度的状态信息，为无人机的控制器提供了准确稳定的输入量。

(4)、在无人机的跟随策略方面，本发明采用平行跟随控制，除了使用目标的全局位置作为输入量外，考虑到无人机跟随目标的实时性，使用追踪目标的全局速度估计信息作为另一控制量，起到估计目标下一时刻位置的作用，大大减少了无人机追踪目标的滞后时间，提升无人机跟踪的快速性，使无人机能够完成无静差的追踪效果。

附图说明

图1是本发明基于双目相机的四旋翼无人机视觉目标跟踪方法流程图；

图2是状态估流程图；

图3是平行跟随控制器框图。

具体实施方式

下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。

实施例

为了方便描述，先对具体实施方式中出现的相关专业术语进行说明：

Offboard:外部模式；

Tiny-Yolov3：实时目标检测算法；

APCE(Average Peak-to Correlation Energy):一种跟踪质量判断指标；

SVM(Support Vector Machine):一种基于监督学习的非线性分类器算法；

ORB(Oriebted Fast and Rotated BRIEF)：一种快速特征点提取和描述的算法；

EKF(Extended Kalman Filter)：卡尔曼滤波算法；

图1是本发明基于双目相机的四旋翼无人机视觉目标跟踪方法流程图。

在本实施例中，如图1所示，本发明一种基于双目相机的四旋翼无人机视觉目标跟踪方法，包括以下步骤：

S1、选取跟踪目标

系统上电并初始化，初始化完成后，通过遥控器将四轴飞行器的控制模式切换到offborad模式，然后四轴飞行器从地面初始位置处起飞至竖直方向高度h＝2米处，并悬停于此，左相机以50Hz的频率向地面站平台传输图像；

地面站计算机将左相机传回的图像进行实时显示，地面站计算机上运行的Tiny-yolov3目标检测算法会将识别出的物体用矩形框框出，并显示目标类别，Tiny-yolov3可检测行人、车辆等常见感兴趣目标；然后用鼠标单击检测到的跟踪目标框区域，框选出跟踪目标；如果目标检测算法未检测出跟踪目标，则通过手动画出跟踪目标框，然后再用鼠标单击跟踪目标框选出跟踪目标；

S2、训练滤波器并更新

S2.1、在跟踪目标框以及周围区域，提取包含31维FHOG特征与1维的灰度特征，整个提取区域的大小的长宽为跟踪目标框长宽的p＝1.5倍，整个提取区域记为S；

S2.2、分别利用提取区域S训练位置追踪滤波器和外观滤波器；

S2.3、在下一帧图像到来时，利用上一帧训练好的位置追踪滤波器寻找相关性响应最大的位置，记为P；然后以P为中心，利用尺度滤波器对目标框区域进行缩放，尺度缩放因子为1.03，得到新一帧图像上的跟踪目标框；

S2.4、在新一帧图像上计算平均峰值相关能量APCE值，然后比较APCE与预设的跟踪丢失阈值的大小，在本实施例中，跟踪丢失阈值取0.35倍的正常跟踪APCE均值，如果APCE值低于跟踪丢失阈值时，则判断为跟踪丢失，进入步骤S2.5，否则，跳转至步骤S2.6；

S2.5、将四旋翼无人机切换到定点悬停模式，再使用SVM算法对跟踪目标进行重定位，直到重新确定跟踪目标，然后将无人机切换回offborad模式，返回步骤S2.3；

S2.6、对位置追踪滤波器和尺度滤波器进行更新；同时判断APCE值是否大于外观滤波器的更新阈值，如果大于，则对外观滤波器进行更新，否则不更新外观滤波器；待三个滤波器处理完成后进入步骤S3；

S3、如图2所示，估计跟踪目标状态

S3.1、提取跟踪目标框内的ORB特征点，并记录下这些特征点的位置；再使用带金字塔的LK光流法，寻找同一时刻右相机图像中对应的点，并计算每个点对应的视差值；

S3.2、按照视差值对右相机图像中对应的点进行升序排序，再剔除视差大小处于前1/3与后1/3的点，将处于中间1/3的点的视差值取平均，作为视差结果；

S3.3、通过双目测距原理和三角化方法，利用视差结果计算出跟踪目标的深度d以及跟踪目标在相机坐标系下的位置坐标值(x_c,y_c,z_c)；

S3.4、将GPS数据、IMU数据和超声波数据通过四旋翼无人机自带的位姿估计程序处理，得到四旋翼无人机在地理坐标系下的位置(X_ch,Y_ch,Z_ch)和姿态(

θ，ψ)；

S3.5、将d、x_c,y_c,z_c、(X_ch,Y_ch,Z_ch)和(

θ，ψ)输入至卡尔曼滤波算法，估计出跟踪目标在地理坐标系下的实际位置(x,y,z)和实际速度

在本实施例中，估计出跟踪目标在地理坐标系下的实际位置(x,y,z)和实际速度

的具体方法为：

1)、对跟踪目标进行运动学模型建模

跟踪目标在理想情况下以恒定速度运动，即跟踪目标速度为0，在x,y,z方向加速度均值a＝0，标准差为σ_a＝0.3的高斯模型，那么以t时刻跟踪目标的绝对位置(X_t,Y_t,Z_t)和速度

为状态量，建立跟踪目标的均匀加速的运动学模型；

其中，T表示当前帧与上一帧的时间差值，a为跟踪目标的加速度，ε(R_t)表示协方差矩阵为R_t的高斯噪声；

不同方向位置和速度相互独立，得到运动方程协方差矩阵R_t；

2)、对跟踪目标进行测量模型建模

以跟踪目标在相机坐标系下的位置(x_c,y_c,z_c)作为观测量，根据相机坐标系到机体坐标系的旋转矩阵R_bc、机体坐标系到地理坐标系的旋转矩阵R_wb，以及四旋翼无人机在地理坐标系下的位置(X_ch,Y_ch,Z_ch)，根据实际目标位置测量值的波动，设定目标在x,y,z方向的测量误差δ_x,δ_y,δ_z均为0.2m，并假设各个方向的测量值相互独立，建立观测量和状态量的测量模型；

其中，δ(Q_t)表示协方差矩阵的误差；协方差矩阵Q_t的具体形式为：

其中，δ_x,δ_y,δ_z分别表示通过LK光流法和三角化测得的在相机坐标系目标x,y,z方向的坐标值的波动幅度；

3)、卡尔曼滤波算法根据前一时刻状态和追踪目标的运动模型对状态量进行估计，并对状态量的协方差

进行预测，计算测量方程的协方差矩阵Q_t和卡尔曼增益K_t，然后根据卡尔曼增益K_t和测量值的大小，校正状态量估计值和协方差矩阵预测值，将校正后的值作为真实状态量值和真实协方差矩阵，得到追踪目标在地理坐标系下的位置(x,y,z)和速度信息

S4、如图3所示，控制四旋翼无人机对跟踪目标进行平行跟随飞行

S4.1、根据实际位置(x,y,z)计算四旋翼无人机的实际位置与期望位置的差值，再将该差值作为外环全局位置PID控制器的输入，同时将实际速度

作

作为PID的控制量，那么，全局位置PID控制器的输出为：

其中，C_x,C_y,C_z分别为在x,y,z方向上外环全局位置PID控制器的输出，d_xexp,d_{y exp},d_{z exp}分别表示四旋翼无人机与跟踪目标在x,y,z方向上的期望距离，p_x,p_y,p_z分别表示在x,y,z方向上外环全局位置PID控制器的比例参数；

S4.2、保持跟踪目标处在图像中心，将跟踪目标在图像中的中心位置的像素坐标与图像实际的中心位置的像素坐标的差值作为yaw角PID控制器的输入，同时对输出C_yaw＝p_yaw·(u_x-u_c)进行限幅，yaw角控制器的输出限幅值为±1，p_yaw表示yaw角PID控制器的比例参数，u_x表示跟踪目标框中心像素点的横向像素坐标值，u_c表示图像实际中心像素点的横向像素坐标；

S4.3、根据外环全局位置PID控制器的输出(C_x,C_y,C_z)，计算出四旋翼无人机期望全局速度

根据YAW角控制器的输出C_yaw，计算出期望的YAW角速度

将期望全局速度和期望YAW角速度与实际的全局速度和实际YAW角速度的差值作为内环速度PID控制器的输入，通过内环速度PID调节后传递给姿态控制器，姿态控制器控制电机的转速，完成四旋翼无人机视觉目标跟踪。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (3)

1.一种基于双目相机的四旋翼无人机视觉目标跟踪方法，其特征在于，包括以下步骤：

(1)、选取跟踪目标

地面站计算机将左相机传回的图像进行实时显示，再通过地面站计算机上运行的目标检测算法在当前帧图像中检测并框出跟踪目标，显示出目标类别，然后用鼠标单击检测到的跟踪目标框区域，框选出跟踪目标；如果目标检测算法未检测出跟踪目标，则通过手动画出跟踪目标框，然后再用鼠标单击跟踪目标框选出跟踪目标；

(2)、训练滤波器并更新

(2.1)、在跟踪目标框以及周围区域，提取包含31维FHOG特征与1维的灰度特征，整个提取区域的大小的长宽为跟踪目标框长宽的p倍，整个提取区域记为S；

(2.2)、分别利用提取区域S训练位置追踪滤波器和外观滤波器；

(2.3)、在下一帧图像到来时，利用上一帧训练好的位置追踪滤波器寻找相关性响应最大的位置，记为P；然后以P为中心，利用尺度滤波器对目标框区域进行缩放，得到新一帧图像上的跟踪跟踪目标框；

(2.4)、在新一帧图像上计算平均峰值相关能量APCE值，然后比较APCE与预设的跟踪丢失阈值的大小，如果APCE值低于跟踪丢失阈值时，则判断为跟踪丢失，进入步骤(2.5)，否则，跳转至步骤(2.6)；

(2.5)、将四旋翼无人机切换到定点悬停模式，再使用SVM算法对跟踪目标进行重定位，直到重新确定跟踪目标，然后返回步骤(2.3)；

(2.6)、对位置追踪滤波器和尺度滤波器进行更新；同时判断APCE值是否大于外观滤波器的更新阈值，如果大于，则对外观滤波器进行更新，否则不更新外观滤波器；待三个滤波器处理完成后进入步骤(3)；

(3)、估计跟踪目标状态

(3.1)、提取跟踪目标框内的ORB特征点，并记录下这些特征点的位置；再使用带金字塔的LK光流法，寻找同一时刻右相机图像中对应的点，并计算每个点对应的视差值；

(3.2)、按照视差值对右相机图像中对应的点进行升序排序，再剔除视差大小处于前1/3与后1/3的点，将处于中间1/3 的点的视差值取平均，作为视差结果；

(3.3)、通过双目测距原理和三角化方法，利用视差结果计算出跟踪目标的深度d以及跟踪目标在相机坐标系下的位置坐标值(x_c,y_c,z_c)；

(3.4)、将GPS数据、IMU数据和超声波数据通过四旋翼无人机自带的位姿估计程序处理，得到四旋翼无人机在地理坐标系下的位置(X_ch,Y_ch,Z_ch)和姿态

(3.5)、将d、x_c,y_c,z_c、(X_ch,Y_ch,Z_ch)和

输入至卡尔曼滤波算法，估计出跟踪目标在地理坐标系下的实际位置(x,y,z)和实际速度

(4)、控制四旋翼无人机对跟踪目标进行平行跟随飞行

(4.1)、根据实际位置(x,y,z)计算四旋翼无人机的实际位置与期望位置的差值，再将该差值作为外环全局位置PID控制器的输入，同时将实际速度

作为PID的控制量，那么，全局位置PID控制器的输出为：

其中，C_x,C_y,C_z分别为在x,y,z方向上外环全局位置PID控制器的输出，d_xexp,d_yexp, d_zexp分别表示四旋翼无人机与跟踪目标在x,y,z方向上的期望距离，p_x,p_y,p_z分别表示在x,y,z方向上外环全局位置PID控制器的比例参数；

(4.2)、保持跟踪目标处在图像中心，将跟踪目标在图像中的中心位置的像素坐标与图像实际的中心位置的像素坐标的差值作为yaw角PID控制器的输入，同时对输出C_yaw＝p_yaw·(u_x-u_c)进行限幅，p_yaw表示yaw角PID控制器的比例参数，u_x表示跟踪目标框中心像素点的横向像素坐标值，u_c表示图像实际中心像素点的横向像素坐标；

(4.3)、根据外环全局位置PID控制器的输出(C_x,C_y,C_z)，计算出四旋翼无人机期望全局速度

根据YAW角控制器的输出C_yaw，计算出期望的YAW角速度

将期望全局速度和期望YAW角速度与实际的全局速度和实际YAW角速度的差值作为内环速度PID控制器的输入，通过内环速度PID调节后传递给姿态控制器，姿态控制器控制电机的转速，完成四旋翼无人机视觉目标跟踪。

2.根据权利要求1所述的基于双目相机的四旋翼无人机视觉目标跟随方法，其特征在于，所述步骤(3.5)中，估计出跟踪目标在地理坐标系下的实际位置(x,y,z)和实际速度

的具体方法为：

1)、对跟踪目标进行运动学模型建模

跟踪目标在理想情况下以恒定速度运动，以t时刻跟踪目标的绝对位置(X_t,Y_t,Z_t)和速度

为状态量，建立跟踪目标的均匀加速的运动学模型；

其中，T表示当前帧与上一帧的时间差值，a为跟踪目标的加速度，ε(R_t)表示协方差矩阵为R_t的高斯噪声；

2)、对跟踪目标进行测量模型建模

以跟踪目标在相机坐标系下的位置(x_c,y_c,z_c)作为观测量，根据相机坐标系到机体坐标系的旋转矩阵R_bc、机体坐标系到地理坐标系的旋转矩阵R_wb，以及四旋翼无人机在地理坐标系下的位置(X_ch,Y_ch,Z_ch)，建立观测量和状态量的测量模型；

其中，δ(Q_t)表示协方差矩阵的误差；协方差矩阵Q_t的具体形式为：

其中，δ_x,δ_y,δ_z分别表示通过LK光流法和三角化测得的在相机坐标系目标x,y,z方向的坐标值的波动幅度；

3)、卡尔曼滤波算法根据前一时刻状态和追踪目标的运动模型对状态量进行估计，并对状态量的协方差

进行预测，计算测量方程的协方差矩阵Q_t和卡尔曼增益K_t，然后根据卡尔曼增益K_t和测量值的大小，校正状态量估计值和协方差矩阵预测值，将校正后的值作为真实状态量值和真实协方差矩阵，得到追踪目标在地理坐标系下的位置(x,y,z)和速度信息

3.根据权利要求2所述的基于双目相机的四旋翼无人机视觉目标跟随方法，其特征在于，所述的运动方程协方差矩阵R_t的计算方法为：

设跟踪目标各方向的位置只与同方向的速度相关，与其它方向的位置、速度相互独立，且跟踪目标的加速度为均值为a，标准差为σ_a的高斯模型，则运动方程协方差矩阵R_t为：

其中，T表示当前帧与上一帧的时间差值。

Images