CN112241178A - 一种无人机自主安全控制方法 - Google Patents

Abstract

本发明公开了一种无人机自主安全控制方法，当无人机规划路径经过保护区域时，若直线路径段与保护域相交，采用中间航路点法生成同向旋转解与反向旋转解，以无人机位置、中间航路点、待飞航点进行路径重规划；若圆弧路径段与保护域相交，若圆弧路径与保护域相交，采用调整运动圆弧曲率的方法计算与保护域相切的点规避保护域；若重规划后的路径与圆弧段或者直线段相交，通过迭代产生避开保护域的路径。本发明使无人机具备自主安全控制处置能力，实现对航区内预定目标的区域保护以及飞行边界的自主判断与处置。一旦无人机因为失控或其他原因导致越出安全区域或飞往保护区域，按照预定的策略自动输出控制信号，最大程度保证地面设备和人员的安全。

Description

一种无人机自主安全控制方法

技术领域

本发明属于飞行器安全控制的技术领域，具体涉及一种无人机自主安全控制方法。

背景技术

无人机飞行过程中，一旦出现故障造成无人机失控将对周边目标构成重大威胁，甚至可能出现越出国境线，造成重大政治及外交影响。因此当无人机在执行飞行任务时，根据无人机执行任务区域、外界环境和航路要求，将无人机的飞行范围限制在一块提前规划好的安全区域内，该区域的选定除了包含无人机飞行航区的飞行边界外，同时尽可能避开人口稠密区及重要经济军事目标等保护区域。

一旦无人机在飞行过程中失去控制，坠毁在地面或海上，将对周边区域的人员生命与财产安全构成威胁，若越出国境线，则可能造成恶劣的政治影响。无人机自主安全控制方法使无人机具备自主安全控制(简称安控)处置能力，实现对航区内预定目标的区域保护以及飞行边界的自主判断与处置。一旦无人机因为失控或其他原因导致越出安全区域或飞往保护区域，按照预定的策略自动输出控制信号，最大程度保证地面设备和人员的安全。

2019年，陶德桂提出了一种自主安控器模块设计方法，通过计算有序排列的相邻向量叉乘实现凸形区域的自主判断。实际空域中，边界线不完全为凸形域，针对边界为凹形空域的自主越界判断控制鲜少研究，本发明提出一种无人机自主安全控制方法，在此基础上，对于边界区域内存在不可飞区域的路径相交检测与规避进行了补充研究。

发明内容

本发明的目的在于提供一种无人机自主安全控制方法，使无人机具备自主安全控制处置能力，实现对航区内预定目标的区域保护以及飞行边界的自主判断与处置。一旦无人机因为失控或其他原因导致越出安全区域或飞往保护区域，按照预定的策略自动输出控制信号，最大程度保证地面设备和人员的安全。

在此基础上，对于边界区域内存在不可飞区域的路径相交检测与规避进行了补充研究。本发明提出的自主安控模块设计方法适用于多类型空域，并且具备预警功能，多类型空域比如凹凸边界域、单连通、多连通域。

本发明主要通过以下技术方案实现：一种无人机自主安全控制方法，当无人机规划路径经过保护区域时，判断相交路径为直线路径或圆弧路径，若直线路径段与保护域相交，采用中间航路点法生成同向旋转解与反向旋转解，以无人机位置、中间航路点、待飞航点进行路径重规划；若圆弧路径段与保护域相交，若圆弧路径与保护域相交，采用调整运动圆弧曲率的方法计算与保护域相切的点规避保护域；若重规划后的路径与圆弧段或者直线段相交，通过迭代产生避开保护域的路径。

为了更好地实现本发明，进一步地，当无人机位置位于保护域的高度区间内时，若所规避对象的类型属性为柱形，则以保护域最大半径为保护圆半径，否则，以保护域在无人机高度平面的水平投影圆半径叠加安全距离为保护圆半径；以保护圆半径为路径重规划所用的安全圆半径。

为了更好地实现本发明，进一步地，直线路径与保护域相交判据：保护域中心点距离直线路径段的距离小于保护域半径，且交点至少有一个点在直线路径段上。

为了更好地实现本发明，进一步地，采用中间航路点算法生成中间航路点，若中间航路点所在圆弧半径小于最小转弯半径，则以最小转弯半径为重规划路径圆弧段半径，否则，以中间航路点所在圆弧半径作为重规划路径圆弧段半径。

为了更好地实现本发明，进一步地，圆弧路径与保护域相交判据：保护圆心距圆弧路径段圆心的距离＜保护圆半径+圆弧路径半径，保护圆心距圆弧路径段圆心的距离＞|保护圆半径-圆弧路径半径|，且至少一个交点在圆弧路径段内。

为了更好地实现本发明，进一步地，通过调整曲率方法生成内切解与外切解，若内切线解对应的圆弧半径大于最小转弯半径，则以内切线解半径为重规划路径的圆弧段半径，否则，若外切线解圆弧对应的半径大于最小转弯半径，则以外切线解半径为重规划路径的圆弧段半径，否则，以最小转弯半径为重规划路径的圆弧段半径。

为了更好地实现本发明，进一步地，保护域的圆心为(x_b,y_b)，曲率为k_b，半径为r_b＝1/k_b，保护域中心点到直线段起始点的距离为d₀，保护域中心点到直线段的垂直投影线为d_n；直线段与保护域相交的判据为d_n≤r_b，且p_ip及p_im落在直线路径区间。

为了更好地实现本发明，进一步地，若直线路径与保护域相交，采用生成中间航路点的方法计算与保护域的安全圆相切的点；记起点为P_s，终点为P_f，C_safe为保护域的安全圆；从P_s至P_f到的路径参数为r(t)，r(t)与C_safe相交，交点为X₁,X₂，X₁为进入点，X₂为退出点，做一条与X₁,X₂连线垂直的直线，交C_safe于M,N，M,N即为新生成的中间航路点；

直线路径的切入点为(x_px,y_px)，切出点为(x_pn,y_pn)，直线路径的单位方向向量为

(x_px,y_px)与(x_3,4,y_3,4)的单位方向向量为：

令θ_na＝t_xn×t_xa,θ_nm＝t_xn×t_xm，

若θ_na＞0,θ_nm＜0，位于直线路径上方的点p_up为(x₃,y₃)，位于直线路径下方的点p_down为(x₄,y₄)，

