自动控制飞行器的协同路径规划方法

摘要

本说明书提供一种自动控制飞行器的协同路径规划方法，方法包括：根据各个选取点的当前位置、飞行器的目标位置和选定区域内障碍物的位置确定各个选取点的当前评价指标；根据一选取点的调整位移、当前位置和历史最优位置，以及全局最优位置，更新一选取点的调整位移；根据一选取点的当前位置和更新后的调整位移更新一选取点的当前位置；更新当前位置设定次数后，在所有选取点的当前位置和全局最优位置中，选择最优评价指标对应的位置作为飞行器的航路点。采用前述方法规划规划飞行器下一航路点时，无需考虑是否与在前经过的航路点形成平滑的曲线，因此使得航路点的选择可以尽可能地达到局部最优。

权利要求

1.一种自动控制飞行器的协同路径规划方法，其特征在于，包括：步骤S101：在选定区域内随机地初始化选取点的当前位置和调整位移；

步骤S102：根据各个所述选取点的当前位置、所述飞行器的目标位置和所述选定区域内障碍物的位置确定各个所述选取点的当前评价指标；

步骤S103：根据一选取点的调整位移、当前位置和历史最优位置，以及全局最优位置，更新所述一选取点的调整位移；所述一选取点的历史最优位置根据所述当前评价指标和历史位置对应的最优评价指标确定；所述全局最优位置为所有选择点中历史最优位置中，评价指标最优的位置；

根据所述一选取点的自学习概率、当前位置和历史最优位置，计算自学习位移；

根据所述一选取点的全局学习概率、当前位置和所述全局最优位置，计算全局学习位移；

根据所述一选取点的调整位移、所述自学习位移和所述全局学习位移，更新所述一选取点的调整位移；

其中：所述自学习概率和所述全局学习概率的和为1；

在所述调整位移为第一次更新的情况下，随机地初始化所述自学习概率；

在所述调整位移不是第一次更新的情况下，获取所述自学习概率的步骤包括：随机地选取另一选取点；

在所述另一选取点的历史最优位置对应的评价指标小于或等于所述一选取点的历史最优位置对应的评价指标的情况下，保持所述一选取点的自学习概率不变；

在所述另一选取点的历史最优位置对应的评价指标大于所述一选取点的历史最优位置对应的评价指标的情况下，根据学习系数、所述另一选取点的自学习概率和所述一选取点的自学习概率更新所述一选取点对应的自学习概率；

步骤S104：根据所述一选取点的当前位置和更新后的调整位移，计算临时位置，并在所述临时位置位于所述选定区域的情况下，采用所述临时位置更新所述一选取点的当前位置；

重复执行步骤S102‑S104设定次数后，再次根据所述选取点的当前位置、所述飞行器的目标位置和所述选定区域内障碍物的位置确定各个所述选取点对应的当前评价指标，以及执行步骤S105；

S105：在所有所述选取点的当前位置和所述全局最优位置中，选择最优评价指标对应的位置作为所述飞行器的航路点。

2.根据权利要求1所述的方法，其特征在于，所述根据学习系数、所述另一选取点的自学习概率和所述一选取点的自学习概率更新所述一选取点的自学习概率，包括：计算所述另一选取点的自学习概率和所述一选取点的自学习概率的概率差值；

根据所述概率差值和所述学习系数计算概率调整值；根据所述一选取点的自学习概率和所述概率调整值，更新所述一选取点对应的自学习概率。

3.根据权利要求1或2任一项所述的方法，其特征在于，确定所述选定区域的步骤包括：确定以所述飞行器所在位置为球心、第一长度为半径的球体范围或者球缺范围内的障碍物区域，并确定所述障碍物区域在所述球体表面或者所述球缺对应的球冠面的投影面积；

根据所述投影面积和第一长度确定设定长度；

将以所述飞行器所在位置为球心、所述设定长度为半径的球体范围或者球缺范围设置为所述选定区域。

4.根据权利要求3所述的方法，其特征在于，所述选定区域的确定步骤包括：在所述飞行器连续经过的多个航路点的位置偏差小于设定偏差的情况下，将以所述飞行器所在位置为中心、第三长度为半径确定的球体范围或者球缺范围排除所述选定区域；

所述第三长度小于所述设定长度。

5.根据权利要求1或2任一项所述的方法，其特征在于，根据各个所述选取点的当前位置、所述飞行器的目标位置、所述选定区域内障碍物的位置确定各个所述选取点对应的当前评价指标，包括，

计算所述飞行器所在位置到所述选取点的第一距离，所述飞行器所在位置到所述目标位置的第二距离，以及，所述选取点到所述目标位置的第三距离；

根据所述第一距离、所述第二距离和所述第三距离计算距离性指标；以及，根据所述飞行器所在位置和所述选取点确定直线路径；

根据所述直线路径和所述障碍物的位置计算碰撞性指标；

根据所述距离性指标和所述碰撞性指标计算所述评价指标。

6.根据权利要求5所述的方法，其特征在于，所述障碍物包括固定障碍物和移动障碍物；

所述根据直线路径和障碍物的位置计算碰撞性指标包括：根据所述直线路径上的点和各个所述固定障碍物位置计算静态碰撞指标；和/或，根据所述直线路径上的点和所述移动障碍物的位置计算对应的间隔距离；

根据各个所述间隔距离和安全距离计算动态碰撞指标；

根据所述静态碰撞指标和所述动态碰撞指标计算所述碰撞性指标。

7.根据权利要求5所述的方法，其特征在于，根据所述选取点的位置计算评价指标，还包括：

根据所述飞行器的当前位置和所述选取点的位置计算可能航向；

根据所述飞行器的当前航向、所述可能航向计算航向偏差；

根据所述航向偏差和所述飞行器最大转向速率计算转向性指标；

所述根据所述距离性指标和所述碰撞性指标计算所述评价指标，包括：根据所述距离性指标、所述碰撞性指标和所述转向性指标计算所述评价指标。

8.一种飞行器系统，用于规划飞行器的飞行路径，其特征在于，包括：障碍物检测传感器，用于至少监测选定区域内的障碍物位置；

存储器，用于存储程序代码以及目标位置；

