一种快速恢复航迹的无人机避障方法技术

本发明专利技术公开了一种快速恢复航迹的无人机避障方法，主要解决现有无人机避障方法航迹恢复速度慢以及恢复效果不佳的问题。其方案是：1)将已知的起始点、目标点、障碍物从全局坐标系转换到局部坐标系；2)判断无人机在局部坐标系中的转弯方向；3)根据无人机最小转弯半径及障碍物半径，对障碍物半径进行扩大处理；4)根据无人机与障碍物的几何关系计算局部坐标系下无人机避障的关键航迹点坐标；5)将局部坐标系下关键航迹点坐标变换到全局坐标系下，无人机绕障碍物依次飞过所有关键航迹点，完成避障及快速航迹恢复。本发明专利技术大大缩短了避障阶段的航迹长度，提高了无人机在绕开障碍物后的航迹恢复速度，可用于无人机协同侦察、定点打击及通信中继。通信中继。通信中继。

【技术实现步骤摘要】
一种快速恢复航迹的无人机避障方法


[0001]本专利技术属于无人机航迹规划领域，尤其涉及一种无人机避障方法，可用于无人机协同侦察、定点打击、通信中继。

技术介绍

[0002]目前，无人机因具有功耗低、重量轻、体积小、机动性强、成本低廉的优点，被广泛应用于协同侦察、定点打击、通信中继等场景。在众多任务场景中，由于无人机与一些障碍物如禁飞区、高山等共享同一空域，因此在执行任务过程中，极有可能与任务区域中的障碍物发生碰撞，对无人机的安全问题造成威胁。所以，在对无人机进行从起始点到目标点的航迹规划时，必须考虑到环境中存在的障碍物是否会对无人机的飞行航线产生影响，进而对无人机进行避障航迹规划。无人机避障指的是通过改变无人机飞行状态，规划出一条避免碰撞的理想航迹。在侦察场景中，无人机在执行侦察任务时会沿着一条既定的离线航迹对任务区域进行全覆盖巡航，遇到障碍物时需要进行避障，因此在避障规划方面不仅需要考虑无人机避障，而且在避障完成后要尽快回到原离线航迹继续巡航。如何提高无人机的航迹恢复速度，缩短避障阶段的整体航迹长度对于提高无人机对任务区域的覆盖率至关重要。
[0003]具有代表性的无人机避障方法主要包括：基于人工势场的避障方法、基于RRT算法的避障方法、基于航向控制律的避障及航迹恢复方法。其中，基于人工势场的避障方法通过建立虚拟势场，根据合力的方向来指引无人机避开障碍物并最终到达目标点；基于RRT算法的避障方法在搜索路径时存在随机性，随机生成满足避开障碍物这一条件的航迹点，实现无人机的避障；基于航向控制律的避障及航迹恢复方法考虑了航迹恢复问题，根据航向控制律，计算出无人机避障过程中的关键航迹点，使得无人机避开障碍物并实现了航迹恢复。
[0004]郭姣绒在其硕士论文“多无人机实时航路规划算法研究与实现”(西安：西安电子科技大学，2018)中提出了一种基于航向控制律的无人机避障及航迹恢复方法。其将障碍物建模为一个二维平面的圆形，以圆心及半径来对其进行描述，首先利用向量叉乘的方法对无人机的避障转弯方向进行判断，然后利用向点转弯的方法计算出转弯角，进而对避障路径上的关键航迹点进行计算，根据对称关系得到航迹恢复阶段的各关键航迹点坐标，最终得到完整的无人机避障及航迹恢复航迹。该方法存在的不足之处是，无人机开始避障的航迹点距离障碍物过远，且恢复航迹的速度较慢，导致整个避障段航迹长度过长。

技术实现思路