若θ_na＜0,θ_nm＞0，位于直线路径上方的点p_up为(x₄,y₄)，位于直线路径下方的点p_down为(x₃,y₃)，

若无人机沿起始位置顺时针旋转，则同向旋转解p_SYN为p_up，反向旋转解p_INV为p_down，

若无人机沿起始位置逆时针旋转，则同向旋转解p_SYN为p_down，反向旋转解p_INV为p_up。

为了更好地实现本发明，进一步地，保护域的圆心为(x_b,y_b)，曲率为k_b，半径为r_b＝1/k_b；圆弧路径的圆心坐标为(x_c,y_c)，曲率为k_c，半径为r_c＝1/k_c，圆弧路径的转弯角为ψ_c，弧段圆心与保护圆心的距离为

记d_cm＝d_c-(r_c+r_b)，d_cp＝d_c+r_b-r_c，运动弧段与保护圆相交的判据则为d_c＜r_c+r_b，d_c＞|r_c-r_b|，即d_cm＜0，d_cp＞0，且保护域落在运动弧段内。

为了更好地实现本发明，进一步地，若圆弧路径与保护域相交，采用调整运动圆弧曲率的方法计算与保护域相切的点规避保护域；

记保护域曲率为k_b，半径为

路径圆弧的曲率为k_f，半径为

目标点为P_f(x_f,y_f)，弧段圆心与保护圆心的距离

终止位置与保护圆心的距离

弧段圆心与终止位置的连线到终止位置与保护域圆心的连线的夹角

最大曲率解与最小曲率解为：

若两组路径解均满足飞行约束，取最短路径；若其中一组不满足飞行约束，则取另一组；若两组均不满足飞行约束，则以无人机最小转弯半径转弯。

为了更好地实现本发明，进一步地，为实现保护域的保护，引入规避类型变量ω，ω＝1表示不同飞行高度平面上，规避圆的半径不同，且等于椭球体投影在当前飞行高度平面的半径+安全距离；ω＝-1表示不同飞行高度平面上，规避圆的半径相同，且等于椭球体投影的最大半径+安全距离。

为了更好地实现本发明，进一步地，记禁飞中心位置为P_prot:(x,y,h,r)，无人机当前高度为h_now，ω＝1，投影圆的半径

ω＝-1，投影圆的半径为r_cir＝r。

为了更好地实现本发明，进一步地，采用射线扫描法判断无人机位置与边界地图之间的关系，假设边界地图由不规则n边形构成，规定逆时针为正，顶点按照逆时针方向排列，依次为P_i(x_i,y_i),i＝0,1,2,…,n-1，过P(x,y)向右作一条水平射线，计算与多边形边界的交点，若交点个数为奇数，则点在多边形内部，若交点个数为偶数，则点在多边形外。

为了更好地实现本发明，进一步地，射线法计算如下：

首先快速剔除矩形外的点，记A＝{0,1,2,…,n-1},n≥3,

若P(x,y)不满足X_min＜x＜X_max,Y_min＜y＜Y_max,则无人机越界；若不满足，对于矩阵内的点，通过点P(x,y)与各边距离判断P(x,y)是否为控制顶点集P_i(x_i,y_i),i＝0,1,2,…,n-1的边点与顶点；对

P(x,y)与各边距离公式如下：

若d满足d＜ε，则P(x,y)为边点，无人机越界；

若对i＝0,1,2,…,n-1，满足

则P(x,y)为顶点，无人机越界；

若P(x,y)非顶点或边点，则通过P(x,y)向右作一条水平射线，计算与多边形边界的交点，

若交点个数为奇数，则点在多边形内部，若交点个数为偶数，则点在多边形外；

对

ε任意小，j＝1,2,…,n-1,n，若满足|y_j-1-y_j|≤ε，则交点个数为0，

若满足|x_j-1-x_j|≤ε，则有一个交点，且交点坐标为x_p＝x_j-1,y_p＝y，

若满足|y_j-1-y_j|＞ε,|x_j-1-x_j|＞ε，则有一个交点，且交点坐标为：

若P(x,y)满足x＜x_p,min(y_j-1,y_j)≤y≤max(y_j-1,y_j)，且交点不为多边形顶点，则交点个数加1，若满足上式且P(x,y)交点为多边形顶点，则同侧顶点交点个数加2，异侧顶点交点个数加1。

为了更好地实现本发明，进一步地，顶点同侧异侧判据如下：

记P_t＝(x_p-x,y_p-y),P_tt＝(x_mod(j+1,n)-x,y_mod(j+1,n)-y),P_tb＝(x_j-1-x,y_j-1-y)，

若满足(P_t×P_tt)(P_t×P_tb)＞0，则交点在同侧，否则交点在异侧。

为了更好地实现本发明，进一步地，设保护域中心点位置为P(x,y)，飞机位置为P'(x',y')，无人机距离保护域的距离为r，保护域的半径为r₀，保护域的安全圆半径为r₁；

若r-r₁＞0，未进入保护域的安全圆；若0＜|r-r₀|＜r₁-r₀，进入保护域的安全圆，未进入保护域；若r-r₀≤0，飞机进入保护域。

安控步骤如下：

第一、自动根据装订的航点进行区域闭合，以装订的航点生成警戒线，采用向量平移变换或线段平移变换实现警戒线的内缩与外扩。

第二、通过地面站指令实现处置程序实时介入。

第三、将任意凹凸多边形区域的无人机警戒线等距内缩生成内缩边界，通过扫描边界线判断无人机位置状态，并设置相应状态标志位，使无人机具备越界告警与越界预警功能。根据状态标志，以预定策略自动输出控制信号，控制无人机执行处置程序。

第四、引入规避类型变量表示不同飞行高度平面上规避圆的半径参数，实现对于保护对象的定参数规避与变参数规避。

第五、依据原规划路径的类型，实现不同类型规划路径与保护域的相交检测与规避路径规划。

本发明内容包括区域管理、策略设计、算法设计。区域管理对禁飞区域进行划分，定义了内部禁飞与外部禁飞概念，对于内部禁飞区域模型，通过引入规避类型参数，实现不同高度平面的变参数自主规避。策略设计提出了越界预警或告警的处置原则。算法设计包括预警线生成、越界判断、相交检测以及规避算法。预警功能通过多边形的收缩实现，越界判断通过射线扫描方法实现，对于内经过仿真验证，本发明提出的设计方法合理可行，基本适应大多数小型无人机安控需求。

