CN109947123B - 一种基于视线导引律的无人机路径跟踪与自主避障方法 - Google Patents

Abstract

本发明公开了一种基于视线导引律的无人机路径跟踪与自主避障方法，包括：1）航迹跟踪控制：无人机根据预先设计航路信息，完成航迹跟踪控制；采用圆弧转弯实现航点平滑切换，减小跟踪误差；增加高度跟踪，实现无人机三维航迹跟踪飞行；2）在线自主避障：对障碍物进行在线检测与预估，将不规则障碍物的预警边界利用一个或多个圆柱构成，利用圆心、半径和高度对障碍物进行数学建模，简化避障算法设计；无人机在飞行过程中实时检测障碍物，在线自主生成避障临时航路，无人机按照规避航路实现对障碍物的绕障飞行。本发明方法具有较好的控制性能，可任意跟踪期望航路并完成在线自主障碍物规避，实现无人机的自主飞行。

Description

一种基于视线导引律的无人机路径跟踪与自主避障方法

技术领域

本发明涉及无人机路径跟踪和自主避障技术领域，特别是一种基于视线导引律的无人机路径跟踪与自主避障方法。

背景技术

无人机以其独特的外形、结构及广阔的应用前景，成为当前的研究热点。自主飞行导航系统是无人机最重要的部分之一，直接决定了无人机的飞行性能。但由于无人机航路中环境的未知性、障碍物的存在等因素限制了无人机的自主飞行。在这种情况下，无人机导航控制系统需具备一定程度的路径跟踪与自主避障适应性。

因此设计一种简单、高效、稳健的路径跟踪与自主避障方法，以降低系统的复杂度、增加系统的稳定性、提高导航飞行的精确性、获得满意的导航控制效果，显得尤为重要。

发明内容

本发明所要解决的技术问题是克服现有技术的不足而提供一种基于视线导引律的无人机路径跟踪与自主避障方法，本发明方法具有较好的控制性能，简单高效，可任意跟踪期望航路并完成在线自主障碍物规避，实现无人机的自主飞行，提高导航飞行的精确性、获得满意的导航控制效果。

本发明为解决上述技术问题采用以下技术方案：

根据本发明提出的一种基于视线导引律的无人机路径跟踪与自主避障方法，包括以下步骤：

步骤1、无人机按照设计航路跟踪目标航路，实时进行航点更新判断，完成航迹跟踪控制；

步骤2、针对航点高度不同，加入高度跟踪控制，实现三维路径跟踪；

步骤3、无人机自主避障方法：对障碍物进行在线建模，将不规则障碍物预警边界利用一个或多个圆柱构成，通过圆心、半径和高度对障碍物进行数学建模，简化避障算法设计；无人机在路径跟踪飞行过程中实时进行障碍物检测，当检测到障碍物时，在线自主生成避障航路，无人机按照规避航路实现对障碍物的绕障飞行。

作为本发明所述的一种基于视线导引律的无人机路径跟踪与自主避障方法进一步优化方案，步骤1中采用圆弧方法完成航路转弯飞行。

作为本发明所述的一种基于视线导引律的无人机路径跟踪与自主避障方法进一步优化方案，步骤1中航迹跟踪控制包括无人机直线段航路跟踪控制和无人机圆弧跟踪控制：

无人机直线段航路跟踪控制所需的侧向加速度a_sl表达式为：

其中，V是无人机地速，L₁为视线距离，表示无人机到目标航路上的参考点距离，d为侧偏距，k_l为侧偏距增益，η是无人机地速矢量与L₁矢量的夹角，其表达式为：

其中，

为无人机地速矢量，

为视线距离矢量；

无人机圆弧跟踪控制所需的侧向加速度a_sc表达式为：

其中k_p，k_d分别为比例、微分增益，R为圆弧段转弯半径，

为侧偏距的微分量；

由侧向加速度即得到无人机飞行所需的滚转角控制信号，其中侧向加速度a_s与滚转角控制信号φ_c的关系为：

φ_c＝arctan(a_s/g)+k_i∫ddt (4)

其中g为重力加速度，k_i为积分增益，侧向加速度a_s是a_sl和a_sc的统称，t为积分时间。

作为本发明所述的一种基于视线导引律的无人机路径跟踪与自主避障方法进一步优化方案，步骤2中高度跟踪控制的无人机高度控制信号h_c表达式为：

h_c＝h₁+k_htanγd₁cosλ (5)

其中h₁为航点WP₁的高度，k_h为高度增益，γ为航迹倾斜角，d₁为无人机与航点WP₁在水平面上的投影距离，λ为无人机航迹与航点向量

