CN104102218A - 基于视觉伺服的感知与规避方法及*** - Google Patents

Abstract

本发明提供一种基于视觉伺服的感知与规避***，包括无人机、图像采集***、视觉伺服控制***和导航定位***，所述视觉伺服***包括视觉目标检测与跟踪模块、安全包络模块和视觉伺服控制器。本发明还提供一种基于所述感知与规避***的规避方法，本发明实现了对周围空域环境的快速感知，以及在不加装任何测距传感器且不需地面操作人员进行干预和操作的情况下，独立自主完成对空间飞行目标的规避机动；本发明载荷要求低、控制精度高、具有较高的智能性，能够提高无人机的空域飞行安全能力。

Description

基于视觉伺服的感知与规避方法及***

技术领域

本发明涉及无人机导航与控制领域，特别涉及一种基于视觉伺服的感知与规避方法及***。

背景技术

近来，随着军事应用与民用领域对无人机的需求日益强烈，加上我国民用空域领域的进一步开放，未来空域将呈现多种类型功能的无人机、有人机进行空域共享和集成的局面，空域将日趋密集。在此情况下，无人机感知与规避(Sense and Avoid，即SAA)能力将成为进入空域飞行、保障无人机飞行安全的先决条件。无人机感知与规避是指无人机利用机载传感器或地面监视***完成对空域飞行环境的监视和飞行目标的状态获取，对潜在碰撞威胁的目标进行规避路径规划，完成规避机动动作，从而保证无人机的航路飞行安全。

SAA技术是目前无人机技术领域亟待解决的关键技术难题。2013年，在美国FAA(Federal Aviation Administration)发布的无人机***空域集成路线图中，明确提出SAA能力是无人机进行国家空域飞行的必备能力。其主要功能分为：目标检测与跟踪、碰撞威胁估计、规避路径规划、规避机动。

无人机机载SAA传感器包括T-CAS，ADS-B、雷达、激光雷达、光电、红外传感器等。其中，光电传感器作为对非合作目标非常重要和有效感知与规避手段，特别是在轻小型无人机***载荷、任务、成本有限，无法搭载大型感知设备如雷达、激光雷达等情况下，基于光电传感器的视觉SAA***在质量、功耗、成本方面的优势使其更易在轻小型***集成和应用；视觉SAA被动式感知的特点适用于战场环境、隐身任务等特定任务场合；且与ADS-B、TCAS等相比，视觉SAA不依靠应答机制，不需要地面控制信息和卫星定位信息，即能够提供对非合作目标独立的感知与规避能力。因此，研究基于光电传感器的视觉SAA***具有重要意义。

发明内容

本发明的目的在于提供一种基于视觉伺服的感知与规避方法及***，以解决现有传感器体积大、功耗较大，不利于在轻小型无人机***中的集成应用的问题，并且本发明能够实现对周围空域环境的快速感知，通过角度安全包络建立视觉反馈控制模型，控制无人机完成对空域目标的感知与规避。

本发明提供一种基于视觉伺服的感知与规避***，包括：

无人机，所述无人机上设置自动驾驶仪，所述自动驾驶仪接收飞行控制指令进行飞行并完成无人机对障碍物的规避动作；

图像采集***，搭载在所述无人机上，用于采集无人机的飞行空域图像信息，采用多相机多视角集中式阵列构型，并通过相机接口实时传输图像信息；

视觉伺服控制***，设置在所述无人机上，包括视觉目标检测与跟踪模块、安全包络模块和视觉伺服控制器，视觉目标检测与跟踪模块接收图像采集***传输的图像信息并实现对图像中飞行目标的检测、定位和跟踪，安全包络模块根据获取的目标状态信息进行图像平面角度安全包络的生成，并基于该安全包络生成图像规避角度，以此角度输入至视觉伺服控制器，视觉伺服控制器输出规避机动角度，并将该角度通过串口传输至自动驾驶仪完成规避动作；

导航定位***，设置在所述无人机上，用于建立航路回归机制，导航定位***检测到无人机不在航路且不存在威胁时，进行航路回归，使无人机返回原始航路。

作为一种优选方式，本发明中所述视觉目标检测与跟踪模块采用多路高清图像处理平台DM8168。

本发明还提供一种基于视觉伺服的感知与规避***的规避方法，包括：

步骤一，通过无人机机载图像采集***实现对飞行视场角内的空域的完全覆盖，获取无人机的飞行空域图像信息；

步骤二，机载视觉伺服控制***中视觉目标检测与跟踪模块接收步骤一中图像信息并进行处理，完成对空域图像中飞行目标的检测、定位和跟踪；

步骤三，根据步骤二中获取的飞行目标状态信息，视觉伺服控制***中安全包络模块进行图像平面角度安全包络生成，通过安全包络确定无人机是否存在碰撞威胁，针对碰撞威胁，计算得到图像意义下的规避角度；

步骤四，将步骤三中的规避角度输入视觉伺服控制***中的视觉伺服器，视觉伺服器输出需要的规避机动俯仰和偏航角度，并传送至无人机自动驾驶仪完成规避机动；

步骤五，利用导航定位***建立航路回归机制，当无人机不存在威胁时，使无人机返回原始航路。

作为一种优选方式，所述步骤二的具体实现方法如下：