1、区域管理

无人机安控区域包括飞行航区与禁飞区，飞行航区为正常状态下最大可飞行区域，禁飞区包括内部禁飞区与外部禁飞区。

如图15所示，本发明中的安控区域如下：

坠毁区：无人机执行处置程序后可能的坠毁散布点形成的带状区域。

警戒线：无人机最大安全飞行区域，顶点位置信息在飞行之前由地面控制设备通过装订航线完成，航线属性为外部区域禁飞，航线共3～127个航路点，每条航线至少3个航点，航点高度为绝对地理高度。地面控制设备可通过遥控指令对航线数据进行维护，具体指令包括：航点装订、编辑航点、航线查询及航点查询。

预警线：装订警戒线数据后，自动生成一条封闭的向内径收缩一定距离的预警线，用以保证无人机预警时的安全返航。

飞行航区：正常状态下最大可飞行区域。

飞行航路：无人机执行任务的航线，在飞行之前装订。

内部禁飞域：覆盖所受保护区域的球，球的半径根据要规避的设施分布范围或居民区大小等具体情况确定。位置信息在飞行之前由地面控制设备通过装订航线完成，航线属性为内部区域禁飞，航线共1～127个航路点，航点高度为绝对地理高度，每1个航点代表1个保护域，对应航点的经度、纬度、高度、半径构成保护域球。

保护域：以保护目标中心点为球心、半径大于内部禁飞域半径的球，保护域由内部禁飞域外扩安全距离得到。

为实现保护域的保护，通常以球体或椭球体或多个球体拼接将保护域进行包裹，通过实现无人机与保护域禁飞圆的碰撞回避完成保护域的保护。实际飞行过程中，由于空域限制或无人机性能限制，需要在保护域在平面投影的临近圆飞行，针对此种情况，引入规避类型变量ω，ω＝1表示不同飞行高度平面上，规避圆的半径不同，取椭球体投影在当前飞行高度平面的半径+安全距离，ω＝-1表示不同飞行高度平面上，规避圆的半径相同，取椭球体投影的最大半径+安全距离，安全距离受到无人机性能限制。

记禁飞中心位置为P_prot:(x,y,h,r)，无人机当前高度为h_now，ω＝1，投影圆的半径

ω＝-1，投影圆的半径为r_cir＝r。

2、控制策略设计

自主安控根据定位信息实时判断无人机当前位置与安控区域间的关系，确定安控状态，满足执行条件或受到地面站指令，按预定的策略自动输出控制信号，操控无人机坠毁在坠毁区。

安控策略设计中需满足一定的自主处置原则，当无人机起飞位置通常位于安全区域内，针对起飞位置可能位于保护域或警戒线外的情况，在无人机起飞位置未进入安全区域前，自主安控不介入，避免执行机构做出错误的动作。无人机飞行过程中，实时判断无人机是否越出边界线及无人机至待飞航点的路径与保护目标间的关系，若越出预警线，设置越预警线告警状态为告警；若越出警戒线，设置越警戒线告警状态为告警；若无人机路径与保护区域相交，设置相交检测状态为相交。

为提高安控模块的可靠性，对安控模块进行实时监控与中断，地面站设置安控指令，当未启用安控指令，自主安控只执行安控判断并下传标志位，当启用安控指令，接入安控处置逻辑，控制无人机在触发安控区域后按照预设策略执行安控程序。安控指令的响应条件为收到有效的“预指令”+“安控指令”，执行内容为收到有效指令奇数次则置“允许安控”状态位为允许，偶数次则置“允许安控”状态位为禁止。

如图6所示，无人机越出预警线时的判据为控制模式处于程控或人工修正，且无人机越出预警线。处置策略取决于安控标志状态。当安控标志为禁止安控，不予处置；当安控标志为自主安控，若飞行阶段位于航行段、指令飞行阶段、应急返航阶段，执行应急返航程序，其他阶段不予处置。若无人机失控，飞行员视情切姿态遥控。

如图7所示，当无人机越出警戒线时的判据为控制模式处于程控或人工修正，无人机越出警戒线。处置策略取决于安控标志状态。当安控标志为禁止安控，不予处置；当安控标志为自主安控，若飞行阶段处于航行段、指令飞行阶段、应急返航阶段，执行迫降程序，其他飞行阶段，不予处置。若无人机失控，飞行员视情切姿态遥控。

当无人机规划路径经过保护区域时，通过判断相交路径为直线路径或圆弧路径进行路径重规划，控制无人机以重规划后路径飞行以避开保护域。若直线路径段与保护域相交，采用中间航路点法生成同向旋转解与反向旋转解，以无人机位置、中间航路点、待飞航点进行路径重规划。若圆弧路径段与保护域相交，通过调整相交圆弧的曲率产生最大曲率路径解与最小曲率路径解。若重规划后的路径与圆弧段或者直线段相交，通过迭代产生避开保护域的路径。

如图8、图9所示，无人机路径与保护域相交的处置如下：

a)判据

直线路径与保护域相交判据：处于程控或人工修正模式，且位置信息有效；保护域中心点距离直线路径段的距离小于保护域半径，且交点至少有一个点在直线路径段上。

圆弧路径与保护域相交判据：处于程控或人工修正模式，且位置信息有效；保护圆心距圆弧路径段圆心的距离＜保护圆半径+圆弧路径半径，保护圆心距圆弧路径段圆心的距离＞|保护圆半径-圆弧路径半径|，且至少一个交点在圆弧路径段内。

b)处置

1)无人机位置未落于保护域的高度区间内，不予处置；

2)无人机位置位于保护域的高度区间内

i.无人机以判断到相交时刻的高度定高平飞；

ii.若所规避对象的类型属性为柱形，以保护域最大半径为保护圆半径，否则，以保护域在无人机高度平面的水平投影圆半径叠加安全距离为保护圆半径；