动力装置，用于驱动所述飞行器飞行和转向；

第一处理器，用于加载所述存储器存储的程序代码和所述目标位置，以及所述障碍物位置，执行如权利要求1‑7任一项所述的方法确定航路点。

说明书

自动控制飞行器的协同路径规划方法

技术领域

[0001] 本发明涉及飞行器技术领域，具体涉及一种自动控制飞行器的协同路径规划方法。

背景技术

[0002] 因为使用方便、成本低廉等优点，自动控制飞行器(简称飞行器)在军事领域和民用领域均具有广泛应用。为满足特殊应用性需求，在飞行器自动控制领域已经有自动驾驶
类的控制技术；但是目前飞行器自动控制技术多是采用构建平滑曲线等方式规划飞行器的
路径；而构建平滑曲线方式规划的路径，为了合理地躲避障碍物并保证路径的平滑性，路径
规划可能并不能达到较优。

发明内容

[0003] 本说明书提供自动控制飞行器的协同路径规划方法和装置，能够实现飞行器航路点的更优选择。

[0004] 一方面，本说明书提供一种自动控制飞行器的协同路径规划方法，包括：

[0005] 步骤S101：在选定区域内随机地初始化选取点的当前位置和调整位移；

[0006] 步骤S102：根据各个所述选取点的当前位置、所述飞行器的目标位置和所述选定区域内障碍物的位置确定各个所述选取点的当前评价指标；

[0007] 步骤S103：根据一选取点的调整位移、当前位置和历史最优位置，以及全局最优位置，更新所述一选取点的调整位移；所述一选取点的历史最优位置根据所述当前评价指标
和历史位置对应的最优评价指标确定；所述全局最优位置为所有选择点中历史最优位置
中，评价指标最优的位置；

[0008] 步骤S104：根据所述一选取点的当前位置和更新后的调整位移，计算临时位置，并在所述临时位置位于所述选定区域的情况下，采用所述临时位置更新所述一选取点的当前
位置；

[0009] 重复执行步骤S102‑S104设定次数后，再次根据所述选取点的当前位置、所述飞行器的目标位置和所述选定区域内障碍物的位置确定各个所述选取点对应的当前评价指标，
以及执行步骤S105；

[0010] S105：在所有所述选取点的当前位置和所述全局最优位置中，选择最优评价指标对应的位置作为所述飞行器的航路点。

[0011] 可选的，所述步骤S103包括：

[0012] 根据所述一选取点的自学习概率、当前位置和历史最优位置，计算自学习位移；

[0013] 根据所述一选取点的全局学习概率、当前位置和所述全局最优位置，计算全局学习位移；

[0014] 根据所述一选取点的调整位移、所述自学习位移和所述全局学习位移，更新所述一选取点的调整位移；

[0015] 其中：所述自学习概率和所述全局学习概率的和为1。

[0016] 可选的，在所述调整位移为第一次更新的情况下，随机地初始化所述自学习概率；

[0017] 在所述调整位移不是第一次更新的情况下，获取所述自学习概率的步骤包括：

[0018] 随机地选取另一选取点；

[0019] 在所述另一选取点的历史最优位置对应的评价指标小于或等于所述一选取点的历史最优位置对应的评价指标的情况下，保持所述一选取点的自学习概率不变；

[0020] 在所述另一选取点的历史最优位置对应的评价指标大于所述一选取点的历史最优位置对应的评价指标的情况下，根据学习系数、所述另一选取点的自学习概率和所述一
选取点的自学习概率更新所述一选取点对应的自学习概率。

[0021] 可选的，所述根据学习系数、所述另一选取点的自学习概率和所述一选取点的自学习概率更新所述一选取点的自学习概率，包括：

[0022] 计算所述另一选取点的自学习概率和所述一选取点的自学习概率的概率差值；

[0023] 根据所述概率差值和所述学习系数计算概率调整值；根据所述一选取点的自学习概率和所述概率调整值，更新所述一选取点对应的自学习概率。

[0024] 可选的，确定所述选定区域的步骤包括：

[0025] 确定以所述飞行器所在位置为球心、第一长度为半径的球体范围或者球缺范围内的障碍物区域，并确定所述障碍物区域在所述球体表面或者所述球缺对应的球冠面的投影
面积；

[0026] 根据所述投影面积和第一长度确定设定长度；

[0027] 将以所述飞行器所在位置为球心、所述设定长度为半径的球体范围或者球缺范围设置为所述选定区域。

[0028] 可选的，所述选定区域的确定步骤包括：

[0029] 在所述飞行器连续经过的多个航路点的位置偏差小于设定偏差的情况下，将以所述飞行器所在位置为中心、第三长度为半径确定的球体范围或者球缺范围排除所述选定区
域；

[0030] 所述第三长度小于所述设定长度。

[0031] 可选的，根据各个所述选取点的当前位置、所述飞行器的目标位置、所述选定区域内障碍物的位置确定各个所述选取点对应的当前评价指标，包括，

[0032] 计算所述飞行器所在位置到所述选取点的第一距离，所述飞行器所在位置到所述目标位置的第二距离，以及，所述选取点到所述目标位置的第三距离；

[0033] 根据所述第一距离、所述第二距离和所述第三距离计算距离性指标；以及，

[0034] 根据所述飞行器所在位置和所述选取点确定直线路径；

[0035] 根据所述直线路径和所述障碍物的位置计算碰撞性指标；

[0036] 根据所述距离性指标和所述碰撞性指标计算所述评价指标。

[0037] 可选的，所述障碍物包括固定障碍物和移动障碍物；

[0038] 所述根据直线路径和障碍物的位置计算碰撞性指标包括：

[0039] 根据所述直线路径上的点和各个所述固定障碍物位置计算静态碰撞指标；和/或，

[0040] 根据所述直线路径上的点和所述移动障碍物的位置计算对应的间隔距离；

[0041] 根据各个所述间隔距离和安全距离计算动态碰撞指标；

[0042] 根据所述静态碰撞指标和所述动态碰撞指标计算所述碰撞性指标。

[0043] 可选的，所述根据直线路径和障碍物的位置计算碰撞性指标包括：

[0044] 根据所述直线路径上的点和所述障碍物的位置计算对应的间隔距离；

