CN111781949A - 一种无人机针对杆型障碍物的规避方法 - Google Patents

Abstract

本发明提供了一种无人机针对杆型障碍物的规避方法，包括如下方法：通过无人机上的探测设备对飞行路径上的周边环境进行感知和识别，所述探测设备实时输出的探测数据；所述无人机根据探测数据获取杆型障碍物的相对位置信息，所述相对位置信息包括无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度；所述无人机根据无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度确定杆型障碍物的轮廓；所述无人机根据杆型障碍物的轮廓计算出当前姿态下最小距离；根据计算出当前姿态下最小距离，无人机进行碰撞紧迫性评估，确定是否调整飞行的轨迹路线。本发明一方面在一定数值精度基础上极大简化了算法的计算复杂度，减少了执行时间。

Description

一种无人机针对杆型障碍物的规避方法

技术领域

本发明涉及无人机技术领域，特别涉及一种无人机针对杆型障碍物的规避方法。

背景技术

在实际作业过程中，无人机与障碍物之间的相对距离随时间变化，尤其是对地面上的静态障碍物而言。尽管田间的静态障碍物类型多种多样，但依旧可以按照障碍物的实际尺寸大小进行分类：微型障碍如架于高空的电缆线、树木的枝干枝丫等；小中型障碍如矗立的电线杆和分散栽培的田间树木等；大型障碍：房屋、高压电塔等。考虑到不同地区在地形地势、作物栽培条件跟当地田间种植管理政策上的差异，很多区域田间存在大面积的树木、高压电塔以及房屋的概率是很小的，而植保无人机一般选取低空作业，即3～5m的飞行高度，故有可能产生威胁的高空电缆线也涵盖在内。而经过实地勘察，电线杆作为生产生活的基础设施，在田间或周围均是普遍存在的。

随着作业进程的推进，喷雾机组不断缩小与田间障碍物之间的距离也意味着两者之间接近程度的不断加深。如果障碍物与无人机之间的距离太近，那么无人机高速旋转的桨叶形成的桨叶气流接触到障碍物壁面会形成反射流，会对桨叶气流的分布造成干扰，从而影响无人机飞行性能，加之电子设备之间各自噪声干扰甚至会导致机体撞击障碍物或其他安全事故的发生。然而，如果该距离太远，即无人机在不具任何因素提示的情况下提前极早进行避让动作，结果是避障顺利但会造成邻行作物重喷及障碍物周边作物漏喷现象，进而导致作物作业效率与生物防治效果得不到有效保证。因此，准确测定植保无人机与田间障碍物之间的最小安全距离是制约航空植保喷雾行业长期发展的关键因素。事实上，无人机的最小距离测量是一项集动力学、运动学、流体力学等多门学科耦合的挑战性工作。这项工作不仅可以保证设备、无人机以及作物的安全，还能对后续喷雾作业实现优质效果所需要的各项技术参数提供指导作用。

为了提高无人机避障的机动性，满足在各种工作条件下的工作需要，提高无人机避障作业的安全性和可靠性，精准确定无人机与障碍物之间的最小距离很重要。因此，对无人机与障碍物之间的最小距离进行量化是有必要的。一些研究人员在对一些自主驾驶类机具进行的开发试验中借助障碍物的探测与识别技术来判断和衡量最小距离，并取得了阶段性的研究成果。Berker M et al.和Bouabdallah在Design and control of quadrotorswith application to autonomous flying文章中使用多个超声波传感器用作障碍物检测，但超声波覆盖范围有限，且无法很好的执行距离控制。Scherer S et al.Flying fastand low among obstacles:Methodology and experiments属于首批将激光扫描仪投入障碍物探测的部门。但激光扫描仪采集的数据为离散信息，对于二维激光扫描仪特性之一，会随时间发生数据漂移对扫描数据的精确性产生很大影响，在农业无人机领域较少见。

事实上，毫米波雷达具有穿透能力强、可探测距离远的优点，其依靠微波波段工作有效避开了环境干扰的巨大影响，但它只能描述场中障碍物的平行距离，无法给出轮廓角度等进一步的信息，相反，超声波传感器探测范围有限且和其他很多传感器一样由于工作机理等各种因素常会受到环境因素和监测盲区的限制。

发明内容