iii.以保护圆半径为路径重规划所用的安全圆半径，直线路径与保护域相交时采用中间航路点算法生成中间航路点，若中间航路点所在圆弧半径小于最小转弯半径，以最小转弯半径为重规划路径圆弧段半径，否则，以中间航路点所在圆弧半径作为重规划路径圆弧段半径。圆弧路径与保护域相交时通过调整曲率方法生成内切解(最大曲率解)与外切解(最小曲率解)，内切线解对应的圆弧半径大于最小转弯半径，则以内切线解半径为重规划路径的圆弧段半径，否则，若外切线解圆弧对应的半径大于最小转弯半径，则以外切线解半径为重规划路径的圆弧段半径，否则，以最小转弯半径为重规划路径的圆弧段半径；

iv.满足路径重规划窗口或收到地面站发送的接入路径重规划指令，生成重规划路径，控制无人机沿着重规划路径飞行。

c)退出条件

待飞航点切换后退出路径重规划。

3、算法设计

安控算法设计包括预警线生成、越界判断、与保护域的相交检测以及规避。

1.1预警线生成

设警戒线的顶点个数为n，顶点集为P_i(x_i,y_i),i＝0,1,2,…,n-1，始点与终点相连构成闭合多边形，预警线生成采用几何变换的方法，将警戒线各边P_iP_mod(i+1,n)沿着内径法向量平移距离L，平移后的边为P_i'P′_mod(i+1,n),相邻两边的交点为P_i'(x_i',y_i'),P_i'即为安控预警线的顶点集。

对于j＝2,3,…,n+1，计算多边形各边的单位内法向量N_v(j-1)。通过判断N_v(j-1)的方向，若N_v(j-1)为外法向量，取反向量。将多边形各边沿单位内法向量N_v(j-1)平移距离L生成新边，计算相邻直线交点即可得到预警线顶点集。

1.2越界判断

假设边界地图由不规则n边形构成，规定逆时针为正，顶点按照逆时针方向排列，依次为P_i(x_i,y_i),i＝0,1,2,…,n-1，无人机位置为P(x,y)，采用射线扫描法判断无人机位置与边界地图之间的关系。过P(x,y)向右作一条水平射线，计算与多边形边界的交点，若交点个数为奇数，则点在多边形内部，若交点个数为

偶数，则点在多边形外。

1.3相交检测

设保护域中心点位置为P(x,y)，飞机位置为P'(x',y')，无人机距离保护域的距离为r，保护域的半径为r₀，保护域的安全圆半径为r₁。

若r-r₁＞0，未进入保护域的安全圆；若0＜|r-r₀|＜r₁-r₀，进入保护域的安全圆，未进入保护域；若r-r₀≤0，飞机进入保护域。

记保护域的圆心为(x_b,y_b)，保护域曲率为k_b，保护域半径为r_b＝1/k_b，保护域中心点到直线段起始点的距离为d₀，保护域中心点到直线段的垂直投影线为d_n。直线段与保护域相交的判据为d_n≤r_b，且p_ip及p_im落在直线路径区间。

保护域的圆心为(x_b,y_b)，曲率为k_b，半径为r_b＝1/k_b，圆弧路径的圆心坐标为(x_c,y_c)，曲率为k_c，半径为r_c＝1/k_c，圆弧路径的转弯角为ψ_c，弧段圆心与保护圆心的距离为

记d_cm＝d_c-(r_c+r_b)，d_cp＝d_c+r_b-r_c，运动弧段与保护圆相交的判据则为d_c＜r_c+r_b，d_c＞|r_c-r_b|，即d_cm＜0，d_cp＞0，且保护域落在运动弧段内。

1.4规避算法设计

若直线路径与保护域相交，采用生成中间航路点的方法计算与保护域的安全圆相切的点。

不妨记起点为P_s，终点为P_f，C_safe为保护域的安全圆。从P_s至P_f到的路径参数为r(t)，r(t)与C_safe相交，交点为X₁,X₂，X₁为进入点，X₂为退出点，做一条与X₁,X₂连线垂直的直线，交C_safe于M,N，M,N即为新生成的中间航路点。

直线路径的切入点为(x_px,y_px)，切出点为(x_pn,y_pn)，直线路径的单位方向向量为

(x_px,y_px)与(x_3,4,y_3,4)的单位方向向量为

令θ_na＝t_xn×t_xa,θ_nm＝t_xn×t_xm，若θ_na＞0,θ_nm＜0，位于直线路径上方的点p_up为(x₃,y₃)，位于直线路径下方的点p_down为(x₄,y₄)，若θ_na＜0,θ_nm＞0，位于直线路径上方的点p_up为(x₄,y₄)，位于直线路径下方的点p_down为(x₃,y₃)。若无人机沿起始位置顺时针旋转，则同向旋转解p_SYN为p_up，反向旋转解p_INV为p_down，若无人机沿起始位置逆时针旋转，则同向旋转解p_SYN为p_down，反向旋转解p_INV为p_up。

若圆弧路径与保护域相交，采用调整运动圆弧曲率的方法计算与保护域相切的点规避保护域。

记保护域曲率为k_b，半径为

路径圆弧的曲率为k_f，半径为

目标点为P_f(x_f,y_f)，弧段圆心与保护圆心的距离

终止位置与保护圆心的距离

弧段圆心与终止位置的连线到终止位置与保护域圆心的连线的夹角

最大曲率解与最小曲率解为

若两组路径解均满足飞行约束，取最短路径；若其中一组不满足飞行约束，则取另一组；若两组均不满足飞行约束，则以无人机最小转弯半径转弯。

本发明的有益效果：

(1)本发明使无人机具备自主安全控制处置能力，实现对航区内预定目标的区域保护以及飞行边界的自主判断与处置。

(2)一旦无人机因为失控或其他原因导致越出安全区域或飞往保护区域，按照预定的策略自动输出控制信号，本发明可以最大程度保证地面设备和人员的安全。

(3)实际空域中，边界线不完全为凸形域，本发明通过射线法判断无人机位置与边界地图之间的关系，解决了边界为凹形空域的自主越界判断的技术问题。

附图说明

图1为射线法示意图；

图2为越界算法仿真示意图；

图3为保护域与直线路径相交示意图；

图4为圆弧段与保护域相交示意图；

图5为中间航点算法示意图；

图6为越预警线处置逻辑图；

图7为越警戒线处置逻辑图；

图8为无人机直线路径与保护域相交处置逻辑图；

图9为无人机圆弧路径与保护域相交处置逻辑图；

图10为无人机路径未与保护域相交示意图；

图11为直线路径与保护域相交的路径重规划；

图12为圆弧路径与保护域相交的路径重规划；