[0045] 根据各个所述间隔距离和安全距离计算碰撞性指标。

[0046] 可选的，根据所述选取点的位置计算评价指标，还包括：

[0047] 根据所述飞行器的当前位置和所述选取点的位置计算可能航向；

[0048] 根据所述飞行器的当前航向、所述可能航向计算航向偏差；

[0049] 根据所述航向偏差和所述飞行器最大转向速率计算转向性指标；

[0050] 所述根据所述距离性指标和所述碰撞性指标计算所述评价指标，包括：

[0051] 根据所述距离性指标、所述碰撞性指标和所述转向性指标计算所述评价指标。

[0052] 另一方面，本说明书提供一种自动控制飞行器的协同路径规划装置，包括：

[0053] 初始化单元，用于在选定区域内随机地初始化选择点的当前位置和调整位移。

[0054] 评价指标计算单元，用于根据各个所述选择点的当前位置、目标位置、所述选定区域内障碍物的位置确定各个所述选择点对应的当前评价指标；

[0055] 调整位移更新单元，用于：根据一选择点的调整位移、当前位置和历史最优位置，以及全局最优位置，更新所述一选择点的调整位移；所述一选择点的历史最优位置根据所
述当前位置对应的当前评价指标和历史位置对应的最优评价指标确定；所述全局最优位置
根据所有选择点的历史最优位置确定；

[0056] 位置更新单元，用于根据所述一选择点的位置和更新后的调整位移，更新所述一选择点的位置；

[0057] 循环控制单元，用于控制所述评价指标计算单元、所述调整位移更新单元和所述位置更新单元循环执行设定次数；

[0058] 评价指标计算单元还用于再循环控制单元控制执行设定次数后，根据所述选取点的当前位置、所述飞行器的目标位置和所述选定区域内障碍物的位置确定各个所述选取点
对应的当前评价指标；

[0059] 航路点确定单元，用于在所有所述选择点的当前位置和所述全局最优位置中，选择所述评价指标最优的位置作为所述飞行器的航路点。

[0060] 再一方面，本说明书还提供一种存储介质，用于存储程序代码；所述程序代码在被加载后执行如前所述的方法。

[0061] 再一方面，本说明书提供一种飞行器系统，用于规划飞行器的飞行路径，包括：

[0062] 障碍物检测传感器，用于至少监测选定区域内的障碍物位置；

[0063] 存储器，用于存储程序代码以及目标位置；

[0064] 动力装置，用于驱动所述飞行器飞行和转向；

[0065] 第一处理器，用于加载所述存储器存储的程序代码和所述目标位置，以及所述传感器监测的所述障碍物位置，执行如权利要求1‑10任一项所述的方法确定航路点。

[0066] 本说明书提供的飞行器路径规划方法，在初始化各个选取点位置和调整位移的情况下，通过对各个选取点位置的评价确定各个选取点对应的历史最优位置，以及所有选取
点对应的全局最优位置，并利用历史位置和全局最优位置实现选取点的位置更新，并在更
新一定次数后确定评价指标最优的位置作为飞行器的航路点。采用前述方法规划规划飞行
器下一航路点时，无需考虑是否与在前经过的航路点形成平滑的曲线，因此使得航路点的
选择可以尽可能地达到局部最优。

[0067] 本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而
易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所
特别指出的内容中来实现和获得。

附图说明

[0068] 附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

[0069] 图1是实施例提供的飞行器系统的结构示意图；

[0070] 图2是实施例提供的自动控制飞行器的协同路径规划方法的流程图；

[0071] 图3是实施例提供的一种死锁问题场景示意图；；

[0072] 图4是实施例提供的飞行器路径规划装置的示意图；

[0073] 其中：01‑障碍物检测传感器，02‑存储器，03‑动力装置，04‑第一处理器；11‑初始化单元，12‑评价指标计算单元，13‑调整位移更新单元，14‑位置更新单元，15‑循环控制单
元，16‑航路点确定单元。

具体实施方式

[0074] 下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。

[0075] 在对本说明书实施例提供的方法和/或装置做介绍前，首先简单介绍应用本实施例方法的飞行器系统。

[0076] 请注意的是，本实施例中提及的飞行器系统是指飞行路径能够实现自动规划的系统(请注意，飞行器系统和飞行器可能并不是一个装置，后文会对此进行说明)不搭载乘客。

[0077] 图1是实施例提供的飞行器系统的结构示意图。如图1所示，实施例提供的飞行器系统包括障碍物检测传感器01、存储器02、动力装置03和第一处理器04。

[0078] 障碍物检测传感器01是对检测区域内障碍物的位置进行检测，以用于后续确定障碍物相对于飞行器位置和障碍物尺寸的传感器。实际应用中，障碍物传感器可以是声波传
感器、激光雷达、多目图像采集设备等类型的传感器，本实施例并不做特别地限定。在实际
应用中，障碍物检测传感器01优选采用声波传感器或者激光雷达等直接确定障碍物位置的
传感器。

[0079] 动力装置03是安装在飞行器上，用于驱动飞行器飞行和转向的装置；在本实施例具体应用中，飞行器可以为固定翼飞行器、旋翼飞行器、无人飞艇、扇翼飞行器或者扑翼飞
行器；对应的，动力装置03为前述各种类型飞行器中的升力产生机构和/或方向调整机构。

[0080] 存储器02用于存储程序代码和飞行器的目标位置；其中前述程度代码被一处理器加载后，可以形成相应的路径规划方法(也就是本说明书实施例提供的基于改进)，用于确
定飞行器的航路点，实现飞行器飞行路径的规划。

[0081] 第一处理器04是用于实现执行前述路径规划方法的处理器，即第一处理器04在加载存储在存储器02的程序代码和飞行器的目标位置，并获取障碍物传感器检测的障碍物位
置后，可以执行前述的路径规划方法，实现飞行器航路点的自动规划。

[0082] 如前文，本说明书实施例中飞行器系统和飞行器可能为两个不同的装置。实际应用中，可能的情况有如下几种情况。

[0083] 第一种情况

[0084] 飞行器系统即为飞行器；在此情况下，障碍物检测传感器01、存储器02和第一处理器04均安装在飞行器上，使飞行器本身就是一独立的飞行器系统，具有障碍物位置的采集、
路径规划和和动力装置03控制功能。