针对现有技术中存在的不足，本发明提供了一种无人机针对杆型障碍物的规避方法，将胶囊法拓展用于无人机避障领域，其中障碍物包括任何有一定直径细长型杆状物体，其可与圆柱体在集合上具备一定相似性，本发明实施案例将机体与障碍物均视作空间点集，三维空间即椭球体和圆柱体，由机体探测设备进行感知和识别，以此来刻划无人机相对于地面静态障碍物的相对位置，为后续的距离衡量提供几何理论计算步骤与实现效果。

本发明是通过以下技术手段实现上述技术目的的。

一种无人机针对杆型障碍物的规避方法，包括如下方法：

通过无人机上的探测设备对飞行路径上的周边环境进行感知和识别，所述探测设备实时输出的探测数据；

所述无人机根据探测数据获取杆型障碍物的相对位置信息，所述相对位置信息包括无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度；所述无人机根据无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度确定杆型障碍物的轮廓；

所述无人机根据杆型障碍物的轮廓计算出当前姿态下最小距离d_min，具体如下：

根据所述无人机包裹机体外缘呈椭球体，建立椭球体的无人机上任意一点在惯性坐标系中的坐标：

x^′＝R^-1(x,y,z)^T，

其中，(x,y,z)^T为所述椭球体上任意点位置；

R^-1为旋转矩阵R的逆矩阵，旋转矩阵R为：

为侧倾角，θ为俯仰角，ψ为偏航角；

所述椭球体的无人机上一点s₁可表示为：s₁＝s₀+λ₂(s₁-s₀)，其中λ₂为常数，b＜＜λ₂＜＜a，s0为椭球体中心位置矢量，s₁为椭球体上任意点位置矢量；a为椭球体长轴半径，b为椭球体短轴半径；

根据杆型障碍物的轮廓确定杆型障碍物两端的位置和回转半径ρ；

所述障碍物上任意一点s₂可表示为：s₂＝u+λ₁(q-u)，其中，λ₁为常数，0<λ₁<1，q和u分别为杆型障碍物沿轴向两端点位置矢量；

椭球体上任意点与障碍物表面之间的最小距离为：min(Δr)＝min(|s₁s₂|-ρ)，Δr为空间最小距离矢量，且Δr＝s₂-s₁，ρ为障碍物的回转半径；

通过二次规划和正交分解，得出min(Δr)＝min(v^Tv)，其中v＝Rx+Q^Ty，Q为是列向量为单位长度且相互正交的矩阵,R为三角矩阵；向量y＝u-s₀；

优化求解得：

其中，其中v_min为v的最小值；

根据当前姿态下最小距离d_min，无人机进行碰撞紧迫性评估，确定是否调整飞行的轨迹路线。

进一步，所述椭球体上任意点与障碍物表面之间的最小距离min(Δr)通过二次规划为：

min(Δr)＝min(Ax+y)^T(Ax+y)

其中，矩阵

向量y＝u-s₀，向量x＝[x₁ x₂]^T，且0<x₁<1，b＜＜x₂＜＜a。

进一步，对矩阵A正交分解后得到min(Δr)＝min(Rx+Q^Ty)^T(Rx+Q^Ty)。

进一步，所述碰撞紧迫性评估具体为：

当前姿态下最小距离d_min小于等于设定阈值距离d_th，通过避障算法重新规划路径；

当前姿态下最小距离d_min小于碰撞危险距离d_haz，通过反向飞行避免碰撞。

进一步，所述探测设备包括毫米波雷达和超声波传感器，用于障碍物识别与感知。

进一步，根据杆型障碍物的轮廓确定杆型障碍物两端的位置和回转半径ρ，具体为：

所述杆型障碍物的轮廓为每一时刻探测设备测得的散点数据组成，所述散点数据包括无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度；根据所述杆型障碍物的轮廓的曲线弧长和弧度求解出该障碍物的回转半径ρ，根据探测设备测得的杆型障碍物高度，确定障碍物轴向两端点的空间位置。

本发明的有益效果在于：

本发明所述的无人机针对杆型障碍物的规避方法，一方面在一定数值精度基础上极大简化了算法的计算复杂度，减少了执行时间；另一方面高效的距离计算与评估有助于植保无人机在作业过程中对田间状况变化的及时调整，顺利完成喷雾作业，提高喷雾效率以达到预期要求。

附图说明

图1为本发明所述的无人机针对杆型障碍物的规避方法流程图。

图2为本发明所述的障碍物相对位置获取示意图。

图3为本发明所述的无人机-障碍物空间最小距离计算示意图。

图4为本发明所述的无人机为中心观测对象的预设区域图。