在水平面上的投影夹角。

作为本发明所述的一种基于视线导引律的无人机路径跟踪与自主避障方法进一步优化方案，无人机自主避障方法具体包括如下步骤：

1)确定前视探测线长度R_L，其表达式为：

其中R_N为障碍物半径，R_min无人机最小转弯半径，V无人机地速，τ₁为开始转弯时无人机滚转角到达最大滚转角φ_max的时间；

2)判断规避方向：无人机与障碍物圆心点的连线与无人机的飞行方向的夹角Δ的表达式为：

Δ＝ψ_N-ψ (7)

其中ψ_N为无人机与障碍物圆心点连线与正北方向的夹角，ψ为无人机的偏航角；当Δ＞0，表示无人机航路方向指向障碍物中心左侧，无人机将沿障碍物左侧规避飞行；当Δ＜0，表示无人机航路方向指向障碍物中心右侧，无人机将沿障碍物右侧规避飞行；

3)进入避障飞行模态：当前视探测线末端进入或触及到障碍物时，即：

D_PN≤R_L+R_N (8)

其中D_PN为无人机到障碍物圆心的距离，无人机实时在线生成半径为R_S圆形规避航路，进入避障飞行模态，实施规避机动飞行；

无人机进入避障飞行模态后，首先在圆形规避航路上选择无人机参考点T，参考点在无人机前方L₁距离处；无人机与障碍物圆心点的连线OP与无人机与参考点的连线TP之间的夹角α的表达式为：

其中|OP|为无人机到障碍物圆心点的距离，无人机与障碍物圆心点的连线OP与正北方向的夹角ψ_N的表达式为：

其中O_E、O_N为障碍物圆心点坐标，P_E、P_N为无人机坐标；如果无人机沿右侧规避障碍物，即Δ＜0，则ψ_a＝ψ_N+α；反之，ψ_a＝ψ_N-α，ψ_a为期望航迹角；则得到参考点T的坐标表达式为：

其中，T_N为参考点的北向分量，T_E为参考点的东向分量；

然后计算出视线导引律所需的角度η_a为：

4)搜索可达航点：在规避飞行过程中，不断检测期望航点是否可达，如果期望的航点与障碍物圆心点的距离小于规避航路半径，则表示期望点被障碍物阻挡，不可到达，将继续判断期望航点的下一航点，直至检测到有可达的航点；

5)退出避障飞行模态：当无人机搜索到可达航点后，不断检测是否达到退出避障飞行模态条件；当无人机与可达航点的连线与无人机与障碍物圆心点的连线的夹角Λ＞90°时，无人机退出避障飞行模态，直线飞向可达航点。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

(1)通过视线导引律计算无人机导航跟踪所需侧向加速度，加入侧偏距补偿，提高路径跟踪性能；

(2)采用圆弧跟踪算法跟新航点，提高飞行平滑性，并加入侧偏距比例微分控制，减小跟踪误差；

(3)加入高度跟踪，实现三维路径跟踪；

(4)对障碍物进行建模，简化了避障方法的研究，实时规划规避航路，提高避障的有效性。

附图说明

图1为无人机路径跟踪与自主避障流程图；

图2为视线导引律几何示意图；

图3为航点切换几何示意图；

图4为圆弧航路跟踪几何示意图；

图5为无人机重力与升力的平衡关系示意图；

图6为高度跟踪几何示意图；

图7为无人机避障流程图；

图8为障碍物规避几何示意图；

图9为无人机平面飞行轨迹仿真结果；

图10为无人机东北天坐标系下飞行轨迹仿真结果。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图及具体实施例对本发明进行详细描述。

本实施例描述了一种基于视线导引律的无人机路径跟踪与自主避障方法，其流程图如图1所示。无人机根据预先设计航路信息，通过本发明方法得到无人机路径跟踪所需的滚转角控制信号φ_c和高度控制信号h_c，实现无人机三维路径跟踪。无人机路径跟踪过程中实时检测障碍物，当检测到障碍物时，在线自主生成避障临时航路，无人机按照规避航路实现对障碍物的绕障飞行。具体实施方法如下：

直线跟踪

如图2所示为视线导引律几何示意图，在无人机前方目标路径上选择参考点M，使其与无人机的直线距离为L₁。由视线导引律得到驱动无人机到达参考点的侧向加速度a_sl，其表达式为：