图13为结合越界与规避保护域的路径航迹；

图14为保护域同时与直线路径、圆弧路径相交的迭代路径重规划；

图15为本发明的安控区域示意图。

具体实施方式

实施例1：

一种无人机自主安全控制方法，当无人机规划路径经过保护区域时，判断相交路径为直线路径或圆弧路径，若直线路径段与保护域相交，采用中间航路点法生成同向旋转解与反向旋转解，以无人机位置、中间航路点、待飞航点进行路径重规划；若圆弧路径段与保护域相交，若圆弧路径与保护域相交，采用调整运动圆弧曲率的方法计算与保护域相切的点规避保护域；如图14所示，若重规划后的路径与圆弧段或者直线段相交，通过迭代产生避开保护域的路径。

实施例2：

本实施例是在实施例1的基础上进行优化，当无人机位置位于保护域的高度区间内时，若所规避对象的类型属性为柱形，则以保护域最大半径为保护圆半径，否则，以保护域在无人机高度平面的水平投影圆半径叠加安全距离为保护圆半径；以保护圆半径为路径重规划所用的安全圆半径。

本实施例的其他部分与实施例1相同，故不再赘述。

实施例3：

本实施例是在实施例1或2的基础上进行优化，直线路径与保护域相交判据：保护域中心点距离直线路径段的距离小于保护域半径，且交点至少有一个点在直线路径段上。

进一步地，采用中间航路点算法生成中间航路点，若中间航路点所在圆弧半径小于最小转弯半径，则以最小转弯半径为重规划路径圆弧段半径，否则，以中间航路点所在圆弧半径作为重规划路径圆弧段半径。

进一步地，保护域的圆心为(x_b,y_b)，曲率为k_b，半径为r_b＝1/k_b，保护域中心点到直线段起始点的距离为d₀，保护域中心点到直线段的垂直投影线为d_n；直线段与保护域相交的判据为d_n≤r_b，且p_ip及p_im落在直线路径区间。

进一步地，如图11所示，若直线路径与保护域相交，采用生成中间航路点的方法计算与保护域的安全圆相切的点；记起点为P_s，终点为P_f，C_safe为保护域的安全圆；从P_s至P_f到的路径参数为r(t)，r(t)与C_safe相交，交点为X₁,X₂，X₁为进入点，X₂为退出点，做一条与X₁,X₂连线垂直的直线，交C_safe于M,N，M,N即为新生成的中间航路点；

直线路径的切入点为(x_px,y_px)，切出点为(x_pn,y_pn)，直线路径的单位方向向量为

(x_px,y_px)与(x_3,4,y_3,4)的单位方向向量为：

令θ_na＝t_xn×t_xa,θ_nm＝t_xn×t_xm，

若θ_na＞0,θ_nm＜0，位于直线路径上方的点p_up为(x₃,y₃)，位于直线路径下方的点p_down为(x₄,y₄)，

若θ_na＜0,θ_nm＞0，位于直线路径上方的点p_up为(x₄,y₄)，位于直线路径下方的点p_down为(x₃,y₃)，

若无人机沿起始位置顺时针旋转，则同向旋转解p_SYN为p_up，反向旋转解p_INV为p_down，若无人机沿起始位置逆时针旋转，则同向旋转解p_SYN为p_down，反向旋转解p_INV为p_up。

本实施例的其他部分与上述实施例1或2相同，故不再赘述。

实施例4：

本实施例是在实施例1或2的基础上进行优化，圆弧路径与保护域相交判据：保护圆心距圆弧路径段圆心的距离＜保护圆半径+圆弧路径半径，保护圆心距圆弧路径段圆心的距离＞|保护圆半径-圆弧路径半径|，且至少一个交点在圆弧路径段内。

进一步地，通过调整曲率方法生成内切解与外切解，若内切线解对应的圆弧半径大于最小转弯半径，则以内切线解半径为重规划路径的圆弧段半径，否则，若外切线解圆弧对应的半径大于最小转弯半径，则以外切线解半径为重规划路径的圆弧段半径，否则，以最小转弯半径为重规划路径的圆弧段半径。

进一步地，保护域的圆心为(x_b,y_b)，曲率为k_b，半径为r_b＝1/k_b；圆弧路径的圆心坐标为(x_c,y_c)，曲率为k_c，半径为r_c＝1/k_c，圆弧路径的转弯角为ψ_c，弧段圆心与保护圆心的距离为

记d_cm＝d_c-(r_c+r_b)，d_cp＝d_c+r_b-r_c，运动弧段与保护圆相交的判据则为d_c＜r_c+r_b，d_c＞|r_c-r_b|，即d_cm＜0，d_cp＞0，且保护域落在运动弧段内。

进一步地，如图12所示，若圆弧路径与保护域相交，采用调整运动圆弧曲率的方法计算与保护域相切的点规避保护域；

记保护域曲率为k_b，半径为

路径圆弧的曲率为k_f，半径为

目标点为P_f(x_f,y_f)，弧段圆心与保护圆心的距离

终止位置与保护圆心的距离

弧段圆心与终止位置的连线到终止位置与保护域圆心的连线的夹角

最大曲率解与最小曲率解为：

若两组路径解均满足飞行约束，取最短路径；若其中一组不满足飞行约束，则取另一组；若两组均不满足飞行约束，则以无人机最小转弯半径转弯。

本实施例的其他部分与上述实施例1或2相同，故不再赘述。

实施例5：

本实施例是在实施例1-4任一个的基础上进行优化，如图13所示，为实现保护域的保护，引入规避类型变量ω，ω＝1表示不同飞行高度平面上，规避圆的半径不同，且等于椭球体投影在当前飞行高度平面的半径+安全距离；ω＝-1表示不同飞行高度平面上，规避圆的半径相同，且等于椭球体投影的最大半径+安全距离。

进一步地，记禁飞中心位置为P_prot:(x,y,h,r)，无人机当前高度为h_now，ω＝1，投影圆的半径

ω＝-1，投影圆的半径为r_cir＝r。

本实施例的其他部分与上述实施例1-4任一个相同，故不再赘述。

实施例6：