[0085] 第二种情况

[0086] 飞行器仅是飞行器系统中的一组成部分；前述的障碍物传感器、存储器02和第一处理器04中的至少部分并不安装在飞行器上，而是独立于飞行器设置。

[0087] 例如，在一种情况下，障碍物检测传感器01安装在飞行器飞行场景中，能够检测飞行器飞行场景中的障碍物和飞行器；存储器02和第一处理器04则安装在一远程服务器中。

[0088] 对应的，前述的远程服务器和飞行器中设置有通信模块，以用于将第一处理器04计算得到的航路点发送至飞行器，或者将根据航路点确定的飞行控制指令发送给飞行器，
由飞行器中的第二处理器根据航路点或者飞行控制指令控制动力装置03。

[0089] 再例如，障碍物检测传感器01安装在飞行器的上，而前述的存储器02和第一处理器04安装在远程服务器上。对应的，在飞行器和远程服务器设置有通信模块，障碍物检测传
感器01检测到的障碍物位置经过通信模块发送给远程服务器，远程服务器中的第一处理器
04处理根据障碍物位置和其他信息生成航路点或者飞行控制指令后，远程服务器将航路点
或者飞行控制指令发送给飞行器，由飞行器的第二处理器根据航路点或者飞行控制指令控
制动力装置03。

[0090] 图2是实施例提供的自动控制飞行器的协同路径规划方法的流程图。如图2所示，本实施例提供的飞行器路径规划方法包括步骤S101‑S107。

[0091] S101：在选定区域内随机地初始化选取点的当前位置和调整位移。

[0092] 本实施例中，选定区域是一根据飞行器的特性、飞行场景特性等确定的，用于在其中选择下一个航路点的区域。根据飞行器特性、飞行场景特性的不同，选定区域相应地也就
不同。

[0093] 例如：在飞行器为旋翼飞行器情况下，选定区域可以是以飞行器为球心、以一长度为半径圈定的球体范围。在飞行器为固定翼飞行器的情况下，选定区域可以是飞行器飞行
方向上，以飞行器所在位置为圆心、以一长度为半径圈定的球缺范围，或者，以在飞行器飞
行方向上的立方体区域或者长方体区域。

[0094] 再本实施例具体应用中，前段的长度确定方法可以不同。例如：在障碍物检测传感器01集成在飞行器的情况下，前述的长度可以是障碍物传感器的探测半径，或者小于障碍
物传感器的探测半径。

[0095] 在本实施例的一个具体应用中，障碍物检测传感器01集成在飞行器中，前述的设定区域被设置为以飞行器所在位置为球心，以一长度为半径的球体或者球缺。为了减小飞
行器和障碍物碰撞的概率，前述的一设定长度根据场景中障碍物的情况确定。具体的，选定
区域的确定步骤包括S1011‑S1014。

[0096] S1011：确定以飞行器所在位置为球心，以第一长度为半径确定的球体或者球缺范围内的障碍物区域。

[0097] 前述的第一长度为障碍物检测传感器01的探测半径。步骤S1011，集成在飞行器中的障碍物传感器工作，确定其可检测范围内的障碍物。

[0098] 因为障碍物传感器安装在飞行器中，所以障碍物传感器检测到的障碍物由障碍物面向飞行器的表面表示。

[0099] 应用注意是，如果第一障碍物位于第二障碍物和障碍物检测传感器01之间，并且相对于前述第二障碍物远离飞行器，则第一障碍物并不能被飞行器探测。为了克服此问题，
本实施例的一个具体应用做了简化处理，根据直接面向飞行器的障碍物表面，以及前述表
面在球体表面或者球缺对应的球冠面上的投影确定的区域认定为障碍物区域。

[0100] S1012：确定障碍物区域在球体表面或者球缺对应的球冠面上的投影面积。

[0101] 在本实施例的一个具体应用中，步骤S1012是确定障碍物区域在以飞行器所在位置为球心，以第一长度为半径确定的球体表面或者球缺对应的球冠面上的投影面积。

[0102] S1013：根据投影面积和第一长度确定设定长度。

[0103] 在本实施例的一个具体应用中，并不是直接将障碍物传感器的探测半径(也就是第一长度)确定的范围作为选定区域，而是根据前述的投影面积和第一长度共同确定的设
定长度。

[0104] 通过对步骤S1011和S1012分析可知，投影面积的大小障碍物距离飞行器的距离和障碍物本身面向障碍物传感器的面积呈正相关，因此：(1)在障碍物和飞行器距离确定的情
况下，障碍物面向障碍物传感器的表面积越大，投影面积越大；(2)在障碍物面向障碍物传
感器的面积确定的情况下，障碍物到飞行器的距离越近，投影面积越大。因此，根据投影面
积和第一长度确定的设定长度，是根据障碍物距离飞行器的距离，以及障碍物面向飞行器
的表面积确定。

[0105] 在实施例的一个具体应用中，在障碍物传感器的探测区域为球体区域的情况下，设定长度采用公式一确定。

[0106]

[0107] 公式一中，RF(t+1)为设定长度，RD为第一长度，为S(t+1)投影面积， 为球体的表面积。α为设定的指数因子，α越大表示投影面积对确定设定长度的影响越大。

[0108] S1014：将以飞行器所在位置为球心，以设定长度为半径的球体范围或者球缺范围设定为选定区域。

[0109] 根据步骤S1011‑S1013的分析可知，设定长度与障碍物到飞行器的距离呈反比，与障碍物面向飞行器的表面积也呈反比。因此，选定区域的大小与障碍物到飞行器的距离呈
反比，与障碍物面向飞行器的表面积也呈反比。

[0110] 采用前述的方法确定的选定区域，在障碍物较多、障碍物距离飞行器较近的情况下，可以适应地减小选定区域的范围，继而减小飞行器航路点的选择，减小飞行器与障碍物
碰撞的概率。

[0111] 在前述实施例的具体应用中，选定区域为球体或者球冠，在其他具体应用中，选定区域也可以是一长方体区域或者立方体区域。此外，前述的投影并不限于在球体或者球冠
表面上的投影，也可以是在长方体或者立方体的多个表面上的投影。