其中V是无人机地速，d为侧偏距，k_l为侧偏距增益，加入侧偏距补偿，增加无人机跟踪飞行稳定性，并减小超调，η是无人机地速矢量与线段L₁的夹角，其表达式为：

其中，

为无人机地速矢量，

为视线距离矢量。当到达参考点后，无人机以相同的方法不断的在目标路径上选择距无人机L₁的参考点达到航路直线跟踪目的。

航点切换策略

为解决航点切换时不光滑问题，本发明采用圆弧跟踪算法实现航点平滑切换，如图3所示为航点切换几何示意图。航点切换步骤如下：

1)判断圆弧跟踪方向。

分别为东北天(ENU)坐标系下航点WP₂到航点WP₁、航点WP₂到航点WP₃的向量，令

当ccw＞0时，无人机顺时针沿圆弧飞行；当ccw＜0时，无人机逆时针沿圆弧飞行；当ccw＝0时，则判定三个航点位于同一条直线上，无人机继续直线跟踪。

2)进入圆弧转弯判断。航点A为圆弧航路的进入点，B为圆弧航路的退出点，进入圆弧航路的判断条件是

变号，其中，C为圆弧的圆心坐标，

为圆心到飞机当前位置的矢量，

为圆心到圆弧起始点A的矢量，由AB段圆弧组成的圆心角为β，从而可以得出圆心C到航点WP₂的距离为R_min/sin(β/2)，R_min为无人机最小转弯半径，其表达式为：

其中φ_max为无人机最大滚转角，g为重力加速度。夹角β的表达式为：

其中

分别为

的单位向量。A点为目标航点航路上一点，A点到航点WP₂的距离d_A表达式为：

其中τ₁为开始转弯时无人机滚转角到达最大滚转角所需时间，该项与无人机地速乘积可得进入圆弧转弯时延断无人机的飞行距离

3)计算圆弧跟踪所需侧向加速度，进行圆弧跟踪平滑切换航点。如图4所示为圆弧航路跟踪几何示意图，圆弧跟踪的侧向加速度的表达式为：

其中k_p，k_d分别为侧偏距的比例和微分系数，

分别为侧偏距及其微分，提高了跟踪精度和抗干扰性能，R为圆弧目标路径的半径。

4)退出圆弧转弯判断。结束圆弧飞行的判断条件是

变号，其中，

表示圆心到无人机当前位置矢量，

为圆心到圆弧退出点的矢量，，退出点B到航点WP₂的距离d_B表达式为：

其中τ₂为最大滚转角到达直线航路飞行时无人机滚转角所需时间，该项与无人机地速乘积可得退出圆弧转弯时延断无人机的飞行距离。

滚转角控制信号

如图5所示为无人机重力与升力的平衡关系示意图，假设无人机水平直线定高匀速飞行时，此时机翼产生的升力F_L抵消了无人机的重力mg，当无人机转弯时，产生一个滚转角φ，升力提供了一个侧向分量，使无人机产生侧向加速度。所以侧向加速度a_s(a_sl和a_sc统称)和滚转角控制信号φ_c的关系为：

φ_c＝arctan(a_s/g)+k_i∫ddt (9)

其中k_i为积分增益。加入积分补偿提高了无人机路径跟踪抗干扰能力。

高度控制信号

如图6所示为高度跟踪几何示意图，由图可得高度控制信号h_c为：

h_c＝h₁+k_htanγd₁cosλ (10)

其中h₁为航点WP₁的高度，k_h为高度增益，γ为航迹倾斜角，d₁为无人机与航点WP₁在水平面上的投影距离，λ为无人机航迹与航点向量

在水平面上的投影夹角。由于无人机会通过圆弧跟踪飞行切换航点，所以引入高度增益进行补偿，防止高度出现跳变现象，高度增益k_h表达式为：

其中|WP₁,WP₂|为两航点距离。

避障策略

如图7所示为无人机避障流程图。本发明自主避障方法对障碍物进行在线建模，将复杂形状的障碍物看成一个或多个圆柱，利用圆心、半径和高度描述障碍物，简化避障算法设计；无人机在路径跟踪飞行过程中实时进行障碍物检测，当检测到障碍物时，在线自主生成避障航路，无人机按照规避航路实现对障碍物的绕障飞行。

如图8所示为障碍物规避几何示意图。障碍物规避步骤如下：

1)确定前视探测线长度R_L。当无人机以最小转弯半径R_min转弯，航路与障碍物相切时，可得最小前视探测距离R_Lmin，其表达式为：