本实施例是在实施例1-5任一个的基础上进行优化，采用射线扫描法判断无人机位置与边界地图之间的关系，假设边界地图由不规则n边形构成，规定逆时针为正，顶点按照逆时针方向排列，依次为P_i(x_i,y_i),i＝0,1,2,…,n-1，过P(x,y)向右作一条水平射线，计算与多边形边界的交点，若交点个数为奇数，则点在多边形内部，若交点个数为偶数，则点在多边形外。

进一步地，射线法计算如下：

首先快速剔除矩形外的点，记A＝{0,1,2,…,n-1},n≥3,

若P(x,y)不满足X_min＜x＜X_max,Y_min＜y＜Y_max,则无人机越界；若不满足，对于矩阵内的点，通过点P(x,y)与各边距离判断P(x,y)是否为控制顶点集P_i(x_i,y_i),i＝0,1,2,…,n-1的边点与顶点；对

P(x,y)与各边距离公式如下：

若d满足d＜ε，则P(x,y)为边点，无人机越界；

若对i＝0,1,2,…,n-1，满足

则P(x,y)为顶点，无人机越界；若P(x,y)非顶点或边点，则通过P(x,y)向右作一条水平射线，计算与多边形边界的交点，若交点个数为奇数，则点在多边形内部，若交点个数为偶数，则点在多边形外；

对

ε任意小，j＝1,2,…,n-1,n，若满足|y_j-1-y_j|≤ε，则交点个数为0，若满足|x_j-1-x_j|≤ε，则有一个交点，且交点坐标为x_p＝x_j-1,y_p＝y，

若满足|y_j-1-y_j|＞ε,|x_j-1-x_j|＞ε，则有一个交点，且交点坐标为：

若P(x,y)满足x＜x_p,min(y_j-1,y_j)≤y≤max(y_j-1,y_j)，且交点不为多边形顶点，则交点个数加1，若满足上式且P(x,y)交点为多边形顶点，则同侧顶点交点个数加2，异侧顶点交点个数加1。

进一步地，顶点同侧异侧判据如下：

记P_t＝(x_p-x,y_p-y),P_tt＝(x_mod(j+1,n)-x,y_mod(j+1,n)-y),P_tb＝(x_j-1-x,y_j-1-y)，

若满足(P_t×P_tt)(P_t×P_tb)＞0，则交点在同侧，否则交点在异侧。

本实施例的其他部分与上述实施例1-5任一个相同，故不再赘述。

实施例7：

本实施例是在实施例1-6任一个的基础上进行优化，设保护域中心点位置为P(x,y)，飞机位置为P'(x',y')，无人机距离保护域的距离为r，保护域的半径为r₀，保护域的安全圆半径为r₁；若r-r₁＞0，未进入保护域的安全圆；若0＜|r-r₀|＜r₁-r₀，进入保护域的安全圆，未进入保护域；若r-r₀≤0，飞机进入保护域。

本实施例的其他部分与上述实施例1-6任一个相同，故不再赘述。

实施例8：

一种无人机自主安全控制方法，图1为射线法示意图，射线法计算如下：

首先快速剔除矩形外的点，记A＝{0,1,2,…,n-1},n≥3,

若P(x,y)不满足X_min＜x＜X_max,Y_min＜y＜Y_max,则无人机越界；若不满足，对于矩阵内的点，通过点P(x,y)与各边距离判断P(x,y)是否为控制顶点集P_i(x_i,y_i),i＝0,1,2,…,n-1的边点与顶点。对

P(x,y)与各边距离公式如下：

若d满足d＜ε，则P(x,y)为边点，无人机越界。若对i＝0,1,2,…,n-1，满足

则P(x,y)为顶点，无人机越界。若P(x,y)非顶点或边点，则通过P(x,y)向右作一条水平射线，计算与多边形边界的交点，若交点个数为奇数，则点在多边形内部，若交点个数为偶数，则点在多边形外。对

ε任意小，j＝1,2,…,n-1,n，若满足|y_j-1-y_j|≤ε，则交点个数为0，若满足|x_j-1-x_j|≤ε，则有一个交点，且交点坐标为x_p＝x_j-1,y_p＝y，若满足|y_j-1-y_j|＞ε,|x_j-1-x_j|＞ε，则有一个交点，且交点坐标为

若P(x,y)满足x＜x_p,min(y_j-1,y_j)≤y≤max(y_j-1,y_j)，且交点不为多边形顶点，则交点个数加1，若满足上式且P(x,y)交点为多边形顶点，则同侧顶点交点个数加2，异侧顶点交点个数加1。

顶点同侧异侧判据如下。

记P_t＝(x_p-x,y_p-y),P_tt＝(x_mod(j+1,n)-x,y_mod(j+1,n)-y),P_tb＝(x_j-1-x,y_j-1-y)，若满足(P_t×P_tt)(P_t×P_tb)＞0，则交点在同侧，否则交点在异侧。

图2为越界算法仿真示意图。外侧虚线为警戒线，内侧虚线为预警线，黑色框线为正常航线，输入随机序列的无人机位置，黑色圆表示未越界，品红色圆表示在预警线外，警戒线内，红色圆表示在警戒线外。

图3为保护域与直线路径相交示意图，直线段与禁飞圈相交的判据为d_n≤r_b，且交点p_ip及p_im落在直线路径区间。判断交点落在直线路经上的方法如下。

记直线路经的从点坐标为P_x:(x_px,y_px)，到点坐标为P_n:(x_pn,y_pn)，禁飞圈圆心为P_b:(x_b,y_b)。

a)计算禁飞圈中心至直线段距离

b)计算禁飞圈与直线段交点

1)直线路径斜率不存在

·若d_n≤r_b，交点坐标为

·若d_n＞r_b，则保护域与直线无交点。

2)直线路径斜率存在

记Δ＝b²-4ac,其中

b＝2bA-2Ay_b-2x_b,a＝1+A²,c＝x_b ²-r_b ²+(b-y_b)²,

·若Δ≥0且d_n≤r_b，则保护域与直线有交点，设

u＝-b²-2Abx_b+2by_b+A²r_b ²-A²x_b ²+2Ax_by_b+r_b ²-y_b ²，

则交点坐标为

y_1,2＝Ax_1,2+b

·若Δ＜0或d_n＞r_b，则保护域与直线无交点。

c)相交检测

X_MIN＝min(x_1,2),X_MAX＝max(x_1,2),x_min＝min(x_px,x_pn),x_max＝max(x_px,x_pn)若满足