[0112] 在前述实施例的具体应用中，障碍物传感器集成在飞行器中。在实施例其他的具体应用中，障碍物检测传感器01也可以独立于飞行器设置；在此情况下，障碍物检测传感器
01检测到障碍物和飞行器后，可以将各种障碍物的位置转换至飞行器坐标系，并采用前述
方法确定选定区域。应当注意，在障碍物检测传感器01在确定设定长度时，需要另外设定一
长度，而不能再使用障碍物传感器的探测距离。

[0113] 还应注意的是，如果障碍物传感器并不集成在飞行器中，并且障碍物检测传感器01的数量和位置设置合理，障碍物传感器可能检测某一区域内的所有障碍物，并且可以确
定所有障碍物的表面形态，并利用所有障碍物的体积确定设定长度。

[0114] 在本申请实施例的具体应用中，步骤S101中随机初始化的选取点数量为可以为一个，也可以为多个。当然，为了保证运算效率以及尽可能地确定合理的航路点，具体应用中
优选设置多个选取点。

[0115] S102：根据各个选取点的当前位置，飞行器的目标位置和选定区域内障碍物的位置确定各个选取点对应的当前评价指标。

[0116] 本说明书实施例的具体应用中的一种理想目标是飞行器能够以尽可能短的路径，并且不与障碍物碰撞的前提下到达目标位置。根据前述目标，用于评价各个选取点优劣的
当前评价指标可以包含距离性指标和碰撞性指标。

[0117] 距离性指标用于表示飞行器从所在位置经过选取点目标位置的长度评级性指标；碰撞性指标用于表示飞行器从所在位置到达选取点与障碍物发生碰撞可能性的评价性指
标。

[0118] 在本实施例的一个具体应用中，计算距离性指标的方法可以包括步骤S201‑S202。

[0119] S201：计算飞行器所在位置到选取点的第一距离，飞行器所在位置到目标位置的第二距离，以及选取点到目标位置的第三距离。

[0120] S202：根据第一距离、第二距离和第三距离计算距离性指标。

[0121] 前述的步骤S201和S202综合采用公式二表示。

[0122] f1(t+1)＝A+B；   公式二

[0123] 其 中 ，为飞行器
的当前位置坐标，(rx(T),ry(T),rz(T))为飞行器的目标位置坐标，(rx(t+1),ry(t+1),rz(t+
1))为选取点的位置坐标。

[0124] 分析公式二可知，在实施例的一个具体应用中，计算的距离性指标以第二距离为参照，求取第一距离和第二距离的第一比值，以及第三距离和第二距离的第二比值，将第一
比值和第二比值的和作为距离性指标。

[0125] 在实施例的一些具体应用中，也可以仅将第三距离和第二距离的比值作为距离性指标。

[0126] 在实施例的另外一些具体应用中，也可以将第一距离和第三距离之和，或者将第三距离作为距离评价性指标。

[0127] 在本实施例的一个具体应用中，计算碰撞性指标的方法可以包括步骤S301‑S302。

[0128] S301：根据飞行器所在位置和选取点位置确定直线路径。

[0129] S302：根据直线路径和障碍物的位置计算碰撞性指标。

[0130] 本实施例具体应用中，假设飞行器以直线飞行地方式从一个航路点到达另一航路点，因此可以采用直线路径上点的位置和障碍物位置的关系，计算碰撞性指标。

[0131] 根据实际应用场景的不同，障碍物可能为固定障碍物，也可能为移动障碍物，或者既包括固定障碍物又包括移动障碍物。

[0132] 在障碍物为固定障碍物的情况下，碰撞性指标为静态碰撞指标。在一个具体应用中，计算静态碰撞指标的方法包括步骤S3011‑S3012。

[0133] S3011：在直线路径上等间隔地分成M段，获得M‑1个分割点。

[0134] S3012：根据选取点的位置、分割点的位置和固定障碍物的位置，计算静态碰撞指标。

[0135] 本实施例中，静态碰撞指标采用公式三和公式四计算得到。

[0136]

[0137] 其中，

[0138] 根据公式三和公式四可知，如果选取点在障碍物范围内，则静态碰撞性指标至少为1。在障碍物区域如S1011确定范围，并且选取点在障碍物范围内的情况下，选取点越是远
离选取点，则相应的静态碰撞指标也就越大。

[0139] 在障碍物为动态障碍物的情况下，碰撞性指标为动态碰撞指标。在一个具体应用中，计算动态碰撞指标的方法包括步骤S3021‑S3022。

[0140] S3021：在直线路径上等间隔地分割M段，获得M‑1个分割点。

[0141] 应当注意，步骤S3021中的M与步骤S3011中的M可能相同，也可能并不同，本实施例并不做限定。

[0142] S3022：根据选取点的坐标、分割点的坐标和各个移动障碍物的坐标距离计算对应的间隔距离。

[0143] 步骤S3022中，是根据选取点的坐标和各个分割点的坐标，分别确定其距离移动障碍物的间隔距离。应当注意，在分割点或者选取点在障碍物范围的情况下，前述的间隔距离
可以被设置为0，也可以设置为负数(负数代表相应点在障碍物范围内，其表示了相应点到
障碍物表面的距离)。

[0144] S3023:根据间隔距离和安全距离计算动态碰撞指标。

[0145] 在本实施例具体应用中，虽然已经确定障碍物为移动障碍物，但是并不能确定移动障碍物的移动特性，即并不能准确地确定移动障碍物在移动方向和移动速度(当然，在一
些情况下可以根据障碍物检测传感器01的历史测量结果确定障碍物的移动状态，但是数据
处理量较大，并且不能保证移动障碍物移动方向和移动速度的准确性)。

[0146] 为了克服无法检测移动障碍物速度等问题，本实施例假设各个移动障碍物在飞行器从当前位置飞行到下一航路点的过程中，移动障碍物的移动距离最多为安全距离。可以
想到，在飞行器从当前位置飞行到下一航路点的时间较短的情况下，前述的假设是合理的。

