CN107272741B - 一种基于最大内切圆圆心移动矢量的无人机自动避障方法 - Google Patents

Abstract

本发明提供一种基于最大内切圆圆心移动矢量的无人机自动避障方法，具体过程为：以无人机所处位置为原点，获取当前时刻无人机所处的无故障区域的内切圆的圆心；根据内切圆圆心位置，计算无人机斥力方向；根据圆心移动距离和斥力方向计算避障控制量，无人机飞控系统根据所述控制量进行控制。根据无人机和水平障碍物的具体位置关系，使用基于最大内切圆圆心移动矢量的路径规划进行自主避障。

Description

一种基于最大内切圆圆心移动矢量的无人机自动避障方法

技术领域

本发明属于无人机避障技术领域，特别涉及一种基于最大内切圆圆心移动矢量的无人机自动避障方法。

背景技术

多旋翼无人机具有体积小、重量轻、开发成本低、安全性好等优点，得到了许多机构和公司企业的广泛青睐。目前，多旋翼无人机被广泛应用在安防巡逻、电力巡视、植保、快递运送、灾害搜救等作业中。但是无人机在手动或者自动巡逻的过程中，随时会遇到障碍物，为了保证无人机和人员的安全，自主避障系统显得尤为重要。

目前主要采用的避障传感器为超声波或者视觉传感器，容易受环境的影响。相对于超声波和视觉，雷达可靠性和探测范围比较高，并且可实现360°检测障碍。人工势场法为目前最常用的局部路径规划算法，该算法将障碍物当作质点，忽略其环境信息，当环境较为复杂时，计算复杂度增加。

发明内容

为了解决上述问题，本发明提供了一种基于最大内切圆圆心移动矢量的无人机自动避障方法，该方法能够快速实现无人机的自主避障，满足无人机控制的实时性要求。

实现本发明的技术方案如下：

一种基于最大内切圆圆心移动矢量的无人机自动避障方法，具体过程为：

以无人机所处位置为原点，获取当前时刻无人机所处的无故障区域的内切圆的圆心；

根据内切圆圆心位置，计算无人机斥力方向；

根据圆心移动距离和斥力方向计算避障控制量，无人机飞控系统根据所述控制量进行控制。

进一步地，本发明所述内切圆的圆心的获取过程为：

步骤一：无人机上雷达扫描无人机防御区域，获得区域中障碍物的角度和距离数据；

步骤二：将雷达获取的数据转换到栅格坐标系下，获得障碍物栅格地图；

步骤三：对障碍物栅格地图进行膨胀-腐蚀-膨胀处理，提取无人机周围无障碍物区域；

步骤四：计算所述无故障区域最大内切圆圆心。

进一步地，本发明所述步骤四的具体过程为：

(1)依据所提取区域边缘，计算初始圆心O′(x₀,y₀)；

x_j,y_j为所提取区域边缘数据点坐标

(2)寻找区域边缘上距离初始圆心O′最近的点Ps1，做内切圆；以O′和点Ps1连线，远离Ps1方向移动圆心，直至内切圆和区域边缘上第二个点Ps2相切；

(3)令内切圆的圆心沿着点Ps1、Ps2连线的垂直平分线的两边分别移动，寻找内切圆与区域边缘的接触点，选取圆心移动距离较大的一边所对应的接触点为点Ps3；

(4)将点Ps1、Ps2、Ps3三点所确定的圆的圆心作为区域内最大内切圆圆心(x_i,y_i)。

进一步地，本发明所述根据内切圆圆心位置，计算斥力方向；

斥力方向为

进一步地，本发明根据所述斥力方向和圆心移动距离计算出避障控制量为：

x，y方向控制量u(x)，u(y)：

u(x)＝Δu(l)*cosθ

u(y)＝Δu(l)*sinθ

Δu(l)＝K_p(err(l)-err(l-1))+K_ierr(l)+K_d(err(l-1)-err(l-2))