X_MIN＞x_max或X_MAX＜x_min，则直线路径未经过保护域，若X_MIN＞x_min，X_MAX＜x_max，

且飞行高度位于保护域的高度区间内，直线路径与保护域相交。

图4为圆弧段与保护域相交示意图，其中逆时针单位矢量t_oa与顺时针单位矢量t_oc表示从弧段圆心到与保护域交点的直线d_oa与d_oc的方向，t_ap与t_am表示弧段半径边界单位矢量，令i＝[p m]，j＝[ac]，有：

sinθ_ij＝|t_ai×t_oj|，cosθ_ij＝t_ai·t_oj，

运动弧段与保护域相交的判据则为d_c＜r_c+r_b，d_c＞|r_c-r_b|，即d_cm＜0，d_cp＞0，且至少有一个交点矢量落在运动弧段内。判断保护域落在运动弧段内的方法如下。

a)计算保护域与圆弧路径的交点P_1，2

y_1，2＝Ax_1，2+b

其中

u＝-b²+r_b ²+A²r_b ²-2Abx_b-A²x_b ²+2by_b+2Ax_by_b-y_b ²

b)计算单位矢量t_oa、t_oc、t_ap、t_am

弧段圆心到P_1，2的单位矢量t_oa、t_oc如下：

记弧段切入点为(x_p，y_p)，弧段切出点为(x_m，y_m)，弧段半径边界矢量t_op，t_om为

c)计算圆弧切入点(x_p，y_p)，切出点(x_m，y_m)与交点P_1，2的夹角

θ_pam＝θ_pa+θ_ma，θ_pcm＝θ_pc+θ_mc，

其中

θ_pa＝arc cos(t_oa·t_op)，θ_ma＝arc cos(t_oa·t_om)

θ_pc＝arc cos(t_oc·t_op)，θ_mc＝arc cos(t_oc·t_om)

记

pa＝t_oa×t_op,ma＝t_oa×t_om,pc＝t_oc×t_op,mc＝t_oc×t_om

pm_ac＝(pa,ma,pc,mc),θ＝(θ_pa,θ_ma,θ_pc,θ_mc)，规定无人机绕着运动圆弧逆时针旋转为正，无人机绕着运动圆弧逆时针旋转时，若pm_ac_i≤0，则θ_i＝2π-θ_i，无人机绕着运动圆弧顺时针旋转时，若pm_ac_i≥0，则θ_i＝2π-θ_i。

d)相交检测

若θ_pam＝ψ_c或θ_pcm＝ψ_c，飞行高度位于保护域的高度区间内，则保护域

与运动弧段相交。

图5为中间航点法示意图。中间航点计算如下。

a)计算直线路径与安全圆的交点

b)计算与交线垂直的直线与安全圆的交点

记

若直线路径斜率不存在，则有

x_3,4＝x₀±r_b,y_3,4＝y₀

若直线路径斜率为0

x_3,4＝x₀,y_3,4＝y₀±r_b

若直线路径斜率存在，记直线X₁X₂的斜率为k₀，则直线MN的斜率

u＝k²r_b ²-k²x₀ ²+2k²x₀x_b-k²x_b ²+2kx₀y₀-2kx₀y_b-2kx_by₀+2kx_by_b+r_b ²-y₀ ²+2y₀y_b-y_b ²则

y_3,4＝k(x_3,4-x₀)+y₀

图10为无人机路径未与保护域相交示意图，对于无人机路径未与保护域相交的情况，不启用路径重规划。

以上所述，仅是本发明的较佳实施例，并非对本发明做任何形式上的限制，凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化，均落入本发明的保护范围之内。

Claims (16)

1.一种无人机自主安全控制方法，其特征在于，当无人机规划路径经过保护区域时，判断相交路径为直线路径或圆弧路径，若直线路径段与保护域相交，采用中间航路点法生成同向旋转解与反向旋转解，以无人机位置、中间航路点、待飞航点进行路径重规划；若圆弧路径段与保护域相交，若圆弧路径与保护域相交，采用调整运动圆弧曲率的方法计算与保护域相切的点规避保护域；若重规划后的路径与圆弧段或者直线段相交，通过迭代产生避开保护域的路径。

2.根据权利要求1所述的一种无人机自主安全控制方法，其特征在于，当无人机位置位于保护域的高度区间内时，若所规避对象的类型属性为柱形，则以保护域最大半径为保护圆半径，否则，以保护域在无人机高度平面的水平投影圆半径叠加安全距离为保护圆半径；以保护圆半径为路径重规划所用的安全圆半径。

3.根据权利要求1或2所述的一种无人机自主安全控制方法，其特征在于，直线路径与保护域相交判据：保护域中心点距离直线路径段的距离小于保护域半径，且交点至少有一个点在直线路径段上。

4.根据权利要求3所述的一种无人机自主安全控制方法，其特征在于，采用中间航路点算法生成中间航路点，若中间航路点所在圆弧半径小于最小转弯半径，则以最小转弯半径为重规划路径圆弧段半径，否则，以中间航路点所在圆弧半径作为重规划路径圆弧段半径。

5.根据权利要求1或2所述的一种无人机自主安全控制方法，其特征在于，圆弧路径与保护域相交判据：保护圆心距圆弧路径段圆心的距离＜保护圆半径+圆弧路径半径，保护圆心距圆弧路径段圆心的距离＞|保护圆半径-圆弧路径半径|，且至少一个交点在圆弧路径段内。

6.根据权利要求5所述的一种无人机自主安全控制方法，其特征在于，通过调整曲率方法生成内切解与外切解，若内切线解对应的圆弧半径大于最小转弯半径，则以内切线解半径为重规划路径的圆弧段半径，否则，若外切线解圆弧对应的半径大于最小转弯半径，则以外切线解半径为重规划路径的圆弧段半径，否则，以最小转弯半径为重规划路径的圆弧段半径。

7.根据权利要求3所述的一种无人机自主安全控制方法，其特征在于，保护域的圆心为(x_b,y_b)，曲率为k_b，半径为r_b＝1/k_b，保护域中心点到直线段起始点的距离为d₀，保护域中心点到直线段的垂直投影线为d_n；直线段与保护域相交的判据为d_n≤r_b，且p_ip及p_im落在直线路径区间。