其中R_N为障碍物半径。为获得最终的R_L的值，必须对开始转弯到达最大滚转角φ_max所需时间τ₁进行补偿，所需时间τ₁乘以无人机的地速V，就可以得到时延段无人机的运动距离，所以前视探测线长度R_L的表达式为：

R_L＝R_Lmin+Vτ₁ (13)

2)判断规避方向。无人机规避障碍物的方向取决于障碍物圆心位于航路的哪一侧，如果无人机飞行航路方向指向障碍物中心的左侧，无人机将沿障碍物左侧实施规避飞行，反之，沿障碍物右侧规避飞行。无人机的飞行方向与无人机与障碍物圆心点连线的夹角Δ的表达式为：

Δ＝ψ_N-ψ (14)

其中ψ_N为无人机与障碍物圆心连线与正北方向的夹角，ψ为无人机的偏航角，当Δ＞0，表示无人机航路方向指向障碍物中心左侧，无人机将沿障碍物左侧规避飞行；当Δ＜0，表示无人机航路方向指向障碍物中心右侧，无人机将沿障碍物右侧规避飞行。

3)进入避障飞行模态。当前视探测线进入或者触及到障碍物时，即无人机到障碍物圆心的距离D_PN≤R_L+R_N，无人机实时在线生成半径为R_S的圆形规避航路，进入避障飞行模态，实施规避机动飞行。选择圆形规避航路因为它可以很容易地由障碍物的大小得到，实际上它是由障碍物的圆心和航路半径R_S确定，R_S由障碍物半径R_N和安全飞行裕量Lsa_fe构成(根据无人机飞行特性确定)，无人机跟踪这个航路飞行，直到以直线方式朝期望航点飞行，并不会穿越障碍物。圆形规避航路半径表达式为：

R_S＝max(R_min,R_N+L_safe) (15)

无人机进入避障飞行模态后，首先在圆形规避航路上选择无人机参考点T，参考点在无人机前方L₁距离处。无人机与障碍物圆心点的连线OP与无人机与参考点的连线TP之间的夹角α的表达式为：

其中|OP|为无人机到障碍物圆心点的距离。无人机与障碍物圆心点的连线OP与正北方向的夹角ψ_N的表达式为：

其中O_E、O_N为障碍物圆心点坐标，P_E、P_N为无人机坐标。如果无人机沿右侧规避障碍物，即Δ＜0，则ψ_a＝ψ_N+α；反之，ψ_a＝ψ_N-α，ψ_a为期望航迹角。则可以得到参考点T的坐标表达式为：

其中，T_N为参考点的北向分量，T_E为参考点的东向分量。然后计算出视线导引律所需的角度η_a为：

4)搜索可达航点。在规避飞行过程中，不断检测期望航点是否可达，如果期望的航点与障碍物圆心点的距离小于规避航路半径，则表示期望点被障碍物阻挡，不可到达，将继续判断期望航点的下一航点，直至检测到有可达的航点。

5)退出避障飞行模态。当无人机搜索到可达航点后，不断检测是否达到退出避障飞行模态条件。当无人机与可达航点的连线与无人机与障碍物圆心点的连线的夹角Λ＞90°时，无人机退出避障飞行模态，直线飞向可达航点。

仿真试验验证

为验证本发明提出的路径跟踪与自主避障方法的有效性，在已有的某型固定翼无人机非线性模型上进行仿真试验。根据无人机的飞行性能，设置仿真参数如下：无人机的最大滚转角φ_max＝30°，无人机巡航速度V_t＝50m/s，视线距离L₁＝300m，安全飞行裕量L_safe＝100m。仿真结果如图9～10所示，图9为无人机水平面飞行轨迹，图中阴影部分是实际障碍物，可以看出无人机可以跟踪预设计的航路完成航迹跟踪飞行，并对障碍物实施有效地规避飞行，当满足绕障飞行退出条件后，无人机退出避障模态并自主切入航路的下一目标航点。图10为无人机东北天坐标系下三维飞行轨迹，从图中结果可以看出无人机很好的跟踪上目标航迹并实施障碍物规避飞行，验证了本发明所提出的路径跟踪和自主避障方法的有效性。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围内。

Claims (4)

1.一种基于视线导引律的无人机路径跟踪与自主避障方法，其特征在于，包括以下步骤：

步骤1、无人机按照设计航路跟踪目标航路，实时进行航点更新判断，完成航迹跟踪控制；

步骤2、针对航点高度不同，加入高度跟踪控制，实现三维路径跟踪；