[0005]本专利技术的目的在于针对上述现有技术的不足，提出一种快速恢复航迹的无人机避障方法，以加快无人机在绕开障碍物后的航迹恢复速度，缩短整个避障阶段的无人机航迹。
[0006]本专利技术的技术方案是：充分考虑无人机避障过程中的环境约束以及无人机自身机动性能约束，即起始点S、目标点G、障碍物P圆心坐标及障碍物半径r，无人机最小转弯半径R
min
、飞行速度v、最小航迹段长度l
min
，将起始点、目标点、障碍物坐标从全局坐标系转换到
局部坐标系中，对无人机避障的转弯方向进行判断，并根据无人机与障碍物间的几何关系对避障过程中的关键航迹点进行计算，将得到的所有航迹点坐标重新转换到全局坐标系中，得到一条平滑可飞、航迹恢复速度快的避障航迹。其实现步骤包括如下：
[0007](1)将已知的起始点S(x
S
,y
S
)、目标点G(x
G
,y
G
)、障碍物P(x
P
,y
P
)从全局坐标系转换到局部坐标系中：
[0008](1a)将起始点S(x
S
,y
S
)作为局部坐标系原点，平移全局坐标系使得全局坐标系的原点与起始点S重合；
[0009](1b)由起始点S(x
S
,y
S
)和目标点(x
G
,y
G
)，确定出由起始点S指向目标点G的矢量
[0010](1c)将矢量与全局坐标系x轴正向的夹角设为θ，将全局坐标系逆时针方向旋转θ，使得x轴正向与矢量重合，得到局部坐标系；
[0011](1d)计算出起始点S、目标点G、障碍物P在局部坐标系下的坐标点S'(x'
S
,y'
S
)、G'(x'
G
,y'
G
)、P'(x'
P
,y'
P
)；
[0012](2)判断无人机在局部坐标系中的转弯方向：
[0013]若障碍物圆心纵坐标值大于0，则判断出圆心位于直线航迹左侧，无人机从右侧绕过障碍物进行避障；
[0014]若障碍物圆心纵坐标值小于等于0，则判断出圆心位于直线航迹右侧或位于直线航迹上，无人机从左侧绕过障碍物进行避障；
[0015](3)对障碍物半径进行处理：
[0016]设无人机最小转弯半径为R
min
，并将其与障碍物半径r和无人机宽度的一半w之和进行比较：
[0017]若R
min
≤r+w，其满足无人机最小转弯半径约束，则按照半径r+w规划无人机避障航迹；
[0018]若R
min
＞r+w，其不满足无人机最小转弯半径约束，则将障碍物半径r扩大到R
min
–
w，按照半径R
min
规划无人机避障航迹；
[0019](4)计算局部坐标系下无人机避障的关键航迹点坐标：
[0020](4a)将无人机从开始避障到回到原航迹的整体过程分为三个阶段：避障阶段、绕飞阶段和航迹恢复阶段；
[0021](4b)计算各阶段无人机避障的航迹点：
[0022]在绕飞阶段，利用二分法得到无人机直飞结束开始绕障碍物飞行时的航迹点C'(x'
C
,y'
C
)，并计算绕飞结束开始直飞时的航迹点D'：
[0023](x'
D
,y'
D
)＝(2x'
P
‑
x'
C
,y'
C
)，
[0024]其中(x'
D
,y'
D
)表示航迹点D'的坐标；
[0025]在避障阶段，计算出无人机开始避障进行转弯时的航迹点A'和转弯结束开始直飞时的航迹点B'：
[0026][0027](x'
B
,y'
B
)＝(x'
C
‑
l
min
·
cosα,y'
C
+l
min
·
sinα)
[0028]其中，(x'
A
,y'
A
)表示航迹点A'的坐标，(x'
B
,y'
B
)表示航迹点B'的坐标，l
min
表示无
人机的最小航迹段长度约束本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种快速恢复航迹的无人机避障方法，其特征在于，包括如下步骤：(1)将已知的起始点S(x
S
,y
S
)、目标点G(x
G
,y
G
)、障碍物P(x
P
,y
P
)从全局坐标系转换到局部坐标系中：(1a)将起始点S(x
S
,y
S
)作为局部坐标系原点，平移全局坐标系使得全局坐标系的原点与起始点S重合；(1b)由起始点S(x
S
,y
S
)和目标点(x
G
,y
G
)，确定出由起始点S指向目标点G的矢量(1c)将矢量与全局坐标系x轴正向的夹角设为θ，将全局坐标系逆时针方向旋转θ，使得x轴正向与矢量重合，得到局部坐标系；(1d)计算出起始点S、目标点G、障碍物P在局部坐标系下的坐标点S'(x'
S
,y'
S
)、G'(x'
G
,y'
G
)、P'(x'
P
,y'
P
)；(2)判断无人机在局部坐标系中的转弯方向：若障碍物圆心纵坐标值大于0，则判断出圆心位于直线航迹左侧，无人机从右侧绕过障碍物进行避障；若障碍物圆心纵坐标值小于等于0，则判断出圆心位于直线航迹右侧或位于直线航迹上，无人机从左侧绕过障碍物进行避障；(3)对障碍物半径进行处理：设无人机最小转弯半径为R
min
，并将其与障碍物半径r和无人机宽度的一半w之和进行比较：若R
min
≤r+w，其满足无人机最小转弯半径约束，则按照半径r+w规划无人机避障航迹；若R
min
＞r+w，其不满足无人机最小转弯半径约束，则将障碍物半径r扩大到R
min
–
w，按照半径R
min
规划无人机避障航迹；(4)计算局部坐标系下无人机避障的关键航迹点坐标：(4a)将无人机从开始避障到回到原航迹的整体过程分为三个阶段：避障阶段、绕飞阶段和航迹恢复阶段；(4b)计算各阶段无人机避障的航迹点：在绕飞阶段，利用二分法得到无人机直飞结束开始绕障碍物飞行时的航迹点C'(x'
C
,y'
C
)，并计算绕飞结束开始直飞时的航迹点D'：(x'
D
,y'
D
)＝(2x'
P
‑
x'
C
,y'
C
)，其中(x'
D
,y'
D
)表示航迹点D'的坐标；在避障阶段，计算出无人机开始避障进行转弯时的航迹点A'和转弯结束开始直飞时的航迹点B'：(x'
B
,y'
B
)＝(x'
C
‑
l
min
·
cosα,y'
C
+l
min
·
sinα)其中，(x'
A
,y'
A
)表示航迹点A'的坐标，(x'
B
,y'
B
)表示航迹点B'的坐标，l
min
表示无人机的最小航迹段长度约束，(x'
H
,y'
H
)表示过点C'与P'C'相垂直的直线与x轴的交点H'的坐标，α表示H'C'与x轴正向的夹角；在航迹恢复阶段，计算出无人机直飞结束开始转弯准备回到原航迹时的航迹点E'、转弯结束回到原航迹时的航迹点F'：
(x'
E
,y'
E
)＝(2x'
P
‑
x'
B
,y'
B
)(x'
F
,y'
F
)＝(2x'
P
‑
x'
A
,y'
A
)其中，(x'
E
,y'
E
)表示航迹点E'的坐标，(x'
F
,y'
F
)表示航迹点F'的坐标；(5)将(4b)中计算出的局部坐标系下的关键航迹点A'、B'、C'、D'、E'、F'坐标变换到全局坐标系下，对应得到航迹点A、B、C、D、E、F的坐标；(6)无人机依次飞过航迹点A、B、C、D、E、F，完成避障及快速航迹恢复。2.根据权利要求1所述的方法，其特征在于：步骤(1d)中，计算全局坐标系下的点S、G、P在局部坐标系下的坐标点S'(x'
S
,y'
S
)、G'(x'
G
,y'
G
)、P'(x'
P
,y'
P
)，公式如下：)，公式如下：)，公式如下：3.根据权利要求1所述的方法，其特征在于：步骤(4b)中在绕飞阶段，利用二分法得到无人机直飞结束开始绕障碍物飞行时的航迹点C'(x'
C
,y'
C
)，实现如下：(4b1)设全局坐标系下的障碍物P转换到局部坐标系下坐标为P'(x'
P
,y'

【专利技术属性】
技术研发人员：马英红，李续楠，焦毅，刘勤，刘伟，张琰，
申请(专利权)人：西安电子科技大学，
类型：发明
国别省市：