8.根据权利要求7所述的一种无人机自主安全控制方法，其特征在于，若直线路径与保护域相交，采用生成中间航路点的方法计算与保护域的安全圆相切的点；记起点为P_s，终点为P_f，C_safe为保护域的安全圆；从P_s至P_f到的路径参数为r(t)，r(t)与C_safe相交，交点为X₁,X₂，X₁为进入点，X₂为退出点，做一条与X₁,X₂连线垂直的直线，交C_safe于M,N，M,N即为新生成的中间航路点；

直线路径的切入点为(x_px,y_px)，切出点为(x_pn,y_pn)，直线路径的单位方向向量为

(x_px,y_px)与(x_3,4,y_3,4)的单位方向向量为：

令θ_na＝t_xn×t_xa,θ_nm＝t_xn×t_xm，

若θ_na＞0,θ_nm＜0，位于直线路径上方的点p_up为(x₃,y₃)，位于直线路径下方的点p_down为(x₄,y₄)，

若θ_na＜0,θ_nm＞0，位于直线路径上方的点p_up为(x₄,y₄)，位于直线路径下方的点p_down为(x₃,y₃)，

若无人机沿起始位置顺时针旋转，则同向旋转解p_SYN为p_up，反向旋转解p_INV为p_down，

若无人机沿起始位置逆时针旋转，则同向旋转解p_SYN为p_down，反向旋转解p_INV为p_up。

9.根据权利要求5所述的一种无人机自主安全控制方法，其特征在于，

保护域的圆心为(x_b,y_b)，曲率为k_b，半径为r_b＝1/k_b；圆弧路径的圆心坐标为(x_c,y_c)，曲率为k_c，半径为r_c＝1/k_c，圆弧路径的转弯角为ψ_c，弧段圆心与保护圆心的距离为

记d_cm＝d_c-(r_c+r_b)，d_cp＝d_c+r_b-r_c，运动弧段与保护圆相交的判据则为d_c＜r_c+r_b，d_c＞|r_c-r_b|，即d_cm＜0，d_cp＞0，且保护域落在运动弧段内。

10.根据权利要求9所述的一种无人机自主安全控制方法，其特征在于，若圆弧路径与保护域相交，采用调整运动圆弧曲率的方法计算与保护域相切的点规避保护域；

记保护域曲率为k_b，半径为

路径圆弧的曲率为k_f，半径为

目标点为P_f(x_f,y_f)，弧段圆心与保护圆心的距离

终止位置与保护圆心的距离

弧段圆心与终止位置的连线到终止位置与保护域圆心的连线的夹角

最大曲率解与最小曲率解为：

若两组路径解均满足飞行约束，取最短路径；若其中一组不满足飞行约束，则取另一组；若两组均不满足飞行约束，则以无人机最小转弯半径转弯。

11.根据权利要求1所述的一种无人机自主安全控制方法，其特征在于，为实现保护域的保护，引入规避类型变量ω，ω＝1表示不同飞行高度平面上，规避圆的半径不同，且等于椭球体投影在当前飞行高度平面的半径+安全距离；ω＝-1表示不同飞行高度平面上，规避圆的半径相同，且等于椭球体投影的最大半径+安全距离。

12.根据权利要求11所述的一种无人机自主安全控制方法，其特征在于，记禁飞中心位置为P_prot:(x,y,h,r)，无人机当前高度为h_now，ω＝1，投影圆的半径

ω＝-1，投影圆的半径为r_cir＝r。

13.根据权利要求1所述的一种无人机自主安全控制方法，其特征在于，采用射线扫描法判断无人机位置与边界地图之间的关系，假设边界地图由不规则n边形构成，规定逆时针为正，顶点按照逆时针方向排列，依次为P_i(x_i,y_i),i＝0,1,2,…,n-1，过P(x,y)向右作一条水平射线，计算与多边形边界的交点，若交点个数为奇数，则点在多边形内部，若交点个数为偶数，则点在多边形外。

14.根据权利要求13所述的一种无人机自主安全控制方法，其特征在于，射线法计算如下：

首先快速剔除矩形外的点，记A＝{0,1,2,…,n-1},n≥3,

若P(x,y)不满足X_min＜x＜X_max,Y_min＜y＜Y_max,则无人机越界；若不满足，对于矩阵内的点，通过点P(x,y)与各边距离判断P(x,y)是否为控制顶点集P_i(x_i,y_i),i＝0,1,2,…,n-1的边点与顶点；对

P(x,y)与各边距离公式如下：

若d满足d＜ε，则P(x,y)为边点，无人机越界；

若对i＝0,1,2,…,n-1，满足

则P(x,y)为顶点，无人机越界；

若P(x,y)非顶点或边点，则通过P(x,y)向右作一条水平射线，计算与多边形边界的交点，

若交点个数为奇数，则点在多边形内部，若交点个数为偶数，则点在多边形外；

对

ε任意小，j＝1,2,…,n-1,n，若满足|y_j-1-y_j|≤ε，则交点个数为0，

若满足|x_j-1-x_j|≤ε，则有一个交点，且交点坐标为x_p＝x_j-1,y_p＝y，

若满足|y_j-1-y_j|＞ε,|x_j-1-x_j|＞ε，则有一个交点，且交点坐标为：

y_p＝y，

若P(x,y)满足x＜x_p,min(y_j-1,y_j)≤y≤max(y_j-1,y_j)，且交点不为多边形顶点，则交点个数加1，若满足上式且P(x,y)交点为多边形顶点，则同侧顶点交点个数加2，异侧顶点交点个数加1。

15.根据权利要求14所述的一种无人机自主安全控制方法，其特征在于，顶点同侧异侧判据如下：

记P_t＝(x_p-x,y_p-y),P_tt＝(x_mod(j+1,n)-x,y_mod(j+1,n)-y),P_tb＝(x_j-1-x,y_j-1-y)，

若满足(P_t×P_tt)(P_t×P_tb)＞0，则交点在同侧，否则交点在异侧。

16.根据权利要求1所述的一种无人机自主安全控制方法，其特征在于，设保护域中心点位置为P(x,y)，飞机位置为P'(x',y')，无人机距离保护域的距离为r，保护域的半径为r₀，保护域的安全圆半径为r₁；若r-r₁＞0，未进入保护域的安全圆；若0＜|r-r₀|＜r₁-r₀，进入保护域的安全圆，未进入保护域；若r-r₀≤0，飞机进入保护域。

Images