步骤3、无人机自主避障方法：对障碍物进行在线建模，将不规则障碍物预警边界利用一个或多个圆柱构成，通过圆心、半径和高度对障碍物进行数学建模，简化避障算法设计；无人机在路径跟踪飞行过程中实时进行障碍物检测，当检测到障碍物时，在线自主生成避障航路，无人机按照规避航路实现对障碍物的绕障飞行；

无人机自主避障方法具体包括如下步骤：

1)确定前视探测线长度R_L，其表达式为：

其中R_N为障碍物半径，R_min无人机最小转弯半径，V无人机地速，τ₁为开始转弯时无人机滚转角到达最大滚转角φ_max的时间；

2)判断规避方向：无人机与障碍物圆心点的连线与无人机的飞行方向的夹角Δ的表达式为：

Δ＝ψ_N-ψ (2)

其中ψ_N为无人机与障碍物圆心点连线与正北方向的夹角，ψ为无人机的偏航角；当Δ＞0，表示无人机航路方向指向障碍物中心左侧，无人机将沿障碍物左侧规避飞行；当Δ＜0，表示无人机航路方向指向障碍物中心右侧，无人机将沿障碍物右侧规避飞行；

3)进入避障飞行模态：当前视探测线末端进入或触及到障碍物时，即：

D_PN≤R_L+R_N (3)

其中D_PN为无人机到障碍物圆心的距离，无人机实时在线生成半径为R_S圆形规避航路，进入避障飞行模态，实施规避机动飞行；

无人机进入避障飞行模态后，首先在圆形规避航路上选择无人机参考点T，参考点在无人机前方L₁距离处；无人机与障碍物圆心点的连线OP与无人机与参考点的连线TP之间的夹角α的表达式为：

其中|OP|为无人机到障碍物圆心点的距离，无人机与障碍物圆心点的连线OP与正北方向的夹角ψ_N的表达式为：

其中O_E、O_N为障碍物圆心点坐标，P_E、P_N为无人机坐标；如果无人机沿右侧规避障碍物，即Δ＜0，则ψ_a＝ψ_N+α；反之，ψ_a＝ψ_N-α，ψ_a为期望航迹角；则得到参考点T的坐标表达式为：

其中，T_N为参考点的北向分量，T_E为参考点的东向分量；

然后计算出视线导引律所需的角度η_a为：

4)搜索可达航点：在规避飞行过程中，不断检测期望航点是否可达，如果期望的航点与障碍物圆心点的距离小于规避航路半径，则表示期望点被障碍物阻挡，不可到达，将继续判断期望航点的下一航点，直至检测到有可达的航点；

5)退出避障飞行模态：当无人机搜索到可达航点后，不断检测是否达到退出避障飞行模态条件；当无人机与可达航点的连线与无人机与障碍物圆心点的连线的夹角Λ＞90°时，无人机退出避障飞行模态，直线飞向可达航点。

2.根据权利要求1所述的一种基于视线导引律的无人机路径跟踪与自主避障方法，其特征在于，步骤1中采用圆弧方法完成航路转弯飞行。

3.根据权利要求1所述的一种基于视线导引律的无人机路径跟踪与自主避障方法，其特征在于，步骤1中航迹跟踪控制包括无人机直线段航路跟踪控制和无人机圆弧跟踪控制：

无人机直线段航路跟踪控制所需的侧向加速度a_sl表达式为：

其中，V是无人机地速，L₁为视线距离，表示无人机到目标航路上的参考点距离，d为侧偏距，k_l为侧偏距增益，η是无人机地速矢量与L₁矢量的夹角，其表达式为：

其中，

为无人机地速矢量，

为视线距离矢量；

无人机圆弧跟踪控制所需的侧向加速度a_sc表达式为：

其中k_p，k_d分别为比例、微分增益，R为圆弧段转弯半径，

为侧偏距的微分量；

由侧向加速度即得到无人机飞行所需的滚转角控制信号，其中侧向加速度a_s与滚转角控制信号φ_c的关系为：

φ_c＝arctan(a_s/g)+k_i∫ddt (11)

其中g为重力加速度，k_i为积分增益，侧向加速度a_s是a_sl和a_sc的统称，t为积分时间。

4.根据权利要求1所述的一种基于视线导引律的无人机路径跟踪与自主避障方法，其特征在于，步骤2中高度跟踪控制的无人机高度控制信号h_c表达式为：

h_c＝h₁+k_htanγd₁cosλ (12)

其中h₁为航点WP₁的高度，k_h为高度增益，γ为航迹倾斜角，d₁为无人机与航点WP₁在水平面上的投影距离，λ为无人机航迹与航点向量

在水平面上的投影夹角。

Images