其中，err(l)为第l次圆心移动距离，l为变量，K_p为比例系数，K_i为积分系数，K_d为微分系数。

有益效果

本发明提供了一种高效、快速的无人机避障方法，包括局部地图的创建和基于最大内切圆圆心移动矢量的路径规划，该路径规划方法通过雷达建图，可描述360°的局部环境，将所建地图中的所有障碍物的作用用最大内切圆圆心移动矢量来表示，可得到精确的避障控制矢量。该方法可完成快速检测障碍和自主避障，计算速度满足无人机控制的实时性要求。

附图说明

图1为本发明避障流程图；

图2为本发明避障系统框图；

图3为求解最大内切圆迭代过程。

具体实施方式

下面结合附图和具体实例对本发明进行详细说明。

本发明的设计思路为：首先利用二维激光雷达所测角度和距离数据建立以无人机为中心的局部环境信息，并将此时的环境信息用栅格图表示；根据无人机和水平障碍物的具体位置关系，使用基于最大内切圆圆心移动矢量的路径规划算法进行自主避障。

本发明一种基于最大内切圆圆心移动矢量的无人机自动避障方法，具体过程为：

步骤一：无人机上雷达扫描无人机防御区域，获取区域中故障物的角度和距离数据，具体包括：

雷达测出的是距离和角度数据，为了避免距离数据发生跳变而产生的误操作，采用中值滤波对距离数据进行处理，处理过的数据可以用来地图构建。

步骤二：将雷达数据(ρ,θ)(ρ为角度，θ所对应的距离)，转换到栅格坐标系(x_g,y_g)(x_g,y_g为整数)，具体包括：

S21.建立栅格度量信息

取单个栅格大小为g*g(g为栅格相对于实际空间大小的度量值)。然后以无人机中心为原点，设定局部防御区域大小为2w*2w(每个栅格初始化为0)，则：-w<x<w,-w<y<w。其中0表示自由栅格，1表示障碍栅格。

S22.将激光雷达数据转换成栅格坐标

相当于把激光雷达数据先转换到机体坐标系(x,y)，然后把该数据在机体坐标系下的坐标转换到栅格坐标系(x_g,y_g)。转换公式如下：

其中，ceil为取整函数。

S23.将在无人机防御区域的障碍点所对应的栅格的f(i,j)值置1，其余栅格的值置0。

步骤三：对障碍栅格地图进行膨胀-腐蚀-膨胀操作，提取无人机周围无故障区域的边缘；

障碍物的膨胀运算首先可以使障碍物与无人机行驶路径之间有一定的缓冲距离，这段距离可以让无人机与障碍物保持在安全的防撞区域内；另一方面，也可以使一些没有连续采集到的同一个障碍物的信息随着膨胀的过程在地图上变成连接的一体障碍，提高了离散数据在栅格地图上对障碍物的表达准确度。障碍物的腐蚀运算，可以避免对障碍的误判。

S31.对当前栅格地图进行膨胀-腐蚀-膨胀处理

假设输入的栅格地图为f(i,j)，输出的栅格地图为g(i,j)，其中i，j为栅格坐标，则膨胀和腐蚀过程可以表述为：

膨胀：

腐蚀：

S32.边缘提取

对于二值栅格图,只需要判断栅格值是否为1并且4领域内是否同时存在0和1。如果存在这样的情况，那么邻域中间点就是边缘点，否则边缘点就是无人机防御区域边缘，得到无人机防御区域内不包含障碍物的区域。

步骤四：针对步骤三中提取出的区域，计算其最大内切圆圆心

面对复杂的环境情况时，人工势场中斥力场的计算会比较复杂。本发明基于人工势场法，本申请基于圆心移动矢量获取无人机的控制输入，实现路径规划。

将雷达所处的位置(即无人机防御区域的中心)定义为原点(0,0)，针对步骤三中所提取出的区域，寻找区域内的最大内切圆，并求出圆心坐标(x_i,y_i)。

其中，最大内切圆圆心求解步骤如下：