图5为本发明所述的碰撞紧迫性评估逻辑判断流程图。

图6为本发明的空间坐标系转换示意图。

图4中：

Ⅰ：无人机的椭圆形凸包络线，可视为机体本体；

Ⅱ:危险区域外边界线；

Ⅲ：次危险区域外边界线；

Ⅳ：自由区域外边界线；

d_haz:碰撞危险距离；

d_th:安全阈值距离。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例的技术方案进行清楚的描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域研究人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，本发明中的所有研究理论或计算方法均有理论来源，所述组件之间均属刚性连接，无人机空间视为刚体，结合所述模型和原理在附录贴有实施案例及结果说明，属于本发明保护范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

下面结合附图，对本发明的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互组合。

如图1所示，本发明所述的无人机针对杆型障碍物的规避方法包括如下步骤：

步骤1，在所述无人机按照先前既定的作业路线飞行的过程中，通过所述探测设备对周边环境进行感知，获取所述探测设备实时输出的探测数据。

本实施案例方法的执行主体是飞行控制设备，具体可以是无人机的飞行控制器。在本实施方式中，无人机能够实现设定作业路线，并要求无人机按照该路线进行飞行，无人机中设置有探测设备，该探测设备能够对当前无人机作业路线周边的障碍物环境进行感知和探测，并输出探测数据，同时允许飞行控制器实时获取该探测数据。

所述探测设备采用毫米波雷达和超声波传感器相结合的组合方式对障碍物实施识别与检测。利用该方法，前者在发挥中长距离抗干扰探测优势的同时后者弥补了近距离识别与定位的弱项。与其余不具有将冗余传感器用于障碍物识别与检测的工程技术相比，该组合方案既达到性能优势的最大化又满足可接受的成本，并提高了系统检测的可靠性。至此将两种依托于不同工作原理的传感器在各自的优劣势上实现互补，对障碍物距离的实测提供了良好的硬件平台。

步骤2，所述无人机根据探测数据获取杆型障碍物的相对位置信息，所述相对位置信息包括无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度；所述无人机根据无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度确定杆型障碍物的轮廓；

无人机与杆型障碍物的相对距离代表探测设备中心点距障碍物表面的空间距离，该距离并不是为本发明需要得到的无人机距障碍物的最小距离；无人机与杆型障碍物的相对角度指障碍物相对于无人机机头正方向的夹角。相对距离和相对角度均用于后续定位障碍物轴向两端点的空间位置。

如图2所示，无人机在每一时刻点都能获取当下障碍物的大致轮廓，该轮廓是根据每一时刻探测设备测得的散点数据构成，所述散点数据包括相对距离和相对角度，将这些散点依次连接可构成轮廓曲线，根据轮廓曲线弧长与弧度求解出该障碍物的半径ρ，此时障碍物的轴线位置可定位，依据地面站提供的高度信息能够确定无人机当下探测到的散点高度，能够计算确定障碍物轴向两端点的空间位置，即q和u。

由于无人机作业是个动态过程，加之探测设备本身特性差异，因此探测设备之间数据可以是互补和冗余的。具体来说，当无人机距离障碍物较远时，高度利用毫米波雷达远距离感知的优势可以实现远程距离及角度的数据获取，随着无人机逐渐靠近障碍物，在获取距离跟角度的同时，障碍物的静态轮廓也能通过超声波全方位获取。通过，每个时刻的来自不同探测设备的散点数据，基本可以构造障碍物不同位置点的点集，实现对障碍物静态轮廓的描述。

步骤3，所述无人机根据杆型障碍物的轮廓计算出当前姿态下最小距离d_min，首先通过地面站获取无人机当前空间位置点坐标，将其视为中心点建立如图3所示的刚体坐标系并建立包裹机体外缘的最小体积椭球体外曲面方程并得到椭球体上任一点坐标，同理将杆型障碍物视作柱体得到障碍物表面任一点坐标，两者差值最小化得到无人机与障碍物之间最小距离d_min，具体如下：

假设所述无人机自身为刚体，建立以所述无人机为原点的刚体坐标系并建立所述包裹机体外缘的最小体积椭球体外曲面方程：

其中(x₀,y₀,z₀)^T是所述无人机中心点坐标；(x,y,z)^T是所述椭球体上任意点位置；

a为椭球体长轴半径，即沿着x_B；b为椭球体短轴半径，沿着y_B；c是极半径，即沿着z_B；且|x|≤a,|y|≤b,|z|≤c,a>0,b>0,c>0，z＜＜b＜＜a。