步骤一中图像采集***获取的图像数据通过相机数据线传送至机载视觉伺服控制***中视觉目标检测与跟踪模块，其中图像采集***中共设置A、B、C三台相机，视觉目标检测与跟踪模块采用多路高清图像处理平台DM8168，利用DM8168实现对获取的多路视频进行处理，通过检测算法获取目标k时刻在图像采集***中某一相机图像平面的位置信息p_k＝[p_xk,p_yk]，获得目标相对于该相机的相对角度：

${\mathrm{\σ}}^{\′}={\tan }^{-1}\frac{p_{\mathrm{xk}}-\frac{w}{2}}{f/\mathrm{\μ}}$

${\mathrm{\γ}}^{\′}={\tan }^{-1}\frac{p_{\mathrm{yk}}-\frac{h}{2}}{f/\mathrm{\μ}}$

其中，w和h分别是以像素为单位的图像的宽度和高度，f为相机镜头焦距，μ为象元尺寸；

根据相机安装位置，可获得目标k时刻相对于无人机的相对角度为Θ_k＝[σ_k,γ_k]，其中

${\mathrm{\σ}}_k=\left\{\begin{array}{c}{\mathrm{\σ}}^{\′}-\frac{4}{9}\mathrm{\π},p_k\⋐A\\ {\mathrm{\σ}}^{\′},p_k\⋐B\\ {\mathrm{\σ}}^{\′}+\frac{4}{9}\mathrm{\π},p_k\⋐C\end{array}\right.$

γ_k＝γ′

式中分别表示检测到的目标来自于图像采集***中的相机A、相机B和相机C；

根据前后时刻的角度变化确定目标的角速度Ω_k＝[ω_xk,ω_yk]＝Θ_k+1-Θ_k,k>0。

作为另一种优选方式，所述步骤三中安全包络的生成步骤及规避角度的计算步骤包括：

①建立目标安全包络圆，圆心为目标点，选取一个半径初始值，并以固定速度进行增长；

②根据目标尺寸建立安全包络椭圆，椭圆圆心为目标点，椭圆短轴的长度与包络圆的半径相等，椭圆长轴的长度为短轴的长度与目标在当前角速度下在预留时间内的外推长度之和，长轴方向平行于目标的运动方向；

③前述生成的包络圆和包络椭圆形成平面角度安全包络，该安全包络由以椭圆短轴为分割线的沿目标运动方向的半椭圆与沿目标运动反方向的半圆组成；

④基于前述平面角度安全包络进行目标威胁估计，建立基于安全包络的威胁标示函数，以此判断是否存在碰撞威胁；

⑤根据最小规避距离原则，在平面角度安全包络寻找距离初始目标最近的点。

特别地，所述步骤三中安全包络的具体生成步骤及规避角度的具体计算步骤如下：

①建立目标安全包络圆，圆心选为目标k时刻相对于无人机的相对角度位置Θ_k=[σ_k，γ_k]，选取一个半径初始值r₀，并以固定速度进行增长，则

r_k+1＝r_k+ε,k＝1,2,3…

r_k＝r₀,k＝0

其中ε是固定增长速度，r_k+1代表k+1时刻的安全包络圆半径；

②根据目标尺寸建立安全包络椭圆，椭圆圆心为目标点Θ_k＝[σ_k,γ_k]，长轴方向平行于目标的运动方向，即目标角速度Ω_k的方向，长轴和短轴的长度分别按下列公式选取：

a＝r_k(1+α‖Ω_k‖)，

b＝r_k，

式中，a和b分别代表包络椭圆的长轴和短轴，r_k为k时刻包络圆的半径，α为角速度转化为轴长的比例因子，该因子代表规避预留时间，‖Ω_k‖为目标运动角速度向量的模；

③前述生成的包络圆和包络椭圆形成平面角度安全包络，该安全包络由以椭圆短轴为分割线的沿目标运动方向的半椭圆与沿目标运动反方向的半圆组成；

④基于前述平面角度安全包络进行目标威胁估计，建立基于时间k的威胁标示函数l_k，当l_k<0时，认为存在碰撞威胁，以此判断是否存在碰撞威胁，其中

$l_k=\left\{\begin{array}{cc}\mathrm{sgn}({(\widetilde{\mathrm{\&sigma;}}-{\mathrm{\&sigma;}}_k)}^2+{(\widetilde{\mathrm{\&gamma;}}-{\mathrm{\&gamma;}}_k)}^2-r_k)& (\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k<0\\ \mathrm{sgn}(\frac{{(\widetilde{\mathrm{\&sigma;}}-{\mathrm{\&sigma;}}_k)}^2}{a}+\frac{{(\widetilde{\mathrm{\&gamma;}}-{\mathrm{\&gamma;}}_k)}^2}{b}-1)& (\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k\&GreaterEqual;0\end{array}\right.,$

其中表示相机B的主轴方向相对于无人机主轴方向的角度，通常可认为是符号函数；

⑤当l_k<0时，根据最小规避距离原则，在平面角度安全包络寻找距离最近的点s^*＝[σ^*,γ^*]作为最小规避机动点：

$s^{*}=\left\{\begin{array}{c}{\mathrm{\&Theta;}}_k+\frac{(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)}{\left|\right|\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k\left|\right|}{r}_k,(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k<0\\ {\mathrm{\&Theta;}}_k\&PlusMinus;e^{\&perp;{\mathrm{\&Omega;}}_k}{r}_k(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k\&GreaterEqual;0\end{array}\right.,$

其中代表k时刻垂直于目标角速度向量的单位向量。

作为又一种优选方式，所述步骤四中规避机动俯仰和偏航角度的具体计算步骤如下：

①定义交互矩阵为

$L_e=\left[\begin{array}{cccccc}\frac{-\cos {\mathrm{\&sigma;}}_k\cos {\mathrm{\&gamma;}}_k}{d}& \frac{-\cos {\mathrm{\&sigma;}}_k\sin {\mathrm{\&gamma;}}_k}{d}& \sin {\mathrm{\&sigma;}}_k& \sin {\mathrm{\&gamma;}}_k& -\cos {\mathrm{\&gamma;}}_k& 0\\ \frac{\sin {\mathrm{\&gamma;}}_k}{d\sin {\mathrm{\&sigma;}}_k}& \frac{-\cos {\mathrm{\&gamma;}}_k}{d\sin {\mathrm{\&sigma;}}_k}& 0& \frac{\cos {\mathrm{\&gamma;}}_k\cos {\mathrm{\&sigma;}}_k}{\sin {\mathrm{\&sigma;}}_k}& \frac{\sin {\mathrm{\&gamma;}}_k\cos {\mathrm{\&sigma;}}_k}{\sin {\mathrm{\&sigma;}}_k}& -1\end{array}\right],$

在视觉伺服控制***的视觉伺服控制器中建立速度控制器，反馈量为完成误差的指数衰减，输出速度控制量V，视觉伺服控制器表达为

$V=[v,w]=-\mathrm{\&lambda;}{L}_e^{+}e,$

式中，λ为误差指数衰减系数，表示L_e的伪逆；

由于在交互矩阵L_e中无法获取目标与本机的距离d的信息，将交互矩阵进行分割如下：

$L_e=[\frac{1}{d}{L}_t,L_{\mathrm{\&omega;}}],$

式中，L_ω表示L_e中的角度控制分量，L_t表示速度运动控制分量；

得到距离无关的视觉伺服控制器：

$\mathrm{\&omega;}=-\mathrm{\&lambda;}{L}_{\mathrm{\&omega;}}^{+}e,$

式中，表示L_ω的伪逆；

为消除稳态误差，对视觉伺服控制器进行补偿，如下：

$\mathrm{\&omega;}=-\mathrm{\&lambda;}{L}_{\mathrm{\&omega;}}^{+}e+L_{\mathrm{\&omega;}}^{+}(\mathrm{\&Omega;}+\frac{s^{*}}{\left|\right|s^{*}\left|\right|}\mathrm{\&epsiv;})=\left[\begin{array}{ccc}{\mathrm{\&omega;}}_x& {\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_z\end{array}\right],$

式中，Ω表示目标运动的角速度，表示误差的方向向量，ω_x、ω_y、ω_z分别表示ω的三个分量；

②输出的俯仰角和偏航角为

${\mathrm{\&phi;}}_c\left(k\right)=\mathrm{\&phi;}\left(k\right)+\tan \frac{V\left(k\right){\mathrm{\&omega;}}_z\left(k\right)}{g}$

式中，和φ_c(k)表示自动驾驶仪输出姿态的俯仰角和偏航角，和φ(k)表示无人机当前姿态的俯仰角和偏航角，Δt表示视觉伺服控制器输出时间间隔步长，ω_y(k)和ω_z(k)分别表示当前时刻视觉伺服控制器输出的俯仰角速度控制量和偏航角速度控制量，g表示重力加速度，V(k)表示当前时刻无人机的速度。

作为再一种优选方式，所述步骤五中航路回归机制的具体建立步骤如下：

①通过本机导航定位***确定无人机是否在航路中，当本机不在航路时，进行航路回归；

②确认时刻k的回归航路角度

${\mathrm{\&gamma;}}_d^{*}={\cos }^{-1}\frac{(P_{d12}\left(k\right)-P_{s12})\&CenterDot;e_{s12}}{\left|\right|P_{d12}-P_{s12}\left|\right|}+\frac{\mathrm{\&pi;}}{2},$

${\mathrm{\&sigma;}}_d^{*}={\cos }^{-1}\frac{(P_{d13}-P_{s13})\&CenterDot;e_{s13}}{\left|\right|P_{d13}-P_{s13}\left|\right|}+\frac{3\mathrm{\&pi;}}{4},$

式中，代表了航路回归时的最小机动规避点位置；

其中P_d是当不存在规避机动时，本机在原本航路上的位置，P_d12和P_d13分别是本机在XY和XZ平面的二维位置；

P_s是通过本机导航定位***获得的本机当前位置,P_s12和P_s13分别是本机在XY和XZ平面的二维位置；

e_s是通过本机导航定位***获得的本机方向向量，e_s12和e_s13分别是本机在XY和XZ平面的二维方向向量。

本发明实现了对周围空域环境的快速感知，以及在不加装任何测距传感器且不需地面操作人员进行干预和操作的情况下，独立自主完成对空间飞行目标的规避机动；本发明载荷要求低、控制精度高、具有较高的智能性，能够提高无人机的空域飞行安全能力。

附图说明

图1为本发明的基于视觉伺服的感知与规避***结构框图；

图2为本发明的基于视觉伺服的感知与规避***的规避方法流程图；

图3a为本发明的图像采集***相机阵列布置图；

图3b为本发明的图像采集***相机阵列视场角度示意图；

图4为本发明的图像平面角度安全包络示意图。

具体实施方式

下面结合附图和实施例对本发明做进一步说明。

如图1所示，本发明提供一种基于视觉伺服的感知与规避***，包括：

无人机，所述无人机上设置自动驾驶仪，所述自动驾驶仪接收飞行控制指令进行飞行并完成无人机对障碍物的规避动作；

图像采集***，搭载在所述无人机上，用于采集无人机的飞行空域图像信息，采用多相机多视角集中式阵列构型，并通过相机接口实时传输图像信息；

视觉伺服控制***，设置在所述无人机上，包括视觉目标检测与跟踪模块、安全包络模块和视觉伺服控制器，视觉目标检测与跟踪模块接收图像采集***传输的图像信息并实现对图像中飞行目标的检测、定位和跟踪，安全包络模块根据获取的目标状态信息进行图像平面角度安全包络的生成，并基于该安全包络生成图像规避角度，以此角度输入至视觉伺服控制器，视觉伺服控制器输出规避机动角度，并将该角度通过串口传输至自动驾驶仪完成规避动作；

导航定位***，设置在所述无人机上，用于建立航路回归机制，导航定位***检测到无人机不在航路且不存在威胁时，进行航路回归，使无人机返回原始航路。

其中，本发明中视觉目标检测与跟踪模块采用多路高清图像处理平台DM8168。

如图2所示，本发明还提供一种基于视觉伺服的感知与规避***的规避方法，包括：

步骤一，通过无人机机载图像采集***实现对飞行视场角内的空域的完全覆盖，获取无人机的飞行空域图像信息。

根据国际民航组织规定，飞行器空间的视场角必须分别满足：水平视场>±110°，垂直视场>±15°，因此必须对上述空域能够进行完全覆盖，且应该尽可能的获取清晰准确的信息。图像采集***采用多相机多视角集中式阵列构型，从相机的角度来讲，即必须满足大市场和远距离高分辨成像，而单一的相机镜头很难在视场角和焦距方面兼顾。因此，采用如图3a所示的多相机集中式阵列构型，图中A、B、C均为设置在增稳云台上的相机，增稳云台能够保证清晰、水平视角图像的获取。利用多个长焦镜头的焦距拼接获得大的视场角度，具体视场角度如图3b所示，同时提高对远距离目标的探测能力。

步骤二，机载视觉伺服控制***中视觉目标检测与跟踪模块接收步骤一中图像信息并进行处理，完成对空域图像中飞行目标的检测、定位和跟踪。

在轻小型无人机中，由于通信带宽的限制，往往不能实现对高清图像的实时传播，多路高清视频的远距离传输在当前的轻小型无人机***中很难实现。

图像采集***获取的图像数据通过相机数据线传送至机载视觉伺服控制***中多路高清图像处理平台DM8168，利用DM8168实现对获取的多路视频进行处理，通过检测算法获取目标k时刻在图像采集***中某一相机图像平面的位置信息p_k＝[p_xk,p_yk]，获得目标相对于该相机的相对角度：

${\mathrm{\&sigma;}}^{\&prime;}={\tan }^{-1}\frac{p_{\mathrm{xk}}-\frac{w}{2}}{f/\mathrm{\&mu;}}$ ${\mathrm{\&gamma;}}^{\&prime;}={\tan }^{-1}\frac{p_{\mathrm{yk}}-\frac{h}{2}}{f/\mathrm{\&mu;}}$

其中，w和h分别是以像素为单位的图像的宽度和高度，f为相机镜头焦距，μ为象元尺寸；

根据相机安装位置，可获得目标k时刻相对于无人机的相对角度为Θ_k＝[σ_k,γ_k]，其中

${\mathrm{\&sigma;}}_k=\left\{\begin{array}{c}{\mathrm{\&sigma;}}^{\&prime;}-\frac{4}{9}\mathrm{\&pi;},p_k\&Subset;A\\ {\mathrm{\&sigma;}}^{\&prime;},p_k\&Subset;B\\ {\mathrm{\&sigma;}}^{\&prime;}+\frac{4}{9}\mathrm{\&pi;},p_k\&Subset;C\end{array}\right.$

γ_k＝γ′

式中分别表示检测到的目标来自于图像采集***中的相机A、相机B和相机C。

根据前后时刻的角度变化确定目标的角速度Ω_k＝[ω_xk,ω_yk]＝Θ_k+1-Θ_k,k>0。

步骤三，根据步骤二中获取的飞行目标状态信息，视觉伺服控制***中安全包络模块进行图像平面角度安全包络生成，通过安全包络确定无人机是否存在碰撞威胁，针对碰撞威胁，计算得到图像意义下的规避角度s^*＝[σ^*,γ^*]。

安全包络定义为表示飞行器与目标最小分离距离的光滑、封闭曲线或曲面，在视觉规避***中，由于距离无法获取，因此无法根据目标的真实三维空间建立安全包络，如图4所示，本发明中建立一种基于角度的安全包络方法，包括如下步骤：

①建立目标安全包络圆，圆心为目标点，选取一个半径初始值，并以固定速度进行增长；

②根据目标尺寸建立安全包络椭圆，椭圆圆心为目标点，椭圆短轴的长度与包络圆的半径相等，椭圆长轴的长度为短轴的长度与目标在当前角速度下在预留时间内的外推长度之和，长轴方向平行于目标的运动方向；

③前述生成的包络圆和包络椭圆形成平面角度安全包络，该安全包络由以椭圆短轴为分割线的沿目标运动方向的半椭圆与沿目标运动反方向的半圆组成；

④基于前述平面角度安全包络进行目标威胁估计，建立基于安全包络的威胁标示函数，以此判断是否存在碰撞威胁；

⑤根据最小规避距离原则，在平面角度安全包络寻找距离初始目标最近的点。

上述建立基于角度的安全包络方法可依据如下具体步骤进行：

①建立目标安全包络圆，圆心选为目标k时刻相对于无人机的相对角度位置Θ_k＝[σ_k,γ_k]，选取一个半径初始值r₀，并以固定速度进行增长，则

r_k+1＝r_k+ε,k＝1,2,3…

r_k＝r₀,k＝0

其中ε是固定增长速度，用来模拟三维空间物体在图像平面的扩张速度，r_k+1代表k+1时刻的安全包络圆半径，图4中的r表示包络圆的半径；

②根据目标尺寸建立安全包络椭圆，椭圆圆心为目标点Θ_k＝[σ_k,γ_k]，长轴方向平行于目标的运动方向，即目标角速度Ω_k的方向，图4中的a和b分别表示包络椭圆的长轴和短轴，长轴和短轴的长度分别按下列公式选取：

a＝r_k(1+α‖Ω_k‖)，

b＝r_k，

式中，a和b分别代表包络椭圆的长轴和短轴，r_k为k时刻包络圆的半径，α为角速度转化为轴长的比例因子，该因子代表规避预留时间，‖Ω_k‖为目标运动角速度向量的模，即长轴的长度为短轴的长度与目标在当前角速度下在预留时间内的外推长度之和；

③前述生成的包络圆和包络椭圆形成平面角度安全包络，该安全包络由以椭圆短轴为分割线的沿目标运动方向的半椭圆与沿目标运动反方向的半圆组成，具体如图4中实线部分所示；

该安全包络既考虑了由于目标接近和联测误差存在造成的规避角度余量，同时安全包络也考虑了目标的运动状态；

④基于前述平面角度安全包络进行目标威胁估计，建立基于时间k的威胁标示函数l_k，当l_k<0时，认为存在碰撞威胁，以此判断是否存在碰撞威胁，其中

$l_k=\left\{\begin{array}{cc}\mathrm{sgn}({(\widetilde{\mathrm{\&sigma;}}-{\mathrm{\&sigma;}}_k)}^2+{(\widetilde{\mathrm{\&gamma;}}-{\mathrm{\&gamma;}}_k)}^2-r_k)& (\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k<0\\ \mathrm{sgn}(\frac{{(\widetilde{\mathrm{\&sigma;}}-{\mathrm{\&sigma;}}_k)}^2}{a}+\frac{{(\widetilde{\mathrm{\&gamma;}}-{\mathrm{\&gamma;}}_k)}^2}{b}-1)& (\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k\&GreaterEqual;0\end{array}\right.,$

其中表示相机B的主轴方向相对于无人机主轴方向的角度，通常可认为是符号函数；

⑤当l_k<0时，根据最小规避距离原则，在平面角度安全包络寻找距离最近的点s^*＝[σ^*,γ^*]作为最小规避机动点：

$s^{*}=\left\{\begin{array}{c}{\mathrm{\&Theta;}}_k+\frac{(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)}{\left|\right|\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k\left|\right|}{r}_k,(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k<0\\ {\mathrm{\&Theta;}}_k\&PlusMinus;e^{\&perp;{\mathrm{\&Omega;}}_k}{r}_k(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k\&GreaterEqual;0\end{array}\right.,$

其中代表k时刻垂直于目标角速度向量的单位向量。

步骤四，将步骤三中的规避角度s^*＝[σ^*,γ^*]输入视觉伺服控制***中的视觉伺服器，视觉伺服器输出需要的规避机动俯仰和偏航角度并通过串口传送至无人机自动驾驶仪完成规避机动。

①定义交互矩阵为

$L_e=\left[\begin{array}{cccccc}\frac{-\cos {\mathrm{\&sigma;}}_k\cos {\mathrm{\&gamma;}}_k}{d}& \frac{-\cos {\mathrm{\&sigma;}}_k\sin {\mathrm{\&gamma;}}_k}{d}& \sin {\mathrm{\&sigma;}}_k& \sin {\mathrm{\&gamma;}}_k& -\cos {\mathrm{\&gamma;}}_k& 0\\ \frac{\sin {\mathrm{\&gamma;}}_k}{d\sin {\mathrm{\&sigma;}}_k}& \frac{-\cos {\mathrm{\&gamma;}}_k}{d\sin {\mathrm{\&sigma;}}_k}& 0& \frac{\cos {\mathrm{\&gamma;}}_k\cos {\mathrm{\&sigma;}}_k}{\sin {\mathrm{\&sigma;}}_k}& \frac{\sin {\mathrm{\&gamma;}}_k\cos {\mathrm{\&sigma;}}_k}{\sin {\mathrm{\&sigma;}}_k}& -1\end{array}\right],$

在视觉伺服控制***的视觉伺服控制器中建立速度控制器，反馈量为完成误差的指数衰减，输出速度控制量V，视觉伺服控制器表达为

$V=[v,w]=-\mathrm{\&lambda;}{L}_e^{+}e,$

式中，λ为误差指数衰减系数，表示L_e的伪逆；

由于在交互矩阵L_e中无法获取目标与本机的距离d的信息，将交互矩阵进行分割如下：

$L_e=[\frac{1}{d}{L}_t,L_{\mathrm{\&omega;}}],$

式中，L_ω表示L_e中的角度控制分量，L_t表示速度运动控制分量；

得到距离无关的视觉伺服控制器：

$\mathrm{\&omega;}=-\mathrm{\&lambda;}{L}_{\mathrm{\&omega;}}^{+}e,$

式中，表示L_ω的伪逆；

为消除稳态误差，对视觉伺服控制器进行补偿，如下：

$\mathrm{\&omega;}=-\mathrm{\&lambda;}{L}_{\mathrm{\&omega;}}^{+}e+L_{\mathrm{\&omega;}}^{+}(\mathrm{\&Omega;}+\frac{s^{*}}{\left|\right|s^{*}\left|\right|}\mathrm{\&epsiv;})=\left[\begin{array}{ccc}{\mathrm{\&omega;}}_x& {\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_z\end{array}\right],$

式中，Ω表示目标运动的角速度，表示误差的方向向量，ω_x、ω_y、ω_z分别表示ω的三个分量；

②输出的俯仰角和偏航角为

${\mathrm{\&phi;}}_c\left(k\right)=\mathrm{\&phi;}\left(k\right)+\tan \frac{V\left(k\right){\mathrm{\&omega;}}_z\left(k\right)}{g}$

式中，和φ_c(k)表示自动驾驶仪输出姿态的俯仰角和偏航角，和φ(k)表示无人机当前姿态的俯仰角和偏航角，Δt表示视觉伺服控制器输出时间间隔步长，ω_y(k)和ω_z(k)分别表示当前时刻视觉伺服控制器输出的俯仰角速度控制量和偏航角速度控制量，g表示重力加速度，V(k)表示当前时刻无人机的速度。

步骤五，利用导航定位***建立航路回归机制，当无人机不存在威胁时，使无人机返回原始航路。

①通过本机导航定位***确定无人机是否在航路中，当本机不在航路时，进行航路回归；

②确认时刻k的回归航路角度

${\mathrm{\&gamma;}}_d^{*}={\cos }^{-1}\frac{(P_{d12}\left(k\right)-P_{s12})\&CenterDot;e_{s12}}{\left|\right|P_{d12}-P_{s12}\left|\right|}+\frac{\mathrm{\&pi;}}{2},$

${\mathrm{\&sigma;}}_d^{*}={\cos }^{-1}\frac{(P_{d13}-P_{s13})\&CenterDot;e_{s13}}{\left|\right|P_{d13}-P_{s13}\left|\right|}+\frac{3\mathrm{\&pi;}}{4},$

式中，代表了航路回归时的最小机动规避点位置；

其中P_d是当不存在规避机动时，本机在原本航路上的位置，P_d12和P_d13分别是本机在XY和XZ平面的二维位置；

P_s是通过本机导航定位***获得的本机当前位置,P_s12和P_s13分别是本机在XY和XZ平面的二维位置；

e_s是通过本机导航定位***获得的本机方向向量，e_s12和e_s13分别是本机在XY和XZ平面的二维方向向量。

本发明提供的方法及***能够实现在不加装任何测距传感器的情况下，在没有操作人员干预的情况下完成对空间飞行目标的规避机动，载荷要求低、控制精度高、具有较高的智能性，能够提高无人机的空域飞行安全能力。

Claims (8)

1.一种基于视觉伺服的感知与规避***，其特征在于，包括：

无人机，所述无人机上设置自动驾驶仪，所述自动驾驶仪接收飞行控制指令进行飞行并完成无人机对障碍物的规避动作；

图像采集***，搭载在所述无人机上，用于采集无人机的飞行空域图像信息，采用多相机多视角集中式阵列构型，并通过相机接口实时传输图像信息；

视觉伺服控制***，设置在所述无人机上，包括视觉目标检测与跟踪模块、安全包络模块和视觉伺服控制器，视觉目标检测与跟踪模块接收图像采集***传输的图像信息并实现对图像中飞行目标的检测、定位和跟踪，安全包络模块根据获取的目标状态信息进行图像平面角度安全包络的生成，并基于该安全包络生成图像规避角度，以此角度输入至视觉伺服控制器，视觉伺服控制器输出规避机动角度，并将该角度通过串口传输至自动驾驶仪完成规避动作；

导航定位***，设置在所述无人机上，用于建立航路回归机制，导航定位***检测到无人机不在航路且不存在威胁时，进行航路回归，使无人机返回原始航路。

2.根据权利要求1所述的基于视觉伺服的感知与规避***，其特征在于，所述视觉目标检测与跟踪模块采用多路高清图像处理平台DM8168。

3.一种基于视觉伺服的感知与规避***的规避方法，其特征在于，包括：

步骤一，通过所述无人机机载图像采集***实现对飞行视场角内的空域的完全覆盖，获取无人机的飞行空域图像信息；

步骤二，机载视觉伺服控制***中视觉目标检测与跟踪模块接收所述步骤一中图像信息并进行处理，完成对空域图像中飞行目标的检测、定位和跟踪；

步骤三，根据所述步骤二中获取的飞行目标状态信息，视觉伺服控制***中安全包络模块进行图像平面角度安全包络生成，通过安全包络确定无人机是否存在碰撞威胁，针对碰撞威胁，计算得到图像意义下的规避角度；

步骤四，将所述步骤三中的规避角度输入视觉伺服控制***中的视觉伺服器，视觉伺服器输出需要的规避机动俯仰和偏航角度，并传送至无人机自动驾驶仪完成规避机动；

步骤五，利用导航定位***建立航路回归机制，当无人机不存在威胁时，使无人机返回原始航路。

4.根据权利要求3所述的规避方法，其特征在于，所述步骤二的具体实现方法如下：

所述步骤一中图像采集***获取的图像数据通过相机数据线传送至机载视觉伺服控制***中视觉目标检测与跟踪模块，其中图像采集***中共设置A、B、C三台相机，视觉目标检测与跟踪模块采用多路高清图像处理平台DM8168，利用DM8168实现对获取的多路视频进行处理，通过检测算法获取目标k时刻在图像采集***中某一相机图像平面的位置信息p_k＝[p_xk,p_yk]，获得目标相对于该相机的相对角度：

${\mathrm{\&sigma;}}^{\&prime;}={\tan }^{-1}\frac{p_{\mathrm{xk}}-\frac{w}{2}}{f/\mathrm{\&mu;}}$ ${\mathrm{\&gamma;}}^{\&prime;}={\tan }^{-1}\frac{p_{\mathrm{yk}}-\frac{h}{2}}{f/\mathrm{\&mu;}}$

其中，w和h分别是以像素为单位的图像的宽度和高度，f为相机镜头焦距，μ为象元尺寸；

根据相机安装位置，可获得目标k时刻相对于无人机的相对角度为Θ_k＝[σ_k,γ_k]，其中

${\mathrm{\&sigma;}}_k=\left\{\begin{array}{c}{\mathrm{\&sigma;}}^{\&prime;}-\frac{4}{9}\mathrm{\&pi;},p_k\&Subset;A\\ {\mathrm{\&sigma;}}^{\&prime;},p_k\&Subset;B\\ {\mathrm{\&sigma;}}^{\&prime;}+\frac{4}{9}\mathrm{\&pi;},p_k\&Subset;C\end{array}\right.$

γ_k＝γ′

式中分别表示检测到的目标来自于图像采集***中的相机A、相机B和相机C；

根据前后时刻的角度变化确定目标的角速度Ω_k＝[ω_xk,ω_yk]＝Θ_k+1-Θ_k,k>0。

5.根据权利要求3所述的规避方法，其特征在于，所述步骤三中安全包络的生成步骤及规避角度的计算步骤包括：

①建立目标安全包络圆，圆心为目标点，选取一个半径初始值，并以固定速度进行增长；

②根据目标尺寸建立安全包络椭圆，椭圆圆心为目标点，椭圆短轴的长度与包络圆的半径相等，椭圆长轴的长度为短轴的长度与目标在当前角速度下在预留时间内的外推长度之和，长轴方向平行于目标的运动方向；

③前述生成的包络圆和包络椭圆形成平面角度安全包络，该安全包络由以椭圆短轴为分割线的沿目标运动方向的半椭圆与沿目标运动反方向的半圆组成；

④基于前述平面角度安全包络进行目标威胁估计，建立基于安全包络的威胁标示函数，以此判断是否存在碰撞威胁；

⑤根据最小规避距离原则，在平面角度安全包络寻找距离初始目标最近的点。

6.根据权利要求5所述的规避方法，其特征在于，所述步骤三中安全包络的具体生成步骤及规避角度的具体计算步骤如下：

①建立目标安全包络圆，圆心选为目标k时刻相对于无人机的相对角度位置Θ_k＝[σ_k,γ_k]，选取一个半径初始值r₀，并以固定速度进行增长，则

r_k+1＝r_k+ε,k＝1,2,3…

r_k＝r₀,k＝0

其中ε是固定增长速度，r_k+1代表k+1时刻的安全包络圆半径；

②根据目标尺寸建立安全包络椭圆，椭圆圆心为目标点Θ_k＝[σ_k，γ_k]，长轴方向平行于目标的运动方向，即目标角速度Ω_k的方向，长轴和短轴的长度分别按下列公式选取：

a＝r_k(1+α‖Ω_k‖)，

b＝r_k，

式中，a和b分别代表包络椭圆的长轴和短轴，r_k为k时刻包络圆的半径，α为角速度转化为轴长的比例因子，该因子代表规避预留时间，‖Ω_k‖为目标运动角速度向量的模；

③前述生成的包络圆和包络椭圆形成平面角度安全包络，该安全包络由以椭圆短轴为分割线的沿目标运动方向的半椭圆与沿目标运动反方向的半圆组成；

④基于前述平面角度安全包络进行目标威胁估计，建立基于时间k的威胁标示函数l_k，当l_k<0时，认为存在碰撞威胁，以此判断是否存在碰撞威胁，其中

$l_k=\left\{\begin{array}{cc}\mathrm{sgn}({(\widetilde{\mathrm{\&sigma;}}-{\mathrm{\&sigma;}}_k)}^2+{(\widetilde{\mathrm{\&gamma;}}-{\mathrm{\&gamma;}}_k)}^2-r_k)& (\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k<0\\ \mathrm{sgn}(\frac{{(\widetilde{\mathrm{\&sigma;}}-{\mathrm{\&sigma;}}_k)}^2}{a}+\frac{{(\widetilde{\mathrm{\&gamma;}}-{\mathrm{\&gamma;}}_k)}^2}{b}-1)& (\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k\&GreaterEqual;0\end{array}\right.,$

其中表示相机B的主轴方向相对于无人机主轴方向的角度，通常可认为是符号函数；

⑤当l_k<0时，根据最小规避距离原则，在平面角度安全包络寻找距离最近的点s^*＝[σ^*,γ^*]作为最小规避机动点：

$s^{*}=\left\{\begin{array}{c}{\mathrm{\&Theta;}}_k+\frac{(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)}{\left|\right|\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k\left|\right|}{r}_k,(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k<0\\ {\mathrm{\&Theta;}}_k\&PlusMinus;e^{\&perp;{\mathrm{\&Omega;}}_k}{r}_k(\widetilde{\mathrm{\&Theta;}}-{\mathrm{\&Theta;}}_k)\&CenterDot;{\mathrm{\&Omega;}}_k\&GreaterEqual;0\end{array}\right.,$

其中代表k时刻垂直于目标角速度向量的单位向量。

7.根据权利要求3所述的规避方法，其特征在于，所述步骤四中规避机动俯仰和偏航角度的具体计算步骤如下：

①定义交互矩阵为

$L_e=$

$\left[\begin{array}{cccccc}\frac{-\cos {\mathrm{\&sigma;}}_k\cos {\mathrm{\&gamma;}}_k}{d}& \frac{-\cos {\mathrm{\&sigma;}}_k\sin {\mathrm{\&gamma;}}_k}{d}& \sin {\mathrm{\&sigma;}}_k& \sin {\mathrm{\&gamma;}}_k& -\cos {\mathrm{\&gamma;}}_k& 0\\ \frac{\sin {\mathrm{\&gamma;}}_k}{d\sin {\mathrm{\&sigma;}}_k}& \frac{-\cos {\mathrm{\&gamma;}}_k}{d\sin {\mathrm{\&sigma;}}_k}& 0& \frac{\cos {\mathrm{\&gamma;}}_k\cos {\mathrm{\&sigma;}}_k}{\sin {\mathrm{\&sigma;}}_k}& \frac{\sin {\mathrm{\&gamma;}}_k\cos {\mathrm{\&sigma;}}_k}{\sin {\mathrm{\&sigma;}}_k}& -1\end{array}\right],$

在视觉伺服控制***的视觉伺服控制器中建立速度控制器，反馈量为完成误差的指数衰减，输出速度控制量V，视觉伺服控制器表达为

$V=[v,w]=-\mathrm{\&lambda;}{L}_e^{+}e,$

式中，λ为误差指数衰减系数，表示L_e的伪逆；

由于在交互矩阵L_e中无法获取目标与本机的距离d的信息，将交互矩阵进行分割如下：

$L_e=[\frac{1}{d}{L}_t,L_{\mathrm{\&omega;}}],$

式中，L_ω表示L_e中的角度控制分量，L_t表示速度运动控制分量；

得到距离无关的视觉伺服控制器：

$\mathrm{\&omega;}=-\mathrm{\&lambda;}{L}_{\mathrm{\&omega;}}^{+}e,$

式中，表示L_ω的伪逆；

为消除稳态误差，对视觉伺服控制器进行补偿，如下：

$\mathrm{\&omega;}=-\mathrm{\&lambda;}{L}_{\mathrm{\&omega;}}^{+}e+L_{\mathrm{\&omega;}}^{+}(\mathrm{\&Omega;}+\frac{s^{*}}{\left|\right|s^{*}\left|\right|}\mathrm{\&epsiv;})=\left[\begin{array}{ccc}{\mathrm{\&omega;}}_x& {\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_z\end{array}\right],$

式中，Ω表示目标运动的角速度，表示误差的方向向量，ω_x、ω_y、ω_z分别表示ω的三个分量；

②输出的俯仰角和偏航角为

${\mathrm{\&phi;}}_c\left(k\right)=\mathrm{\&phi;}\left(k\right)+\tan \frac{V\left(k\right){\mathrm{\&omega;}}_z\left(k\right)}{g}$

式中，和φ_c(k)表示自动驾驶仪输出姿态的俯仰角和偏航角，和φ(k)表示无人机当前姿态的俯仰角和偏航角，Δt表示视觉伺服控制器输出时间间隔步长，ω_y(k)和ω_z(k)分别表示当前时刻视觉伺服控制器输出的俯仰角速度控制量和偏航角速度控制量，g表示重力加速度，V(k)表示当前时刻无人机的速度。

8.根据权利要求3所述的规避方法，其特征在于，所述步骤五中航路回归机制的具体建立步骤如下：

①通过本机导航定位***确定无人机是否在航路中，当本机不在航路时，进行航路回归；

②确认时刻k的回归航路角度

${\mathrm{\&gamma;}}_d^{*}={\cos }^{-1}\frac{(P_{d12}\left(k\right)-P_{s12})\&CenterDot;e_{s12}}{\left|\right|P_{d12}-P_{s12}\left|\right|}+\frac{\mathrm{\&pi;}}{2},$

${\mathrm{\&sigma;}}_d^{*}={\cos }^{-1}\frac{(P_{d13}-P_{s13})\&CenterDot;e_{s13}}{\left|\right|P_{d13}-P_{s13}\left|\right|}+\frac{3\mathrm{\&pi;}}{4},$

式中，代表了航路回归时的最小机动规避点位置；

其中P_d是当不存在规避机动时，本机在原本航路上的位置，P_d12和P_d13分别是本机在XY和XZ平面的二维位置；

P_s是通过本机导航定位***获得的本机当前位置,P_s12和P_s13分别是本机在XY和XZ平面的二维位置；

e_s是通过本机导航定位***获得的本机方向向量，e_s12和e_s13分别是本机在XY和XZ平面的二维方向向量。