(1)计算最小二乘圆心为初始圆心

x_j,y_j为所提取区域边缘数据点坐标

(2)以轮廓上距离初始圆心最近的点Ps1半径为ρ(P_s1,O')做圆，以O'和Ps1连线，远离Ps1方向移动圆心，直至内切圆和轮廓上第二个点Ps2相切，见附图3。

(3)获得两接触点Ps1、Ps2后，沿两接触点连线的垂直平分线向两边寻找第三个接触点Ps3。圆心的移动距离分别为t1，t2，取较大的距离对应的点为Ps3。根据三个点坐标：Ps1、Ps2、Ps3，带入圆公式

(x-x_i)²+(y-y_i)²＝r²

求得圆心坐标(x_i,y_i)和半径r。

步骤五：根据最大内切圆圆心，计算斥力方向和斥力大小；

斥力方向为：

斥力大小为：

η为比例系数。

步骤六：计算出避障控制量传给无人机的飞控系统实现对无人机的控制利用传统PID算法，离散化增量式PID表示公式如下：

Δu(l)＝K_p(err(l)-err(l-1))+K_ierr(l)+K_d(err(l-1)-err(l-2))

其中，err(l)为第l次圆心移动距离，l为变量，K_p为比例系数，K_i为积分系数，K_d为微分系数，分别计算出x和y方向控制量u(x),u(y):斥力的大小和err()大小有关，计算出Δu(l)，然后根据斥力方向θ和Δu(l)，计算出x，y方向控制量。

u(x)＝Δu(l)*cosθ

u(y)＝Δu(l)*sinθ

利用x和y方向控制量u(x),u(y)对无人机进行控制。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于最大内切圆圆心移动矢量的无人机自动避障方法，其特征在于，具体过程为：

以无人机所处位置为原点，获取当前时刻无人机所处的无障碍物区域的内切圆的圆心；

根据内切圆圆心位置，计算无人机斥力方向；

根据圆心移动距离和斥力方向计算避障控制量，无人机飞控系统根据所述控制量进行控制；

所述内切圆的圆心的获取过程为：

步骤一：无人机上雷达扫描无人机防御区域，获得区域中障碍物的角度和距离数据；

步骤二：将雷达获取的数据转换到栅格坐标系下，获得障碍物栅格地图；

步骤三：对障碍物栅格地图进行膨胀-腐蚀-膨胀处理，提取无人机周围无障碍物区域；

步骤四：计算所述无障碍物区域最大内切圆圆心；

所述步骤四的具体过程为：

(1)依据所提取区域边缘，计算初始圆心O′(x₀,y₀)；

为所提取区域边缘数据点坐标

(2)寻找区域边缘上距离初始圆心O′最近的点Ps1，做内切圆；以O′和点Ps1连线，远离Ps1方向移动圆心，直至内切圆和区域边缘上第二个点Ps2相切；

(3)令内切圆的圆心沿着点Ps1、Ps2连线的垂直平分线的两边分别移动，寻找内切圆与区域边缘的接触点，选取圆心移动距离较大的一边所对应的接触点为点Ps3；

(4)将点Ps1、Ps2、Ps3三点所确定的圆的圆心作为区域内最大内切圆圆心(x_i,y_i)。

2.根据权利要求1所述基于最大内切圆圆心移动矢量的无人机自动避障方法，其特征在于，所述根据内切圆圆心位置，计算斥力方向；

斥力方向为

3.根据权利要求1所述基于最大内切圆圆心移动矢量的无人机自动避障方法，其特征在于，根据所述斥力方向和圆心移动距离计算出避障控制量为：

x，y方向控制量u(x)，u(y)：

u(x)＝Δu(l)*cosθ

u(y)＝Δu(l)*sinθ

Δu(l)＝K_p(err(l)-err(l-1))+K_ierr(l)+K_d(err(l-1)-err(l-2))

其中，err(l)为第l次圆心移动距离，l为变量，K_p为比例系数，K_i为积分系数，K_d为微分系数。

Images