结合无人机-障碍物空间最小距离计算示意图3，通过所述地面站获取无人机当前空间位置信息，具体到空间某一位置点，设所述位置点坐标为(x₀,y₀,z₀)^T，该点可以视为无人机中心点。

位于椭球体上任意一点在惯性坐标系中的坐标可以表示成公式(1)

详细地，公式(1)中坐标的转换需要借助旋转矩阵进行计算，计算公式如公式(2)所示

x′＝R^-1(x,y,z)^T (2)

其中R^-1为所述旋转矩阵逆矩阵。

惯性坐标系可以均匀且各向同性描述空间，而在一般的工程动力学问题中，与地球相固结的坐标系可以作为一个近似的惯性系。旋转矩阵用来描述物体绕右手笛卡尔坐标系三轴进行有次序旋转一定角度之后的基本矩阵的复合，可以用来描述刚体在空间的姿态角。飞行动力学中，如图6关于右手笛卡尔坐标系的x_w、y_w、z_w轴的旋转分别叫做侧倾、俯仰、偏航旋转。

所述的旋转矩阵R：

其中，

为侧倾角，θ为俯仰角，ψ为偏航角。这三个角度即分别绕上述x_w、y_w、z_w轴旋转的角度。

通过所述转换矩阵将所述刚体坐标系中的点转换为所述惯性坐标系中，结合无人机-障碍物空间最小距离计算示意图3分析，椭球体上一点在惯性坐标系中具体位置能够获得并用向量表示方法可表示为

同理椭球体球心表示为

根据图3可知，椭球体上一点s₁可表示为公式(3)

s₁＝s₀+λ₂(s₁-s₀) (3)

其中λ₂为常数，b＜＜λ₂＜＜a，s₀为机体中心位置矢量，s₁为椭球体上任意点位置矢量。

同理，所述障碍物上任意一点s₂可表示为公式(4)

s₂＝u+λ₁(q-u) (4)

其中，λ₁为常数，0<λ₁<1，q和u为杆型障碍物沿轴向两端点位置矢量。

如此，问题转变为计算椭球体上一点与障碍物表面一点之间的最小距离问题，由公式(5)表示

min(Δr)＝min(|s₁s₂|-ρ) (5)

其中Δr代表所述空间最小距离矢量，且Δr＝s₂-s₁，ρ为障碍物的半径，考虑到ρ为常数，因公式(5)能够转写为公式(6)

min(Δr)＝min(|s₁s₂|) (6)

所述公式(6)还可进一步改写成如公式(7)所示的二次规划形式

min(Δr)＝min(Ax+y)^T(Ax+y) (7)

其中矩阵

向量y＝u-s₀，向量x＝[x₁ x₂]^T，且0<x₁<1，b＜＜x₂＜＜a。

通过对矩阵A进行正交分解能够将所述公式(7)转写为公式(8)

min(Δr)＝min(Rx+Q^Ty)^T(Rx+Q^Ty) (8)

其中，Q为是列向量为单位长度且相互正交的6×2阶矩阵,R为2×2阶上三角矩阵。

通过简单代换，将公式(8)转换成公式(9)

min(Δr)＝min(v^Tv) (9)

其中，v＝Rx+Q^Ty。

那么最小距离可由公式(9)优化求解，如公式(10)所示：

其中v_min是对公式(9)中v取最小，即可行解集最接近原点或可行解集边界的点。

步骤4，根据计算出当前姿态下最小距离d_min，无人机进行碰撞紧迫性评估，确定是否调整飞行的轨迹路线。如图4和图5所示，当前姿态下最小距离d_min小于等于设定阈值距离d_th，通过避障算法重新规划路径；前姿态下最小距离d_min大于设定阈值距离d_th，保持原飞行轨迹。当前姿态下最小距离d_min小于碰撞危险距离d_haz，通过反向飞行避免碰撞。

步骤5至步骤7表明在进行完步骤4所述的碰撞紧迫性评估后对当下无人机所处的情况作出判断而后将调整信号发送给飞行控制器，由此通过信号接收控制器作出有关于避障路径的调整路径。通过用户使用的遥控界面或是通过数传连接的计算机地面站界面均能实时反映出当下无人机飞行的轨迹路线。