[0147] 如果前述各个点到障碍物的间隔距离均大于安全距离，则飞行器并不会与移动障碍物发生碰撞；而如果前述有一个点到移动障碍物的间隔距离小于安全距离，则证明在经
过此点时飞行器和移动障碍物距离过近，飞行器和移动障碍物相撞的可能性也增加。基于
前述的分析，可以根据间隔距离和安全距离的大小计算动态碰撞指标。

[0148] 在本实施例的一个具体应用中，动态碰撞指标可以采用公式五和公式六计算得到。

[0149]

[0150]

[0151] 公式五和公式六中，Rs表示安全距离，Dj(t+1)表示第j个分割点或者选取点到障碍物的间隔距离，cj2(t+1)为第j个分割点或者选取点对应的动态碰撞指标。

[0152] 在实施例具体应用中，在一条直线路径上，各个分割点和选取点对应的安全距离可以相同，也可以不同。

[0153] 在一种情况下，由飞行器当前位置到选取点，分割点或者选取点对应的安全距离依次地增大。通过分析可知，如此设置可以减小随时间的累计，移动障碍物与飞行器碰撞可
能性增大的问题。

[0154] 除了前述确定动态碰撞指标的方法外，在本实施例另一应用中，还可以根据障碍物的范围和安全距离相加，确定根据障碍物边缘扩大的区域，并将此区域作为非安全区域，
随后以非安全区域和直线路径，采用前述确定静态碰撞指标的步骤计算动态碰撞指标。

[0155] 在本实施例一个具体应用中，在选定区域的障碍物包括固定障碍物和移动障碍物的情况下，碰撞指标为静态碰撞指标和动态碰撞指标求和的结果。

[0156] 应当注意的是，在前文叙述中能够区分移动障碍物和固定障碍物的前提是，障碍物传感器可以通过不同时刻检测的障碍物信号而识别障碍物是固定障碍物还是移动障碍
物。

[0157] 而在一些实际应用情况下，区分选定区域的障碍物是固定障碍物还是移动障碍物并不是可行的。针对此类情况，可以将选定区域的所有障碍物均假定为移动障碍物，而采用
前文确定动态碰撞指标作为碰撞性指标。

[0158] 根据前文描述，在确定距离性指标和碰撞性指标后，可以根据距离性指标和碰撞性指标计算当前评价指标。

[0159] 在一个具体应用中，可以采用公式七计算当前评价指标。

[0160] F(t+1)＝λ1f1(t+1)+λ2f2(t+1)   公式七

[0161] 公式七中，f1(t+1)为距离性指标，f2(t+1)为碰撞性指标，λ1和λ2为权重系数。

[0162] 在本实施例另外一些具体应用中，除了考虑前述的距离性指标和碰撞性指标外，还可能考虑因为飞行器航向变化造成的资源消耗(可以成为转向代价)。由于飞行器航向变
化造成的资源消耗采用转向性指标表示。一种用于确定转向性指标的方法包括步骤S3031‑
S3033。

[0163] S3031：根据飞行器当前位置和选取点的位置计算可能航向。

[0164] 在本实施例的具体应用中，飞行器的可能航向采用当前位置和选取点确定的矢量方向表示，也就是前述的可能航向为由飞行器当前位置到选取点确定的方向。

[0165] S3032：根据飞行器的当前航向和可能航向计算航向偏差。

[0166] 飞行器的当前航向可以为飞行器在当前位置的移动方向，也可以是由当前位置和已经过的最近航路点确定的方向，本实施例并不做具体的限定。

[0167] 实际应用中，根据飞行器的当前航向和可能航向计算航向偏差，即是计算在飞行器由当前位置飞行到选取点时飞行方向的变化。可以确定，航向偏差为一角度。根据实际情
况可知，航向偏差在0°‑180°之间。

[0168] S3033：根据航向偏差和飞行器最大转向速率计算转向性指标。

[0169] 步骤S3033是的航向偏差和飞行器的最大转向速率相比得到的比较结果，并将比较结果作为转向性指标。

[0170] 在实施例一具体应用中，转向性指标可以采用公式八得到。

[0171]

[0172] 公式八中，P为表示当前航向的向量，Q为表示可能航向的向量；在一个应用中，P＝(rx(t)‑rx(t‑1),ry(t)‑ry(t‑1),rz(t)‑rz(t‑1))，Q＝(rx(t+1)‑rx(t),ry(t+1)‑ry(t),rz(t+
1)‑rz(t))。

[0173] 当然，在一些应用中，也可以将航向偏差作为转向性指标，或者根据航向偏差查找相应的对照表确定对应数值作为转向性指标。

[0174] 在当前评价指标包括转向性指标的情况下，计算当前评价指标可以采用公式九表示。

[0175] F(t+1)＝λ1f1(t+1)+λ2f2(t+1)+λ3f3(t+1)   公式九

[0176] 公式九中，f1(t+1)为距离性指标，f2(t+1)为碰撞性指标，f3(t+1)为航向性指标，λ1，λ2和λ3为权重系数。

[0177] 在步骤S102完成，确定各个选取点对应的当前评价指标后，可以执行步骤S103。

[0178] S103：根据一选取点的调整位移、当前位置和历史最优位置以及全局最优位置，更新一选取点的调整位移。

[0179] 步骤S103中提及的历史最优位置是指一选取点的评价指标最优时对应的位置。一选取点的历史最优位置为一选择点的最优评价指标对应的位置，一选取点的历史最有位置
根据当前位置对应的当前评价指标和历史位置对应的最优评价指标确定。

[0180] 步骤S103中提及的全局最优位置是指所有选取点中的历史最优位置中，评价指标最高的位置。

[0181] 本说明书实施例中，步骤S103用于更新各个选取点的调整位移。

[0182] 应当注意的是，本实施例中，本实施例中的调整位移是一矢量，是表示从一选取点当前位置到此选取点下一次求算得到的位置的位移。可以想到，调整位移可以采用一三维
向量表示。

[0183] 本实施例具体应用中，根据一选取点的调整位移、当前位置和历史位置，以及全局最优位置，更新一选取点的调整位移的方法有如下几种。

[0184] 方法一

[0185] 方法一可以包括步骤S401‑S403。

[0186] S401：根据一选取点的自学习系数、历史最优位置和当前位置，计算自学习位移。在一个具体应用中，自学习位移可以采用公式十表示。