以植保无人机举例来说，植保无人机能够按照预定作业路线进行喷洒作业。然而，实际田间环境状况复杂，障碍物类型多样化，包括但不限于电线杆、高压电线塔、高压线以及房屋、树木等。因此，本实施方式所述的高杆型障碍物可以是电线杆、高压电线塔或是高压线等此类在几何机理上具备一定直径长细型类圆柱体几何特征的障碍物。

以电线杆此类普遍性存在的田间障碍物来说，所述障碍物可视为具有一定高度及半径厚度的圆柱体，并将所述的半径厚度设为ρ,其实际高度及半径需要前期实地估测得到大致尺寸范围。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (6)

1.一种无人机针对杆型障碍物的规避方法，其特征在于，包括如下方法：

通过无人机上的探测设备对飞行路径上的周边环境进行感知和识别，所述探测设备实时输出的探测数据；

所述无人机根据探测数据获取杆型障碍物的相对位置信息，所述相对位置信息包括无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度；所述无人机根据无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度确定杆型障碍物的轮廓；

所述无人机根据杆型障碍物的轮廓计算出当前姿态下最小距离d_min，具体如下：

根据所述无人机包裹机体外缘呈椭球体，建立椭球体的无人机上任意一点在惯性坐标系中的坐标：

x′＝R^-1(x,y,z)^T，

其中，(x,y,z)^T为所述椭球体上任意点位置；

R^-1为旋转矩阵R的逆矩阵，旋转矩阵R为：

为侧倾角，θ为俯仰角，ψ为偏航角；

所述椭球体的无人机上一点s₁可表示为：s₁＝s₀+λ₂(s₁-s₀)，其中λ₂为常数，b＜＜λ₂＜＜a，s₀为椭球体中心位置矢量，s₁为椭球体上任意点位置矢量；

a为椭球体长轴半径，b为椭球体短轴半径；

根据杆型障碍物的轮廓确定杆型障碍物两端的位置和回转半径ρ；

所述障碍物上任意一点s₂可表示为：s₂＝u+λ₁(q-u)，其中，λ₁为常数，0<λ₁<1，q和u分别为杆型障碍物沿轴向两端点位置矢量；

椭球体上任意点与障碍物表面之间的最小距离为：min(Δr)＝min(|s₁s₂|-ρ)，Δr为空间最小距离矢量，且Δr＝s₂-s₁，ρ为障碍物的回转半径；

通过二次规划和正交分解，得出min(Δr)＝min(v^Tv)，其中v＝Rx+Q^Ty，Q为是列向量为单位长度且相互正交的矩阵,R为三角矩阵；向量y＝u-s₀；

优化求解得：

其中，其中v_min为v的最小值；

根据当前姿态下最小距离d_min，无人机进行碰撞紧迫性评估，确定是否调整飞行的轨迹路线。

2.根据权利要求1所述的无人机针对杆型障碍物的规避方法，其特征在于，所述椭球体上任意点与障碍物表面之间的最小距离min(Δr)通过二次规划为：

min(Δr)＝min(Ax+y)^T(Ax+y)

其中，矩阵

向量y＝u-s₀，向量x＝[x₁ x₂]^T，且0<x₁<1，b＜＜x₂＜＜a。

3.根据权利要求2所述的无人机针对杆型障碍物的规避方法，其特征在于，对矩阵A正交分解后得到min(Δr)＝min(Rx+Q^Ty)^T(Rx+Q^Ty)。

4.根据权利要求1所述的无人机针对杆型障碍物的规避方法，其特征在于，所述碰撞紧迫性评估具体为：

当前姿态下最小距离d_min小于等于设定阈值距离d_th，通过避障算法重新规划路径；

当前姿态下最小距离d_min小于碰撞危险距离d_haz，通过反向飞行避免碰撞。

5.根据权利要求1所述的无人机针对杆型障碍物的规避方法，其特征在于，所述探测设备包括毫米波雷达和超声波传感器，用于障碍物识别与感知。

6.根据权利要求1所述的无人机针对杆型障碍物的规避方法，其特征在于，根据杆型障碍物的轮廓确定杆型障碍物两端的位置和回转半径ρ，具体为：

所述杆型障碍物的轮廓为每一时刻探测设备测得的散点数据组成，所述散点数据包括无人机与杆型障碍物的相对距离和无人机与杆型障碍物的相对角度；根据所述杆型障碍物的轮廓的曲线弧长和弧度求解出该障碍物的回转半径ρ，根据探测设备测得的杆型障碍物高度，确定障碍物轴向两端点的空间位置。

Images