[0187] vi1＝U(0,c1)·(pbesti‑xi)   公式十

[0188] 公式十中，U(0,c1)为自学习系数，其表示了(0,c1)中的随机数,c1为一预设常数。根据公式十可以确定，自学习位移与历史最优位置和当前位置的差值呈正比，与自学习系
数呈正比；pbesti为历史最优位置，xi为当前位置。

[0189] S402：根据一选取点的全局学习系数、全局最优位置和当前位置，计算全局学习位移。在一个具体应用中，全局学习位移可以采用公式十一表示。

[0190] vi2＝U(0,c2)·(gbest‑xi))   公式十一

[0191] 公式十一中，U(0,c2)为全局学习系数，其表示(0,c2)中的随机数，c2也为一预设常数。根据公式十一可以确定，全局学习位移与全局最优位置和当前位置的差值成正比，与全
局学习系数成反比。

[0192] S403：根据调整位移、对应的自学习位移和全局学习位移，更新一选取点的调整位移。

[0193] 步骤S403可以采用公式十二表示。

[0194] vi＝η·(vi+vi1+vi2)   公式十二

[0195] 公式十二中，η为调整系数。

[0196] 根据公式十二可知，一选取点更新后的调整位移由当前的调整位移，最新计算的自学习位移和全局学习位移求和得到。

[0197] 结合前述步骤S401‑S403分析可知，在方法一中，自学习位移和全局学习位移相对独立，二者并没有直接相关关系。

[0198] 请注意是，在方法一中，c1、c2分别为自学习系数和全局学系数，二者中任一均可以大于1，也可以小于1。

[0199] 方法二

[0200] 方法二包括步骤S501‑S503。

[0201] S501：根据一选取点的自学习概率、历史最优位置和当前位置，计算自学习位移。

[0202] 步骤S501可以采用公式十三表示。

[0203] vi1＝pi·(pbesti‑xi)   公式十三

[0204] 公式十三中，pi为自学习概率，pbesti为历史最优位置，xi为一选取点的当前位置。

[0205] S502：根据一选取点的全局学习概率、全局最优位置和当前位置，计算全局学习位移。

[0206] 步骤S502可以采用公式十四表示。

[0207] vi2＝(1‑pi)·(gbest‑xi))   公式十四

[0208] 公式十四中，选取点的全局学习概率为1‑pi，也就是全局学习概率和自学习概率的和为1。gbest为全局最优位置，xi为一选取点的当前位置。

[0209] S503：根据调整位移、对应的自学习位移和全局学习位移，更新一选取点的调整位移。

[0210] 步骤S503确定的调整位移可以采用公式十五表示。

[0211] vi＝η·(vi+c1·vi1+c2·vi2)   公式十五

[0212] 公式十五中，η为调整系数。

[0213] 根据公式十五可知，一选取点更新后的调整位移是将当前调整位移、求算得到的自学习位移和全局学习位移得到。

[0214] 在方法二可知，自学习概率和全局学习概率二者之和为1，在二者之一的概率增加时，另一概率相应得减小。即，一选取点的自学习位移和全局学习位移的变化并不是独立
地，，而是存在此消彼长的关系。如此，可以保证调整位移的变化处在合理的范围内，并是的
调整位移的变化能够满足合理的。

[0215] 在方法二中，根据不同的情况下，自学习概率的获得方法不同；例如，在一实施例具体应用中，自学习概率的获取方法包括步骤S5011‑S5015。

[0216] S5011：判断调整位移是否为第一次更新；若是，执行S5012；若否，执行S5013。

[0217] S5012：随机地初始化自学习概率。

[0218] S5013：随机地选取另一选取点；判断另一选取点的历史最优位置对应的评价指标是否小于或等于一选取点的历史最优位置对应的当前评价指标；若是，执行步骤S5014；若
否，执行S5015。

[0219] S5014：保持一选取点的自学习概率不变。

[0220] S5015：根据学习系数、另一选取点的自学习概率和一选取点的自学习概率更新一选取点的自学习概率。

[0221] 在一个具体应用中，步骤S5015可以采用公式十六表示。

[0222] pi＝pi+β·(pq‑pi)   公式十六

[0223] 公式十六中，pi为一选取点的自学习概率，pq为另一选取点的自学习概率，β∈[0,1]表示学习系数。根据公式十六可知，实现自学习概率更新的方法步骤为：首先计算另一选
取点的自学习概率和一选取点自学习概率的差值，根据概率差值和学习系数计算调整值，
根据一选取点的自学习概率和概率调整至，更新一选取点对应的自学习概率。

[0224] 采用前述S5011‑S5015对一选取点的自学习概率进行修正，同时实现了对全局学习概率的修正；采用这样的方法，能够实现在求解过程中选取点选取和开发的智能型和平
衡性，更好地求解多峰函数的问题。

[0225] 在本实施例的其他应用中，各个选择点的自学习概率可以每次均随机的设定，并不限于前述步骤S5011‑S5015确定的方法。

[0226] 在完成步骤S103后，执行步骤S104。

[0227] S104：根据一选取点的位置和更新后的调整位移，计算临时位置；并在临时位置位于选定区域的情况下，采用临时位置更新一选取点的位置。

[0228] 步骤S104中，根据一选取点的位置和更新后的调整位移，计算临时位置，是以一选取点的位置为起点，以调整位移为位移量求算新的位置的过程；在具体应用中，临时位置的
计算方法可以采用公式十七表示。

[0229] xi＝xi+vi   公式十七

[0230] 公式十七中，xi为选取点的位置，vi为调整位移。

[0231] 为了避免计算后的临时位置位于选定区域外，本说明书实施例中，在计算选取点的临时位置后，还应当判断临时位置是否位于选定区域内；如果临时位置在选定区域内，则
可以将临时位置更新选取点的位置，并随后执行步骤S105；而如果临时位置在选择区域之
外，则需要返回步骤S103中重新计算调整位移，并重新执行本步骤。

[0232] 步骤S105：判断循环次数是否达到设定次数；若是，执行步骤S106；若否，则重新执行步骤S102‑S105。

[0233] 为了尽可能地在选定区域选择足够多的位置判断其是否可以作为选定的航路点，在本实施例中，每个选取点均需要经过设定次数的迭代。步骤S105即用于判断各个选取点
的迭代次数是否达到设定次数。如果选取点的迭代次数并没有达到设定次数，则重新执行
步骤S102‑S105。

[0234] 步骤S106：再次各个选取点的当前位置、飞行器的目标位置和选定区域内障碍物的位置确定各个选取点对应的当前评价指标。

[0235] 因为步骤S106的执行过程与步骤S102相同，因此在此不再对步骤S106的内容做展开描述，相关内容可以参见前述步骤S102的描述。

[0236] S107：在所有选取点的当前位置和全局最优位置中，选择评价指标最优的位置作为飞行器的航路点。

[0237] 在执行步骤S106后，确定了所有选取点最后一次迭代后对应的当前位置对应的当前评价指标；同时，在最后一次迭代前，已经确定了所有航路点的历史最优位置对应的评价
指标，因此可以比较各个选取点的当前评价指标和历史位置最优位置对应的评价指标，确
定最好的评价指标，并将最好的评价指标对应的位置作为飞行器的航路点。

[0238] 根据对步骤S101‑S107的分析可知，本说明书实施例提供的飞行器路径规划方法，在初始化各个选取点位置和调整位移的情况下，通过对各个选取点位置的评价确定各个选
取点对应的历史最优位置，以及所有选取点对应的全局最优位置，并利用历史位置和全局
最优位置实现选取点的位置更新，并在更新一定次数后确定评价指标最优的位置作为飞行
器的航路点。

[0239] 可以想到，采用步骤S101‑S107确定的飞行器路径规划方法，在规划飞行器下一航路点时，无需考虑是否与在前经过的航路点形成平滑的曲线，因此使得航路点的选择可以
尽可能地达到局部最优。

[0240] 在本实施例具体应用中，飞行器在飞行到某一特殊区域时，可能出现死锁问题。死锁问题指的是飞行器在进入到某一区域后而在此区域移动，而无法飞出此区域而达到目标
位置的问题。

[0241] 图3是实施例提供的一种死锁问题场景示意图。如图3所示，在某些飞行场景中，因为目标位置在障碍物和当前位置之间，飞行器在飞到当前位置后，在确定选定区域并计算
下一航路点时，计算的航路点可能仍然在障碍物远离目标位置的一侧；并且随后飞行器规
划的多个航路点均在此障碍物远离目标位置的一侧处；此时飞行器进入了死锁区间(即产
生了死锁问题)，而无法飞出。

[0242] 为了解决前述问题，在本申请实施例的一个具体应用中，在飞行器飞行过程中，还记录飞行器经过的航路点的位置；如果飞行器连续经过的多个航路点的位置偏差小于设定
值，则将飞行器所在位置为中心、第三长度为半径确定的球体范围或者球缺范围排除选定
区域；前述的第三长度小于设定长度。可以想到，采用前述方法，如果出现死锁问题，则本实
施例将确定一死锁区域，并将死锁区域排除在选定区域。因此，在随后航路点选择过程中，
飞行器不会在死锁区域内选择航路点，而时在死锁区域外选择航路点，以保证飞离死锁区
域。实际应用中，死锁区域可以保持一定的周期，以确保飞行器在飞离死锁区域后，又再次
飞入死锁区域圈定的区域。

[0243] 除了提供前述的自动控制飞行器的协同路径规划方法外，本说明书实施例还提供一种自动控制飞行器的协同路径规划装置。因为自动控制飞行器的协同路径规划装置和前
述的方法基于相同的发明构思，解决相同的技术问题并达到相同的技术效果，因此下文近
对自动控制飞行器的协同路径规划装置结构做介绍，相关的技术问题和技术效果可以参见
方法部分表述。

[0244] 图4是实施例提供的自动控制飞行器的协同路径规划装置的示意图。如图4所示，本实施例装置包括初始化单元11、评价指标计算单元12、调整位移更新单元13、位置更新单
元14、循环控制单元15和航路点确定单元16。

[0245] 初始化单元11，用于在选定区域内随机地初始化选择点的当前位置和调整位移。

[0246] 评价指标计算单元12，用于根据各个选择点的当前位置、目标位置、选定区域内障碍物的位置确定各个选择点对应的当前评价指标。

[0247] 调整位移更新单元13，用于：根据一选择点的调整位移、当前位置和历史最优位置，以及全局最优位置，更新一选择点的调整位移；一选择点的历史最优位置根据当前位置
对应的当前评价指标和历史位置对应的最优评价指标确定；全局最优位置根据所有选择点
的历史最优位置确定。

[0248] 位置更新单元14用于根据一选择点的位置和更新后的调整位移，更新一选择点的位置。

[0249] 循环控制单元15用于控制评价指标计算单元12、调整位移更新单元13和位置更新单元14循环执行设定次数；

[0250] 评价指标计算单元12还用于再循环控制单元15控制执行设定次数后，根据选取点的当前位置、飞行器的目标位置和选定区域内障碍物的位置确定各个选取点对应的当前评
价指标。

[0251] 航路点确定单元16用于在所有选择点的当前位置和全局最优位置中，选择评价指标最优的位置作为飞行器的航路点。

[0252] 在具体应用中，前述的各个单元可以按照执行前述方法的具体步骤，以实现对应的功能。

[0253] 除了提供前述的自动控制飞行器的协同路径规划方法和装置外，本说明书实施例还提供一种可读存储介质和一种飞行器系统。

[0254] 可读存储介质中存储有程序代码，程序代码在被处理器加载后可以用于执行如前文的自动控制飞行器的协同路径规划方法。

[0255] 如图1所示，飞行器系统包括障碍物检测传感器01、存储器02、动力装置03和第一处理器04。其中障碍物检测传感器01用于至少检测选定区域内的障碍物位置；存储器02用
于存储程序代码和目标位置；动力装置03用于驱动飞行器的飞行和转向；第一处理装置用
于加载存储器02存储的程序代码和目标位置，以及根据障碍物检测传感器01检测的障碍物
位置，执行如前的自动控制飞行器的协同路径规划方法，以确定飞行器的航路点。

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