CN113253761B - 实时路径规划方法、装置、可移动载具及存储介质 - Google Patents

Abstract

本发明实施例提供一种实时路径规划方法、装置、可移动载具及存储介质，所述方法用于可移动载具，包括：获取所述可移动载具的当前行进数据，行进路况信息以及全局DEM数据；根据所述当前行进数据，所述全局DEM数据以及所述行进路况信息，判断当前行进路线是否安全；若不安全，则根据所述可移动载具的当前行进数据以及动力学约束规划局部行进路线；控制所述可移动载具根据所述局部行进路线行进，能够高效平稳的动态的规划局部行进路线，从而实现了对可移动载具的精确控制，避免了可移动载具进入危险区域。

Description

实时路径规划方法、装置、可移动载具及存储介质

技术领域

本发明涉及运动控制领域，尤其是涉及一种实时路径规划方法、装置、可移动载具及计算机可读存储介质。

背景技术

可移动载具，例如无人机、机器人、移动小车、移动船或水下的移动设备等，在农业、工业、影视、搜救、警用、军事等很多领域发挥很重要的作用，可以适应复杂的环境。

传统的可移动载具，例如，无人机，需要在作业之前，由操作人员首先设计航线。航线可以包括若干坐标点序列组成的若干段局部行进线段，可移动载具依次访问这些局部行进线段。由于操作人员对行进路线区域内的状况信息无法做到实时了解，容易造成航线与实际任务区域内信息冲突，最终导致碰撞事故。

可移动载具路径规划是指其在特定评价指标最优的约束下，如路径长度、路径搜索时间、运动能量损耗等要求，且满足障碍物无碰撞的前提下，生成一条从起始位置到目标位置的路径，是可移动载具自主巡航，提高行进效率和安全性的重要过程。可移动载路径规划算法能够辅助可移动载执行航线行进路线，在可移动载访问与实际任务区域信息冲突的局部行进线段的过程中，规划出一条安全的行进航线。

目前，常见的可移动载辅助路径规划，以无人机辅助路径规划为例，主要为：低空多旋翼无人机依赖机载感知传感器如：激光雷达、超声波传感器、单目摄像机等感知环境与地图，实时规划路径。

该方法只适用于低空多旋翼无人机范畴，其机载传感器的搜索范围有限，不适用于现如今飞行海拔较高的作业环境。为了解决上述问题，有必要提出一种能够实时根据行进路线的碰撞风险，动态的规划行进路线方法，进而使可移动载具安全且高效的完成行进路线。

发明内容

本发明实施例的一个目的是提供一种实时路径规划方法、装置、可移动载具及存储介质，能够根据全局DEM数据对可移动载具行进路线的安全性进行实时判断，并对可移动载具的行进路线进行动态规划，有效的避免了可移动载具进入危险区域的可能。

第一方面，本发明实施例提供了一种实时路径规划方法，用于可移动载具，包括：

获取可移动载具的当前行进数据，行进路况信息以及全局DEM数据；

根据当前行进数据，全局DEM数据以及行进路况信息，判断当前行进路线是否安全；

若不安全，则根据可移动载具的当前行进数据以及动力学约束规划局部行进路线；

控制可移动载具根据局部行进路线行进。

第二方面，本发明实施例提供了一种实时路径规划装置，用于可移动载具，包括：

获取模块，用于获取可移动载具的当前行进数据，行进路况信息以及全局DEM数据；

判断模块，用于根据当前行进数据，全局DEM数据以及行进路况信息，判断当前行进路线是否安全；

确定模块，用于在所述当前行进路线不安全时，根据可移动载具的当前行进数据以及动力学约束规划局部行进路线；

控制模块，用于控制可移动载具根据局部行进路线行进。

第三方面，本发明实施例提供了一种可移动载具，包括：

包括动力装置与如上述所述的实时路径规划装置，其中，实时路径规划装置用于向动力装置发送指令，动力装置用于根据指令产生动力，使得可移动载具按照实时路径规划装置确定的规划路径行进。

第四方面，本发明实施例还提供了一种计算机可读存储介质，计算机可读存储介质包括指令，当指令在计算机上运行时，使得计算机执行如上述所述的实时路径规划方法。

本发明实施例提供的实时路径规划方法、装置、可移动载具及计算机可读存储介质，通过获取所述可移动载具的当前行进数据，行进路况信息以及全局DEM数据；根据所述当前行进数据，所述全局DEM数据以及所述行进路况信息，判断当前行进路线是否安全；若不安全，则根据所述可移动载具的当前行进数据以及动力学约束规划局部行进路线；控制所述可移动载具根据所述局部行进路线行进，能够高效平稳的动态的规划局部行进路线，从而实现了对可移动载具的精确控制，避免了可移动载具进入危险区域。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是根据本发明实施例的飞行器的示意性架构图 ；

图2是根据本发明实施例的实时路径规划方法的示意性流程图；

图3是图2中步骤S2的示意性流程图；

图4是图2中步骤S3的示意性流程图；

图5是图4中步骤S31的示意性流程图；

图6是图5中步骤S313的示意性流程图；

图7是本发明实施提供的一矩形区域的示意图；

图8是图5中步骤S314的示意性流程图；

图9是图5中步骤S315的示意性流程图；

图10是本发明实施例提供的一节点位置关系图；

图11示出根据本发明实施例提供的实时路径规划装置200的结构示意图；

图12示出根据本发明实施例提供的可移动载具300的结构示意图；

图13是根据本发明实施例的计算机设备的示意性框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为了更好地理解本发明，下面结合图1至图11对本发明实施例提供的实时路径规划方法、装置、可移动载具及计算机可读存储介质进行详细描述。应注意，这些实施例并不是用来限制本发明公开的范围。如图1所示，在本发明的一个实施例中，可移动载具为飞行器，如，无人机，在其他可能的实施例中，可移动载具还可以是载人飞行器、机器人、汽车、船、航模、无人飞艇、固定翼飞行器和无人热气球等。

如图1所示，飞行器100可以包括动力装置110、实时路径规划装置120和机架130。动力装置110可以包括电子调速器(简称为电调)111、一个或多个螺旋桨112以及与一个或多个螺旋桨112相对应的一个或多个电机113，其中电机113连接在电子调速器111与螺旋桨112之间，电机113和螺旋桨112设置在对应的机臂上；电子调速器111用于接收实时路径规划装置120产生的驱动信号，并根据驱动信号提供驱动电流给电机113，以控制电机113的转速。电机113用于驱动螺旋桨112旋转，从而为飞行器100提供动力。

实时路径规划装置120可以包括控制模块121和导航模块122。导航模块122内部存有全局DEM数据，通过与控制模块121通信，获取当前飞行器100的飞行状态，并实时判断飞行器100当前行进路线是否存在碰撞风险，制定辅助路径规划策略。控制模块121用于控制飞行器100的飞行，实时获取飞行器100当前的飞行状态，并与导航模块实时通信。控制模块121根据导航模块122对当前行进路线的风险判断以及规划的路径，控制无人机按照规划路径飞行。

应理解，上述对于飞行器100各组成部分的命名仅是出于标识的目的，不应理解为是对本发明的实施例的限制。

本发明提供了一种实时路径规划方法，图2为本发明实施例提供的实时路径规划方法的流程图，本发明实施例的实时路径规划方法可以应用于各种类型的可移动载具，例如可以应用于图1所示的飞行器100，该方法例如可以由图1中所示的实时路径规划装置120执行。如图2所示，该方法具体可以包括：

步骤S1：获取可移动载具的当前行进数据，行进路况信息以及全局DEM数据。

可选的，在本发明的一个实施例中，获取可移动载具的当前行进数据可以包括：获取可移动载具的当前位置，当前行进姿态，当前行进速度，当前行进模式，当前行进路线的起始点以及目标点。其中，当前位置可以包括地理标系下的经度lon，纬度lat以及高度high坐标；当前行进姿态可以包括俯仰角pitch，横滚角roll以及偏航角yaw；当前行进速度可以包括北向速度Vn，东向速度Ve以及地向速度Vd；当前行进模式可以包括以下模式的任意一种：辅助路径规划模式、任务行进模式、降落模式、手动控制模式，在辅助路径规划模式下可移动载具正在进行辅助路径规划，在任务行进模式下，即可移动载具按照任务路线行进，没有进行辅助路径规划；当前行进路线的起始点以及目标点可以为整段行进路线的起始点以及目标点，还可以是飞行航线的相邻的两个航点。

可选的，行进路况信息可以包括：起始点周围预定范围内禁飞区的信息以及电子围栏的边界坐标。

本发明实施例中，飞行器100的控制模块121包括GPS定位装置，待控制模块121初始化后，加载自身定位位置周围一定范围内的禁飞区信息。可选的，可以加载周围50kM内的禁飞区信息，且可以选定加载的禁飞区总个数可以为10个。禁飞区加载完毕后，再加载用于限定飞行器100飞行范围的电子围栏的信息。控制模块121加载完毕后将禁飞区的信息以及电子围栏的边界坐标发送到导航模块122。

步骤S2：根据当前行进数据，全局DEM数据以及行进路况信息，判断当前行进路线是否安全。

本发明实施例中，DEM（Digital Elevation Model，数字高程模型）是用一组有序数值阵列形式表示地面高程的一种实体地面模型。

本发明实施例中，可以根据当前行进数据，全局DEM数据以及行进路况信息，判断出飞行器的当前行进路线是否存在撞山等风险，判断当前行进路线是否安全。

步骤S3：若不安全，则根据可移动载具的当前行进数据以及动力学约束规划局部行进路线。

本发明实施例中，若当前行进路线不安全，可以根据可移动载具的当前行进数据以及动力学约束，通过路径规划算法实时的规划局部行进路线，规划出安全的行进路线。

本发明实施例中，在确定当前行进路线不安全时，由于可移动载具可能已经按照当前行进路线移动，或者可移动载具由于具有移动速度，而且进行路径规划也有一定的时间延迟，因此，可以对当前行进路线进行局部规划，从而得到局部行进路线。

根据本发明实施例，可以首先根据可移动载具的当前行进模式确定是否规划局部行进路线，若当前行进模式为手动模式或降落模式等非辅助路径规划模式以及任务行进模式时，不规划局部行进路线。若当前行进模式为辅助路径规划模式时，根据可移动载具的其他当前行进数据以及动力学约束规划局部行进路线。若当前行进模式为任务行进模式时，则控制可移动载具进入辅助路径规划模式，并根据可移动载具的其他当前行进数据以及动力学约束规划局部行进路线。

本发明实施例中，可以根据所述当前行进数据以及所述动力学约束，生成路径规划的节点列表，然后根据所述节点列表建立路径规划列表，从而根据所述路径规划列表生成所述局部行进路线。

步骤S4：控制可移动载具根据局部行进路线行进。

本发明实施例中，针对原当前行进路线为任务行进模式的可移动载具，在其按照局部路线行进后，则退出辅助路径规划模式，以局部行进路线为行进路线，继续任务行进模式。或者，可移动载具在完成局部行进路线后，退出辅助路径规划模式，继续任务行进模式。

本发明实施例中，若判断当前行进路线不安全，则可根据飞行器100的当前行进数据以及动力学约束规划局部行进路线。若判断当前行路线安全，则可以根据当前行进路线继续行进，当达到导航模块122获取当前飞行器100的飞行状态的周期，则重新执行上述S1-S3的步骤。

根据本发明实施例的实时路径规划方法，能够实时监测飞行器100的飞行数据，行进路况信息，并通过行进路况信息以及全局DEM数据判断飞行器100的当前行进路线是否安全，若不安全，则根据飞行器100在飞行过程中的当前行进数据以及动力学约束规划局部行进路线，能够做到实时调控飞行器100的行进路线，确保了飞行器100在飞行过程中的安全性。该方法通过实时规划局部行进路线，使飞行器100的行进路线可根据实际的行进区域的具体情况自适应的调整路线，灵活性强，安全性更高。

可选的，在本发明的一个实施例中，如图3所示，步骤S2具体可以包括：

步骤S21：分别根据起始点以及目标点，在全局DEM数据中确定当前行进路线对应的至少一个区块。

需要说明的是，DEM数据是由预设尺寸的区块组成的，根据当前行进路线的起始点以及目标点，可以确定当前行进路线对应的区块，若当前行进路线跨越多个区块，则该当前行进路线对应多个区块，若当前行进路线的起始点和目标点位于同一个区块，则当前行进路线对应一个区块。

步骤S22：根据至少一个区块，确定局部DEM 数据。具体地，若确定对应的区块的数目为多个，则将多个区块合并，合并后的区块为确定的局部DEM数据。若确定对应的区块为一个，则该区块为确定的局部DEM数据。其中，局部DEM数据均在电子围栏的边界坐标范围内。

步骤S23：根据局部DEM数据，判断当前行进路线是否安全。

可选的，在本发明的一个实施例中，步骤S23具体可以包括：

根据局部DEM数据的分辨率在当前行进路线上进行采样，确定多个采样点。具体的，在起始点和目标点所组成的当前行进路线上，根据局部DEM数据本身的分辨率（一般为30M×30M）进行采样，得到多个采样点。需要说明的是，根据起始点和目标点的经纬度坐标以及海拔高度，可以得到当前行进路线上任意一个点的经纬度坐标以及海拔高度，当进行采样后，每个采样点的经纬度坐标以及海拔高度都可以确定。

在确定多个采样点后，分别确定起始点，目标点以及多个采样点相同水平坐标对应的局部DEM数据中的地表高度，并分别将起始点，目标点以及多个采样点的海拔高度与对应的地表高度进行比较，确定起始点，目标点以及多个采样点中海拔高度高于对应的地表高度，且高度差不小于安全阈值的点为安全点。其中，安全阈值可以为20m。若起始点，目标点以及多个采样点均确定为安全点，则确定当前行进路线为安全路径。

具体的，对于起始点，目标点以及所有采样点，分别比较其海拔高度与该水平坐标下对应的局部DEM数据中的地表高度，若海拔高度高于地表高度，并且高度差不小于一定的安全阈值（安全阈值可以为20m），则视其为安全点，否则该点为不安全点。任何一个采样点为不安全点，此段行进路线视为不安全，当起始点，目标点以及所有采样点都是安全点时，该段行进路线视为安全路径。

可选的，在本发明的一个实施例中，如图4所示，步骤S3具体可以包括：

步骤S31：根据当前行进数据以及动力学约束，生成路径规划的节点列表。

具体的，根据控制模块121实时获取飞行器100的当前行进数据，可以基于当前行进数据获取到下一时刻的预测位置，将该预测位置、当前行进数据中的目标点以及基于动力学约束得到的搜索节点加入到路径规划的节点列表，从而生成该节点列表。需要指出的是，该节点列表至少包括2个节点：预测位置以及目标点。

步骤S32：根据所述节点列表建立路径规划列表。

本发明实施例中，可以基于节点列表中的节点生成路径节点，并基于路径节点生成路径规划列表。需要说明的是，节点列表中的中的节点数目可以多于或等于路径规划列表中的路径节点。

步骤S33：根据所述路径规划列表生成所述局部行进路线。

本发明实施例中，可以根据路径规划列表中的各个路径节点的顺序，将各个路径节点连接，生成局部行进路线。

例如，路径规划列表中的路径节点为A、B、D、G，则将这4个路径节点依次串联起来，生成局部行进路线。

根据本发明实施例的实时路径规划方法，通过实时获取飞行器100的当前行进数据，并结合动力学约束建立路径规划列表，将不符合动力学约束的节点舍弃，能够实时获取有效数据，精准度高，时效性更强。

可选的，在本发明的一个实施例中，如图5所示，步骤S31具体可以包括：

步骤S311：根据滚动时域控制策略以及当前行进数据，确定可移动载具的下一时刻的预测位置。

具体地，根据滚动时域控制策略，确定每次进入路径规划任务时的当前位置的地理坐标，并以当前位置为原点建立水平坐标系，根据当前位置的水平坐标以及当前行进速度确定下一时刻的预测位置。

可选的，以当前位置的地理坐标为原点，建立水平坐标系。可以通过以下公式，将任意一点的地理坐标转换为以当前位置为原点的水平坐标：

Local_position.x = (Local_coord.lon- Now_coord.lon) * LonToM

Local_position.y = (Local_coord.lat - Now_coord.lat) * R2 （1）

Local_position.z = Local_coord.high

其中，R2为极半径，Local_position为某一点的以当前位置为原点的水平坐标系下的坐标，Local_coord为该某一点的地理坐标，Now_coord为当前位置的地理坐标。

其中，LonToM 为转换系数，LonToM = (R1 *π/ 180) * cos(Local_coord.lat *π/ 180)，R1为地球赤道半径，π为圆周率。

根据如下公式，确定下一时刻的预测位置的水平坐标：

Next_position.x = Now_positon.x + Ve * dt

Next_position.y = Now_positon.y + Vn * dt （2）

Next_position.z = Now_positon.z + Vd * dt

其中，Now_positon为当前位置的地理坐标Now_coord转化为水平坐标系下的坐标，由于当前位置为水平坐标系的原点，所以：

Now_positon.x = 0

Now_positon.y = 0 （3）

Now_positon.z = Now_coord.high

其中，dt为控制模块121和导航模块122定频通信的时间周期，Ve、Vn和Vd分别为飞行器100当前的东向、北向、地向速度。

通过滚动时域控制策略，能够实时确定飞行器100的当前行进速度，并利用飞行器100的当前行进速度预测其下一时刻的位置，实现了对飞行器100的实时的规划路径。

需要指出的是，本发明实施例中，在后续进行路径规划时的节点以及路径节点都是以该当前位置的地理坐标为原点建立的水平坐标系下的坐标进行计算的。当前行进数据中的目标点的坐标为地理坐标，可以通过上述公式（1）将其转换为以当前位置为原点的水平坐标。

步骤S312：将该预测位置存入节点列表。

需要指出的是，节点列表中的节点按照加入顺序排列，该预测位置是第一个加入至节点列表中的节点，是节点列表中的初始点。

步骤S313：在采样区域生成随机采样点。

具体的，将预测位置存入节点列表后，通过采样策略，在预设的采样区域生成随机采样点。需要指出的是，后续可以通过判断随机采样点是否在禁飞区、搜索节点与父节点之间的路径是否安全，以及是否达到结束搜索节点存入节点列表的条件，来重新生成随机采样点。

步骤S314：根据动力学约束以及随机采样点，确定搜索节点。

具体的，分别计算所确定的随机采样点与节点列表的节点之间是否满足对应的动力学约束，并确定搜索节点。

步骤S315：将该搜索节点存入所述节点列表。

本发明实施例中，在每确定一个搜索节点后，将该搜索节点加入至节点列表。

本发明实施例中，在每次将搜索节点存入节点列表时，先将迭代次数加1，然后判断是否满足结束搜索节点存入节点列表的条件，若满足，将所述目标点存入所述节点列表，不再在节点列表中存入搜索节点，即不再产生随机采样点以确定新的搜索节点；否则，将所述搜索节点存入节点列表，并在采样区域重新生成随机采样点，以确定新的搜索节点，并将新的搜索节点存入所述节点列表，直到满足结束搜索节点存入节点列表的条件。

需要指出的是，结束搜索节点存入节点列表，也就是结束重新确定新的搜索节点，即结束重新生成随机采样点，确定搜索节点的循环结束。

其中，结束搜索节点存入节点列表的条件包括以下至少一个：

1）所述迭代次数大于最大迭代次数。

本发明实施例中，该最大迭代次数可以自定义设置，且针对不同的两相邻航点之间生成的节点列表，所设置的最大迭代次数可以为多个不同的值。

2）搜索节点与所述目标点的欧式距离小于搜索截止距离。

需要指出的是，在每次将搜索节点加入至节点列表后，均需要计算该新加入的搜索节点与目标点的欧式距离，确定其是否小于搜索截止距离，例如，在节点列表中每存入一个搜索节点new_pt后，判断该new_pt与目标点的欧式距离是否小于搜索截止距离。

该搜索截止距离可以预设置，用于表示进行路径规划的最小距离，当两个点之间的距离小于该最小距离，则说明这两个点之间距离很小，不需要再规划节点了，当两个点之间的距离大于或等于该搜索截止距离，则仍需要在起始点和目标点之间规划出节点，因此，需要在采样区域重新生成随机采样点，以确定新的搜索节点，并将新的搜索节点存入所述节点列表，直到满足结束搜索节点存入节点列表的条件。

3）搜索节点与所述目标点之间的路径为安全路径，且所述搜索节点与所述目标点之间满足所述动力学约束。

本发明实施例中，在每次将搜索节点加入至节点列表后，均需要计算该搜索节点与目标点之间的路径是否为安全路径以及该搜索节点与目标点之间是否满足动力学约束，若为安全路径且满足动力学约束，则两个点之间不再需要路径规划，否则，即安全路径与动力学约束中至少一个不满足，则在采样区域重新生成随机采样点，以确定新的搜索节点，并将新的搜索节点存入所述节点列表，直到满足结束搜索节点存入节点列表的条件。

本发明上述实施例中，根据滚动时域控制策略以及所述当前行进数据，确定所述可移动载具的下一时刻的预测位置；将所述预测位置存入所述节点列表；在采样区域生成随机采样点；根据所述动力学约束以及所述随机采样点，确定搜索节点；将所述搜索节点存入所述节点列表，不仅实现了根据当前行进数据以及动力学约束实时进行路径规划，而且在路径规划时，通过结束搜索节点存入节点列表的条件自动的生成节点列表中的节点，提升了路径规划的效率以及成功率。

在本发明的一个实施例中，如图6所示，步骤S313可以包括但不限于以下步骤：

步骤S3131：根据当前位置的水平坐标以及目标点的水平坐标，确定水平采样区域。

需要说明的是，当前位置的水平坐标以及目标点的水平坐标，均是以当前位置为原点建立的水平坐标系，因此，当前位置的水平坐标为（0，0），而根据上述公式（1），可以将当前行进数据中的目标点的坐标为地理坐标转换为以当前位置为原点的水平坐标。

本发明实施例中，为了避免过于狭长或者面积太小的采样区域导致规划失败，可以将水平采样区域统一处理为一个正方形区域，具体计算方式如下：

1.找到以当前位置Now_position和目标点Goal_position为对角的水平矩形区域。

图7是本发明实施提供的一矩形区域的示意图，如图7所示，当前位置Now_position的水平坐标为（0，0），目标点Goal_position的水平坐标为（x2，y2）。以当前位置Now_position和目标点Goal_position为对角的水平矩形区域R如图7中的矩形，其4个顶点的坐标分别为（0，y2）、（x2，y2）、（x2，0）以及（0，0）。

2.该矩形区域的几何中心点坐标为（x1, y1），以该中心点设置正方形的水平采样区域。

3.定义矩形区域R的较长边长的一半为m，该正方形的边长为L= 2*m*r，其中r为扩大系数，可以预设置，本发明实施例中提供一个经验值：r可以为1.5。已知正方形的中心点坐标和边长，可以得出正方形四个顶点的坐标，由上述描述可知，正方形的四个顶点的坐标为（x1-r*m， y1-r*m）、（x1 – r*m, y1 + r*m）、（x1+r*m， y1+r*m）、（x1 + r*m, y1 - r*m）。

如图7所示，在扩展出正方形时，假设较长边长为（0，y2）与（x2，y2）这两个顶点之间的边，则其较长边长I= x2，即较长边长的一半m= I/2= x2/2= x1，最终得出的正方形的4个顶点如图7所示。

需要说明的是，若该矩形本身为正方形，则依然可以按照该方法扩展出正方形区域。

在扩展出正方形区域后，由于实际选择的采样点可能会超出该区域，因此对该区域根据扩大系数进行扩大。

步骤S3132：根据水平采样区域以及随机采样策略，在所述水平采样区域内生成随机采样点，并确定所述随机采样点的水平坐标。

本发明实施例中，随机采样策略可以为目标偏向概率阈值采样策略。生成随机采样点并确定随机采样点水平坐标Rand_position(rand_x,rand_y)计算公式如下：

如果 rand_p > rand_bias

rand_x = Goal_position.x （4）

rand_y = Goal_position.y

其中，rand_p为生成的随机采样点的随机概率，可选的，通过随机算法在（0，1）内任意选取一值生成rand_p，rand_bias为目标偏向概率阈值，该目标偏向概率阈值可用于表示生成的随机采样点为目标点的阈值，目标偏向概率阈值可以预先设置，例如，设置为0.5、0.7等，通过设置目标偏向概率阈值，可以将生成随机采样点的随机概率限制在一个范围内，当生成的随机概率超过这个范围，随机采样点即为目标点，从而减少采样点的随机性，提高规划算法的收敛速度。

(Goal_positionx, Goal_position.y)为目标点水平坐标，rand_x表示生成的随机采样点的x轴的坐标，rand_y表示生成的随机采样点的y轴的坐标。当生成的随机采样点的随机概率大于目标偏向概率阈值，则随机采样点为目标点。

上述实施例中，通过采用目标偏向概率采样策略，同时限制合理的采样范围，能够有效的提高本发明实施例的实时路径规划方法的算法收敛速度。

步骤S3133：根据所述局部DEM数据以及所述随机采样点的水平坐标，确定所述随机采样点是否在禁飞区。

本发明实施例中，禁飞区可以是以多个水平坐标点为边界形成的封闭区域，当确定随机采样点的水平坐标（rand_x，rand_y）后，通过判断该随机采样点的水平坐标是否在该封闭区域内，确定该随机采样点是否在禁飞区。

步骤S3134：若是，则根据所述水平采样区域以及随机采样策略重新生成随机采样点，直到重新生成的随机采样点的水平坐标不在禁飞区。

具体的，通过查询所确定的局部DEM数据，判断随机采样点是否在禁飞区，若随机采样点落在禁飞区，则重新确定采样点。通过对局部DEM数据查询，能够有效确保随机采样点的有效性。

步骤S3135：若不是，则确定随机采样点的垂直高度坐标。

本发明实施例中，可以根据所述随机采样点的水平坐标，所述局部DEM数据中海拔最高值，所述当前位置的高度坐标以及所述目标点的高度坐标，确定所述随机采样点的高度坐标。

在确定随机采样点的高度坐标时，可以在局部DEM数据找到与随机采样点水平坐标相同的数据，从而查询该DEM数据下，随机采样点的水平坐标对应的DEM海拔高度，即随机采样点的垂直高度，然后根据该随机采样点的垂直高度、局部DEM数据中海拔最高值、当前位置的高度坐标以及目标点的高度坐标，确定随机采样点的最大高度坐标，然后根据随机采样点的垂直高度以及最大高度坐标，确定随机采样点的高度坐标。

确定随机采样点的最大高度坐标时，可以通过以下方式中的一种：

1）将随机采样点的垂直高度为min_z，在局部DEM数据中确定海拔最高值为high_max，选取high_max、Now_position.z和目标点垂直高度Goal_position.z中的最大值，确定为随机采样点最大高度坐标max_z。

需要指出的是，该确定最大高度坐标的方式，一般用于最大高度坐标为非随机采样点的垂直高度的情况。

2）在确定high_max，Now_position.z和目标点垂直高度Goal_position.z中的最大值后，在该最大值的基础上加上高度容忍阈值，确定为随机采样点最大高度坐标max_z，其中，该高度容忍阈值可以为50m。

在确定随机采样点的最大高度坐标max_z后，在随机采样点的垂直高度以及最大高度坐标 (min_z，max_z)范围内，生成随机高度rand_z，从而生成随机采样点rand_pt的水平坐标以及高度坐标(rand_x，rand_y，rand_z)。

需要指出的是，通过上述方式1）确定随机采样点的最大高度坐标时，存在随机采样点的垂直高度为最大高度坐标的情况，此时随机高度的范围为min_z。

本发明实施例的实时路径规划方法，通过分别确定水平面内的随机采样点，再根据所确定的局部DEM数据选择垂直平面内点的范围，在该范围内寻找符合约束的点，提高了确定搜索节点的成功率，避免将大量搜索资源浪费在计算约束上。

需要说明的是，本发明实施例中也可以先按照步骤S3135确定随机采样点的高度后，然后按照步骤S3133至S3134的方法确定随机采样点的水平坐标。

在本发明的一个实施例中，如图8所示，步骤S314可以包括但不限于以下步骤：

步骤S3141：在节点列表中确定与随机采样点满足动力学约束的至少一个约束节点。

本发明实施例中，在将预测位置存入节点列表，并在采样区域生成随机采样点后，在节点列表中确定与该随机采样点满足动力学约束的所有约束节点，该约束节点的数目可以是至少一个。

需要指出的是，若节点列表中不存在与该随机采样点满足动力学约束的节点，则无法确定搜索节点，则舍弃该随机采样点，重新按照步骤S313的方法生成新的随机采样点。

本发明实施例中，可以分别对节点列表中任意一个节点与随机采样节点做动力学约束检查，若某个任意点与随机采样节点满足动力学约束，该任意点为约束节点。按照此方法，轮询节点列表中的所有节点，从而确定节点列表中所有与随机采样点满足动力学约束的至少一个约束节点。

可选的，当节点列表中仅包括预测位置节点时，确定该预测位置节点是否与随机采样点满足动力学约束，若满足，则预测位置节点为该随机采样点的约束节点，若不满足，则舍弃该随机采样点，重新按照步骤S313的方法生成新的随机采样点。

本发明实施例中，可以分别对节点列表中任意一个节点与随机采样节点做动力学约束检查，若某个任意点与随机采样节点满足动力学约束，该任意点为约束节点。按照此方法，轮询节点列表中的所有节点，从而确定节点列表中所有与随机采样点满足动力学约束的至少一个约束节点。

可选的，当节点列表中仅包括预测位置时，确定该预测位置是否与随机采样点满足动力学约束，若满足，则该预测位置为该随机采样点的约束节点，若不满足，则舍弃该随机采样点，重新按照步骤S313的方法生成新的随机采样点。根据本发明实施例的实时路径规划方法，通过采用动力学约束作为路径规划的约束条件，有效的保障了路径规划效果的可靠性和安全性。

步骤S3142：分别确定至少一个约束节点与随机采样点之间的欧式距离。

本发明实施例中，任意一个约束节点与随机采样点之间的欧式距离的计算方式如下：

Dist = sqrt(Dx * Dx + Dy * Dy + Dz * Dz) （5）

其中，Dist 表示任意一个约束节点与随机采样点之间的欧式距离， sqrt()为取平方根函数，Dx表示随机采样点与该任意一个约束节点在x轴上的相对距离，Dy表示随机采样点与该约束节点在y轴上的相对距离，Dz表示随机采样点与该约束节点在垂直方向上的距离。

例如，随机采样点为rand_pt ，节点列表中的任意一点为nodes_pt ，则这两个节点之间的Dx、Dy 以及Dz 可以通过以下公式计算：

Dx = fabs(rand_pt.x–nodes_pt.x)

Dy = fabs(rand_pt.y–nodes_pt.y) （6）

Dz = fabs(rand_pt.z–nodes_pt.z)

其中，fabs()为取绝对值函数，随机采样点为rand_pt，节点列表中的任意一点为nodes_pt。

步骤S3143：选取最小的欧式距离对应的约束节点为最小约束节点。即，选取与随机采样节点间欧式距离最小的约束节点作为最小约束节点。

本发明实施例中，在确定每一个约束节点与随机采样点之间的欧式距离后，选取其中到采样点欧式距离最小值所对应的约束节点，记该约束节点为最小约束节点near_pt，其对应的欧式距离称为最小的欧式距离，记作min_dist。

步骤S3144：在随机采样点以及最小约束节点的连线中以连接步长确定搜索节点。

本发明实施例中，在随机采样点rand_pt以及最小约束节点near_pt的连线中，从最小约束节点到随机采样点的方向截取连接步长step确定搜索节点new_pt。其中，可根据不同的场景对step取值进行调整。

本发明实施例中，若最小的欧式距离大于连接步长，则根据随机采样点的坐标以及最小约束节点的坐标以及最小的欧式距离，确定搜索节点的坐标，若最小的欧式距离小于或等于连接步长，则该随机采样点即为搜索节点。

例如，可以通过如下步骤，确定搜索节点：

如果 min_dist > step，

new_pt.x=near_pt.x+((rand_pt.x-near_pt.x)*step)/ min_dist

new_pt.y=near_pt.y+((rand_pt.y-near_pt.y)*step)/ min_dist （7）

new_pt.z=near_pt.z+((rand_pt.z-near_pt.z)*step)/ min_dist

否则 new_pt = rand_pt。

其中，new_pt为搜索节点，step为用于截取near_pt与rand_pt连线的连接步长。

本发明实施例中，通过以上方法确定搜索节点，提升了生成搜索节点的效率和准确率，避免生成不满足动力学约束的搜索节点，从而提升了路径规划的整体效率。

在本发明的一个实施例中，如图9所示，步骤S315可以包括但不限于以下步骤：

步骤S3151：在所述节点列表中确定所述搜索节点对应的父节点。

本发明实施例中，某节点的父节点是指：当某一个节点N存在若干个父节点的备选项时，可以分别使得这些节点作为节点N的临时父节点，能够使得节点N代价值最小的临时父节点，则称之为节点N的父节点，即，该节点的真正父节点。

本发明实施例中，可以将最小约束节点作为搜索约束节点，并在所述节点列表中确定与所述搜索节点满足所述动力学约束且与所述搜索节点的欧式距离小于重连接距离的至少一个搜索约束节点，分别确定每一搜索约束节点作为所述搜索节点的临时父节点时，所述搜索节点的代价值，选择所述搜索节点的代价值最小时所对应的搜索约束节点作为所述搜索节点的父节点，即该搜索节点的真正父节点。

需要指出的是，在所述节点列表中确定与所述搜索节点满足所述动力学约束且与所述搜索节点的欧式距离小于重连接距离的搜索约束节点中，可能不包括最小约束节点，若包括该最小约束节点，由于该最小约束节点已经作为搜索约束节点，则无需再次将该最小约束节点作为搜索约束节点。

本发明实施例中，在所述节点列表中确定与所述搜索节点满足所述动力学约束且与所述搜索节点的欧式距离小于重连接距离的搜索约束节点时，可以计算节点列表中的任意一点与该搜索节点是否满足动力学约束，对于满足动力学约束的节点，分别计算每一节点与该搜索节点的搜索点欧式距离，从中确定与搜索节点间的欧式距离小于重连接距离的节点，这些节点以及最小约束节点组成搜索约束节点。

本发明实施例中，并不仅限于先确定与该搜索节点满足动力学约束的节点，然后从中确定与搜索节点的欧式距离小于重连接距离的节点，也可以先从节点列表中确定与该搜索节点的欧式距离小于重连接距离的节点，然后从中确定与该搜索节点满足动力学约束的节点。

需要说明的是，若节点列表中除最小约束节点之外的其他节点与搜索节点均不满足动力学约束，则直接将该最小约束节点作为该搜索节点的父节点。

本发明实施例中，节点与搜索节点之间的欧式距离的计算方法可以参照采样点欧式距离的计算方法。重连接距离reconnect_dis的计算公式如下：

reconnect_dis = search_radius * sqrt(ln(nodes_num) / nodes_num) （8）

其中，search_radius为可调常量，也可以表示为调整系数，可以预设置。nodes_num 为节点列表中节点的数量。

在确定搜索约束节点（包括最小约束节点以及从节点列表中确定与搜索节点满足动力学约束且与搜索节点的欧式距离小于重连接距离的节点）后，分别确定每一个搜索约束节点作为该搜索节点的临时父节点时，该搜索节点的代价值，从中选择搜索节点的代价值最小时所对应的搜索约束节点作为所述搜索节点的父节点。例如，在计算出每一个搜索约束节点分别作为搜索节点的临时父节点时，搜索节点的代价值new_pt.cost，从中找出搜索节点为最小代价值min_reconnect_cost时对应的搜索约束节点，该搜索约束节点为该搜索节点的父节点。

本发明实施例中，可以根据每一搜索约束节点与所述搜索节点的欧式距离以及每一搜索约束节点的代价值确定所述搜索节点的代价值。

某一个节点的代价值计算方法如下：

n_value = n_dist + np_value （9）

其中，n_value 为某节点的父节点，n_dist为该节点与其父节点之间的欧式距离，np_value为该节点的父节点的代价值。

需要指出的是，节点列表中的任意一个节点都有自己的代价值，对于加入至节点列表中的第一个节点：预测位置，可以预设置其代价值为0，对于第二个加入至节点列表的节点，其代价值可以根据其与预测位置之间的欧式距离加上0（即预测位置的代价值）得到。

步骤S3152：判断所述搜索节点与对应的父节点之间的路径是否安全。

需要说明的是，在确定搜索节点的父节点后，判断搜索节点与对应的父节点之间的路径是否安全时，可以参考步骤S23的方法判断搜索节点与对应的父节点连线之间路径是否为安全路径的方法。

若安全，执行S3153，否则执行S3154。

步骤S3153：将所述搜索节点存入所述节点列表。

本发明实施例中，将搜索节点存入所述节点列表时，可以按照各个节点的存入顺序依次排列各个节点。

需要指出的是，对于节点列表中除去初始节点，即预测位置之外的其他节点，都存在其对应的父节点，而且这些节点可以不存在对应的子节点。节点列表中的任意一个节点可以存在多个子节点。

步骤S3154：舍弃该搜索节点，在采样区域重新生成随机采样点，以确定新的搜索节点。

若搜索节点与对应的父节点之间的路径不安全，则舍弃该搜索节点，并重新按照步骤S313至S314的方法生成新的随机采样点，确定新的搜索节点。

本发明实施例中，通过以上方法确定搜索节点的父节点，提升了路径规划的成功率以及效率。

图10是本发明实施例提供的一节点位置关系图。其中，随机采样点为rand_pt，节点列表中的任意一点为nodes_pt，节点列表中nodes_pt _p为nodes_pt的父节点，节点列表中nodes_pt_ pp为nodes_pt_p的父节点，将以上四个点的连线， nodes_pt_ pp、nodes_pt_p以及nodes_pt组成的夹角的内切圆的切线速度为Vector1，nodes_pt_p、near_pt以及rand_pt组成的夹角的内切圆的切线速度为Vector2。其中：

Vector1 = [nodes_pt.x - nodes_pt _p.x, nodes_pt.y-nodes_pt _p.y]

Vector2 = [rand_pt.x - nodes_pt.x, rand_pt.y-nodes_pt.y] （10）

动力学约束包括但不限于以下约束：

1.最小偏航角yaw约束：是指节点nodes_pt、nodes_pt_p和nodes_pt_ pp组成的夹角大于或等于某一个预设角度，例如：120°。

2.最大速度方向变化约束：是指上述Vector1和Vector2方向的角度变化小于预设的最大速度方向变化值，例如，30°。

3.最大俯仰角θ约束：是指由两个节点坐标确定的俯仰角θ小于或等于预设角度，例如，10°。其中，

θ = arctan(fabs(dz) / sqrt(dx * dx + dy * dy)

其中dz = rand_pt.z -nodes_pt.z

dx = rand_pt.x -nodes_pt.x

dy = rand_pt.y -nodes_pt.y （11）

其中，fabs()为取绝对值函数，sqrt()为取平方根函数。若rand_pt与nodes_pt满足最大俯仰角约束，则nodes_pt为rand_pt的约束节点。

需要指出的是，判断某节点与其父节点之间是否满足动力学约束时，可以以当前位置为初始节点的父节点，若能够进行最小偏航角yaw约束、最大速度方向变化约束以及最大俯仰角θ约束的判断，则进行这些约束中的至少一个的判断，否则对能够进行的约束进行判断。例如，对于节点列表中的初始节点，即预测位置，可以以当前位置为其父节点，判断两者之间是否满足动力学约束中的最大俯仰角θ约束，对于节点列表中以预测位置为父节点的节点，可以判断两者之间是否满足动力学约束中的最大俯仰角θ约束和最小偏航角约束。

在确定随机采样点的约束节点时，根据上述动力学约束轮询节点列表中的每一节点，确定随机采样点的约束节点。在节点列表中确定与所述搜索节点满足所述动力学约束的至少一个搜索约束节点，以及在节点列表中确定目标点的父节点的方法都可以参照此方法。

本发明实施例中，通过动力学约束确定搜索节点，避免了生成无效的搜索节点，提升路径规划的效率。

可选的，在本发明的一个实施例中，步骤S32具体可以包括：

在所述节点列表中确定所述目标点的父节点，从所述目标点开始按照节点与父节点的关系顺序，依序提取所述节点列表中的节点，并以所述节点为路径节点建立所述路径规划列表。

确定目标点的父节点，可以按照步骤S3151的方法确定目标点的父节点，即，分别计算出节点列表中各个节点与该目标点满足动力学约束的目标约束节点，然后分别确定每一目标约束节点与该目标点的欧式距离，选取目标点的代价值最小时对应的目标约束节点为该目标点的父节点，可以参照确定搜索节点的父节点的方法确定目标点的父节点。

或者，也可以直接确定目标点的父节点。当结束节点列表的节点的搜索的条件中包括：搜索节点与所述目标点的欧式距离小于搜索截止距离，和/或搜索节点与所述目标点之间的路径为安全路径，且所述搜索节点与所述目标点之间满足所述动力学约束时，即，若某一搜索节点存入节点列表后，该搜索节点与目标点的欧式距离小于搜索截止距离和/或该搜索节点与目标点之间的路径为安全路径，且满足动力学约束，则可以直接确定该搜索节点为目标点的父节点。

在确定目标点的父节点后，从目标点开始按照节点与父节点的关系顺序，依序提取节点列表中的节点，并以提取的节点为路径节点建立路径规划列表。其中，路径规划列表中各路径节点的顺序为：以目标点为最后一位，依次向前提取各个路径节点对应的父节点。需要指出的是，节点列表中的节点可以多于或等于路径规划列表中的路径节点。

例如，节点列表中的节点为A、B、C、D、G，其中，G是目标点，D是G的父节点，B是C和D的父节点，A是B的父节点，则从目标点G开始，按照节点与父节点的关系，依序提取的节点为G—D—B—A，以G、D、B、A为路径节点建立路径规划列表，建立的路径规划列表中各个路径节点的顺序为A、B、D、G，目标点为最后一个路径节点，节点列表中的C不会提取到路径规划列表中。

本发明实施例中，通过在所述节点列表中确定所述目标点的父节点，从所述目标点开始按照节点与父节点的关系顺序，依序提取所述节点列表中的节点，并以所述节点为路径节点建立所述路径规划列表的方法，基于节点与父节点的关系，对节点列表中的节点进行提取，避免了冗余的节点造成的规划的路径不精简，造成的资源浪费，不仅提升了路径规划的效率，也提升了路径规划的质量。

可选的，在本发明的一个实施例中，步骤S32还可以包括：

在所述路径规划列表的相邻两个路径节点之间进行等分采样并生成多个等分采样节点，将多个所述等分采样节点分别***对应的相邻两个路径节点之间，生成包括各个路径节点以及所述等分采样节点的排列顺序的路径规划列表，以所述路径规划列表中的所述目标点为当前节点开始，执行如下步骤：

步骤A：按照所述排列顺序搜索路径节点以及所述等分采样节点，将与当前节点满足动力学约束且距离最远的路径节点或等分采样节点作为该当前节点的新父节点；

步骤B：判断所述新父节点是否为所述路径规划列表的最后一个所述路径节点；

步骤C：若不是，则以所述新父节点为当前节点，继续执行步骤A；

步骤D：若是，则以所述目标点以及所确定的多个所述新父节点，按照节点与父节点的关系顺序，更新所述路径规划列表。

具体的，对于生成的路径规划列表，对该列表中的每两个相邻的路径节点组成的线段进行中等分采样，每段采样数量sample_num可以预设置，例如，sample_num可以为5。把每段的线段中生成的等分采样点***到对应的相邻两个路径节点之间，生成包括各个路径节点以及等分采样点的排列顺序的路径规划列表，即得到新的路径节点插值过后的新路径节点列表。对于新路径节点列表中的每一个路径节点，都只能与其相邻的路径节点相连。使用贪心剪枝算法，以新路径节点列表的最后一个路径节点wp_last（可以为目标点）为当前节点开始搜索，找到与该当前节点满足动力学约束的最远的路径节点wp_last_p，将该点作为wp_last的新的父节点，并舍弃wp_last节点与wp_last_p节点之间的节点。然后以wp_last_p为当前节点，继续向前搜索，重复上述操作，找到与其满足动力学约束的最远的路径节点，作为wp_last_p的新的父节点。依次类推，直到找到新路径节点列表中的最后一个节点，剪枝结束，并将新路径节点列表中确定的全部路径节点依次按照节点与父节点的关系顺序放入路径规划列表。

需要指出的是，更新的路径规划列表中，各个路径节点的顺序为，以目标点为最后一位，依次向前提取各个路径节点对应的父节点。需要指出的是，插值后的路径规划列表中路径节点的数量可以多于或等于剪枝后的路径规划列表中的路径节点。

根据本发明实施例的实时路径规划方法，通过在路径节点进行插值并使用贪心剪枝算法优化生成最终的路径，有效的提高算法的平滑性，更利于飞行器100对于行进路线的跟踪飞行。

可选的，在本发明的一个实施例中，步骤S33具体可以包括：

1.将路径规划列表中的路径节点的水平坐标系的坐标转换为地理坐标系的坐标。

具体的，在建立路径规划列表后，将路径规划列表中的每一路径节点的地理坐标转换为以当前位置为原点的水平坐标的计算公式如下：

Path_coord.lat = (Path_position.y / R2) + Now_coord.lat

Path_coord.lon = (Path _postion.x / LonToM) + Now_coord.lon （12）

Path_coord.high = Path_positon.z

其中， R2为极半径，Path _position为路径节点在以当前位置为原点的水平坐标系下的坐标，Path_coord为该路径节点的地理坐标，Now_coord为当前位置的地理坐标。

其中，LonToM 为转换系数，LonToM = (R1 *π/ 180) * cos(Local_coord.lat *π/ 180)，R1为地球赤道半径，π为圆周率。

2.根据所述路径规划列表生成所述局部行进路线的输出结果。

确定所述路径规划列表中路径节点的数目，若确定路径节点超过路径节点阈值N个，则将前N个路径节点坐标作为输出结果，用于规划路线。其中，N为大于0 的正整数，例如，N可以为3，N的具体数值可根据实际情况设定。若确定路径节点小于等于N个，则将路径节点坐标全部作为输出结果，用于规划路线。

通过设置上述路径节点阈值，实现在每个规划周期内，将生成的路径规划列表中的前N个路径节点作为输出结果Planning_result，这样保证尽可能高的数据接收成功率。

可选的，输出结果可以包括：当前行进路线是否安全，以及是否需要进入辅助路径规划模式，以及生成的路径节点的数量，以及各路径节点的地理坐标系的坐标。具体的，若当前行进路线不安全，则需要进入辅助路径规划模式，地理坐标系的坐标可以包括纬度、经度、海拔高度。

3.等待时间戳更新至发送时刻，发送输出结果。通过采用规划时间作为本发明实施例的实时路径规划方法结束的截止条件，能够避免在有限的通信周期内无法生成合适的航线的问题。

需要指出的是，导航模块122可以向控制模块121发送该输出结果。

根据本发明实施例的实时路径规划方法，飞行器100能够实时判断当前行进路线是否安全，并实时规划更新局部行进路线，当飞行器100判断当前行进路线不安全时，则进入辅助路径规划模式，同时控制器根据规划的局部行进路线对行进方向进行实时的调整，避免了碰撞事故的发生。

本发明实施例通过滚动时域控制策略，能够根据可移动载具的速度实时预测下一时刻的位置，实现了路径的实时规划。并且还采用了目标偏向概率采样策略进行节点采样，提高了算法的收敛速度，同时考虑可移动载具的运动学约束，包括最小偏航角、最大俯仰角、最大速度方向变化率等，限制采样范围，提高最终生成路径的可跟踪性，大大提高了搜索成功率，与传统路径规划算法相比，避免将大量搜索资源浪费在计算约束上。生成规划路径后，本发明还使用了路径节点插值的贪心剪枝算法优化最终生成的路径，提高算法的平滑性，更利于可移动载具的跟踪行进。与此同时，以规划时间戳作为算法结束的截止条件，避免在有限的通信周期内无法生成合适的行进路线的问题。

依据本发明实施例的实时路径规划方法，能够实时判断可移动载具的行进路线是否安全，并且在当前行进路线存在碰撞风险时，能够高效且精准的规划布局行进路线，有效的避免了可移动载具的碰撞风险，大大提高了行进的安全性。

上文描述了根据本发明实施例的实时路径规划方法，下面分别结合图11至图13描述根据本发明实施例的实时路径规划装置，可移动载具及计算机可读存储介质。

图11示出根据本发明实施例提供的实时路径规划装置200的结构示意图。该装置200 例如可以为图1中所示的实时路径规划装置120。如图11所示，实时路径规划装置200，用于可移动载具，包括：

获取模块210，用于获取可移动载具的当前行进数据，行进路况信息以及全局DEM数据；

判断模块220，用于根据当前行进数据，全局DEM数据以及行进路况信息，判断当前行进路线是否安全；

确定模块230，用于在所述当前行进路线不安全时，根据可移动载具的当前行进数据以及动力学约束规划局部行进路线；

控制模块240，用于控制可移动载具根据局部行进路线行进。

可选的，获取模块210以及控制模块240例如可以设置在图1中所示的控制模块121内部，判断模块220和确定模块230例如可以设置在图1中所示的导航模块122内部。

可选的，在本发明的一个实施例中，获取模块210具体用于：获取可移动载具的当前位置，当前行进姿态，当前行进速度，当前行进模式，当前行进路线的起始点以及目标点。

其中，行进路况信息具体可以包括：起始点周围预定范围内禁飞区的信息以及电子围栏的边界坐标。

可选的，在本发明的一个实施例中，所述判断模块220，配置为分别根据起始点以及目标点，在全局DEM数据中确定所述当前行进路线对应的至少一个区块；根据至少一个区块，确定局部DEM 数据，以及根据局部DEM数据，判断当前行进路线是否安全，其中，局部DEM数据均在电子围栏的边界坐标范围内。

可选的，在本发明的一个实施例中，所述判断模块220，配置为根据所述局部DEM数据的分辨率在所述当前行进路线上进行采样，确定多个采样点；分别确定所述起始点，所述目标点以及多个所述采样点相同水平坐标对应的所述局部DEM数据中的地表高度；分别将所述起始点，所述目标点以及多个所述采样点的海拔高度与对应的所述地表高度进行比较；确定所述起始点，所述目标点以及多个所述采样点中海拔高度高于对应的所述地表高度，且高度差不小于安全阈值的点为安全点；若所述起始点，所述目标点以及多个所述采样点均确定为所述安全点，则确定所述当前行进路线为安全路径。

可选的，在本发明的一个实施例中，确定模块230还用于根据所述当前行进数据以及所述动力学约束，生成路径规划的节点列表；根据所述节点列表建立路径规划列表；以及根据所述路径规划列表生成所述局部行进路线。

可选的，在本发明的一个实施例中，确定模块230还用于：根据滚动时域控制策略以及当前行进数据，确定可移动载具的下一时刻的预测位置；将预测位置存入节点列表；在采样区域生成随机采样点；根据动力学约束以及随机采样点，确定搜索节点；以及将所述搜索节点存入所述节点列表。

可选的，在本发明的一个实施例中，确定模块230还用于：根据当前位置的水平坐标以及目标点的水平坐标，确定水平采样区域；根据水平采样区域以及随机采样策略，在所述水平采样区域内生成随机采样点，并确定所述随机采样点的水平坐标；根据所述局部DEM数据以及所述随机采样点的水平坐标，确定所述随机采样点是否在禁飞区；若是，则根据所述水平采样区域以及随机采样策略重新生成随机采样点，直到重新生成的随机采样点的水平坐标不在禁飞区；若不是，则确定所述随机采样点的高度坐标。

可选的，在本发明的一个实施例中，确定模块230还用于根据所述随机采样点的水平坐标，所述局部DEM数据中海拔最高值，所述当前位置的高度坐标以及所述目标点的高度坐标，确定所述随机采样点的高度坐标。

可选的，在本发明的一个实施例中，确定模块230还用于：在节点列表中确定与所述随机采样点满足所述动力学约束的至少一个约束节点；分别确定所述至少一个约束节点与所述随机采样点的采样点欧式距离；选取最小采样点欧式距离对应的所述约束节点为最小约束节点；在所述随机采样点以及所述最小约束节点的连线中以连接步长确定搜索节点。

可选的，在本发明的一个实施例中，确定模块230还用于：在所述节点列表中确定所述搜索节点对应的父节点；判断所述搜索节点与对应的父节点之间的路径是否安全；若安全，将所述搜索节点存入所述节点列表；若不安全，则舍弃该搜索节点，在采样区域重新生成随机采样点，以确定新的搜索节点。

可选的，在本发明的一个实施例中，动力学约束至少包括以下的一项或多项：最小偏航角约束，最大速度方向变化约束以及最大俯仰角约束。

可选的，在本发明的一个实施例中，确定模块230还用于：将所述最小约束节点作为搜索约束节点，并在所述节点列表中确定与所述搜索节点满足所述动力学约束且与所述搜索节点的欧式距离小于重连接距离的至少一个搜索约束节点；分别确定每一搜索约束节点作为所述搜索节点的临时父节点时，所述搜索节点的代价值；选择所述搜索节点的代价值最小时所对应的搜索约束节点作为所述搜索节点的父节点。

可选的，在本发明的一个实施例中，确定模块230还用于根据每一搜索约束节点与所述搜索节点的欧式距离以及每一搜索约束节点的代价值确定所述搜索节点的代价值。

可选的，在本发明的一个实施例中，所述确定模块230还用于将迭代次数加1；判断是否满足结束搜索节点存入节点列表的条件；若满足，将所述目标点存入所述节点列表；否则，将所述搜索节点存入节点列表，并在采样区域重新生成随机采样点，以确定新的搜索节点，并将新的搜索节点存入所述节点列表，直到满足结束搜索节点存入节点列表的条件；

其中，结束搜索节点存入节点列表的条件包括以下至少一个：所述迭代次数大于最大迭代次数；所述搜索节点与所述目标点的欧式距离小于搜索截止距离；所述搜索节点与所述目标点之间的路径为安全路径，且所述搜索节点与所述目标点之间满足所述动力学约束。

可选的，在本发明的一个实施例中，所述确定模块230还用于：在所述节点列表中确定所述目标点的父节点；从所述目标点开始按照节点与父节点的关系顺序，依序提取所述节点列表中的节点，并以所述节点为路径节点建立所述路径规划列表。

可选的，在本发明的一个实施例中，所述确定模块230还用于：在所述路径规划列表的相邻两个路径节点之间进行等分采样并生成多个等分采样节点；将多个所述等分采样节点分别***对应的相邻两个路径节点之间，生成包括各个路径节点以及所述等分采样节点的排列顺序的路径规划列表；以所述路径规划列表中的所述目标点为当前节点开始，执行如下步骤：

步骤A：按照所述排列顺序搜索路径节点以及所述等分采样节点，将与当前节点满足动力学约束且距离最远的路径节点或等分采样节点作为该当前节点的新父节点；

步骤B：判断所述新父节点是否为所述路径规划列表的最后一个所述路径节点；

步骤C：若不是，则以所述新父节点为当前节点，继续执行步骤A；

步骤D：若是，则以所述目标点以及所确定的多个所述新父节点，按照节点与父节点的关系顺序，更新所述路径规划列表。

可选的，在本发明的一个实施例中，所述确定模块230还用于：将所述路径规划列表中的路径节点的水平坐标系的坐标转换为地理坐标系的坐标；根据所述路径规划列表生成所述局部行进路线的输出结果；等待时间戳更新至发送时刻，发送所述输出结果。

可选的，在本发明的一个实施例中，所述输出结果包括：所述当前行进路线是否安全；以及是否需要进入所述辅助路径规划模式；以及生成的所述路径节点的数量；以及各所述路径节点的地理坐标系的坐标。

可选的，在本发明的一个实施例中，可移动载具包括以下任意之一：载人飞行器，无人飞行器（无人机），机器人，汽车，船。

图12示出根据本发明实施例提供的可移动载具300的结构示意图。可移动载具300可以包括：实时路径规划装置310与动力装置320。实时路径规划装置310可以如上述实施例所述的实时路径规划装置。动力装置320可以如图1中所示的动力装置110。实时路径规划装置310用于向动力装置320发送指令，动力装置320用于根据该指令产生动力，使得可移动载具300按照该实时路径规划装置310确定的规划路径行进，以实现安全行驶。

本发明实施例的实时路径规划方法，装置以及可移动载具，根据全球范围内的DEM数据，能够实时的判断当前行进路程是否有碰撞风险。当存在碰撞风险时，通过结合可移动载具的动力学约束以及路径规划算法规划新的行进路线，为可移动载具提供辅助驾驶功能，并将规划的结果截取预设数量的路径节点，保证了尽可能高的数据接收成功率。

本发明实施例的实时路径规划方法，获取所述可移动载具的当前行进数据，行进路况信息以及全局DEM数据；根据所述当前行进数据，所述全局DEM数据以及所述行进路况信息，判断当前行进路线是否安全；若不安全，则根据所述可移动载具的当前行进数据以及动力学约束规划局部行进路线；控制所述可移动载具根据所述局部行进路线行进，能够高效平稳的动态的规划局部行进路线，从而实现了对可移动载具的精确控制，避免了可移动载具进入危险区域。

本发明实施例通过滚动时域控制策略，能够根据可移动载具的速度实时预测下一时刻的位置，实现了路径的实时规划。并且还采用了目标偏向概率采样策略进行节点采样，提高了算法的收敛速度，同时考虑可移动载具的运动学约束，包括最小偏航角、最大俯仰角、最大速度方向变化率等，限制采样范围，提高最终生成路径的可跟踪性，大大提高了搜索成功率，与传统路径规划算法相比，避免将大量搜索资源浪费在计算约束上。生成规划路径后，本发明还使用了路径节点插值的贪心剪枝算法优化最终生成的路径，提高算法的平滑性，更利于可移动载具的跟踪行进。与此同时，以规划时间戳作为算法结束的截止条件，避免在有限的通信周期内无法生成合适的行进路线的问题。

依据本发明实施例的实时路径规划方法，装置以及可移动载具，能够实时判断可移动载具的行进路线是否安全，并且在当前行进路线存在碰撞风险时，能够高效且精准的规划布局行进路线，有效的避免了可移动载具的碰撞风险，大大提高了行进的安全性。

本发明实施例还提供了一种计算机可读存储介质，位于如图13所示的计算机设备400中，图13是示出能够实现根据本发明实施例的实时路径规划方法，装置以及可移动载具的计算设备的示例性硬件架构的结构图。

如图13所示，计算设备400包括输入设备401、输入接口402、中央处理器403、计算机可读存储介质404、输出接口405、以及输出设备406。其中，输入接口402、中央处理器403、计算机可读存储介质404、以及输出接口405通过总线410相互连接，输入设备401和输出设备406分别通过输入接口402和输出接口405与总线410连接，进而与计算设备400的其他组件连接。

具体地，输入设备401接收来自外部的输入信息，并通过输入接口402将输入信息传送到中央处理器403；中央处理器403基于计算机可读存储介质404中存储的计算机可执行指令对输入信息进行处理以生成输出信息，将输出信息临时或者永久地存储在计算机可读存储介质404中，然后通过输出接口405将输出信息传送到输出设备406；输出设备406将输出信息输出到计算设备400的外部供用户使用。

在一个实施例中，图13所示的计算设备400可以被实现为一种实时路径规划装置，该实时路径规划装置可以包括：存储器，被配置为存储程序；处理器，被配置为运行存储器中存储的程序，以执行上述实施例描述的实时路径规划方法。

根据本发明的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本发明的实施例包括一种计算机程序产品，其包括有形地包含在机器可读介质上的计算机程序，计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以从网络上被下载和安装，和/或从可拆卸存储介质被安装。

在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。计算机程序产品包括一个或多个计算机指令，当其在计算机上运行时，使得计算机执行上述各个实施例中描述的方法。在计算机上加载和执行计算机程序指令时，全部或部分地产生按照本发明实施例的流程或功能。计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线（例如同轴电缆、光纤、数字用户线（DSL））或无线（例如红外、无线、微波等）方式向另一个网站站点、计算机、服务器或数据中心进行传输。计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质（例如固态硬盘）等。

以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使对应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (18)

1.一种实时路径规划方法，用于可移动载具，其特征在于，所述方法包括：

获取所述可移动载具的当前行进数据，行进路况信息以及全局DEM数据；

根据所述当前行进数据，所述全局DEM数据以及所述行进路况信息，判断当前行进路线是否安全；

若不安全，则根据所述可移动载具的当前行进数据以及动力学约束规划局部行进路线；

控制所述可移动载具根据所述局部行进路线行进；

其中，所述根据所述可移动载具的当前行进数据以及动力学约束规划局部行进路线，包括：根据所述当前行进数据以及所述动力学约束，生成路径规划的节点列表；

所述根据所述当前行进数据以及所述动力学约束，生成路径规划的节点列表，包括：根据滚动时域控制策略以及所述当前行进数据，确定所述可移动载具的下一时刻的预测位置；将所述预测位置存入所述节点列表；在采样区域生成随机采样点；根据所述动力学约束以及所述随机采样点，确定搜索节点；将所述搜索节点存入所述节点列表；

所述根据所述动力学约束以及所述随机采样点，确定搜索节点，包括：在节点列表中确定与所述随机采样点满足所述动力学约束的至少一个约束节点；分别确定所述至少一个约束节点与所述随机采样点之间的欧式距离；选取最小的欧式距离对应的所述约束节点为最小约束节点；在所述随机采样点以及所述最小约束节点的连线中以连接步长确定搜索节点。

2.根据权利要求1所述的实时路径规划方法，其特征在于，所述获取所述可移动载具的当前行进数据，包括：获取所述可移动载具的当前位置，当前行进姿态，当前行进速度，当前行进模式，当前行进路线的起始点以及目标点；

所述行进路况信息包括：所述起始点周围预定范围内禁飞区的信息以及电子围栏的边界坐标。

3.根据权利要求2所述的实时路径规划方法，其特征在于，所述根据所述当前行进数据，所述全局DEM数据以及所述行进路况信息，判断当前行进路线是否安全，包括：

分别根据所述起始点以及所述目标点，在所述全局DEM数据中确定所述当前行进路线对应的至少一个区块；

根据所述至少一个区块，确定局部DEM 数据；

根据所述局部DEM数据，判断所述当前行进路线是否安全。

4.根据权利要求3所述的实时路径规划方法，其特征在于，所述根据所述局部DEM数据，判断所述当前行进路线是否安全，具体包括：

根据所述局部DEM数据的分辨率在所述当前行进路线上进行采样，确定多个采样点；

分别确定所述起始点，所述目标点以及多个所述采样点相同水平坐标对应的所述局部DEM数据中的地表高度；

分别将所述起始点，所述目标点以及多个所述采样点的海拔高度与对应的所述地表高度进行比较；

确定所述起始点，所述目标点以及多个所述采样点中海拔高度高于对应的所述地表高度，且高度差不小于安全阈值的点为安全点；

若所述起始点，所述目标点以及多个所述采样点均确定为所述安全点，则确定所述当前行进路线为安全路径。

5.根据权利要求3所述的实时路径规划方法，其特征在于，所述根据所述可移动载具的当前行进数据以及动力学约束规划局部行进路线，还包括：

根据所述节点列表建立路径规划列表；

根据所述路径规划列表生成所述局部行进路线。

6.根据权利要求5所述的实时路径规划方法，其特征在于，在采样区域生成随机采样点，包括：

根据当前位置的水平坐标以及目标点的水平坐标，确定水平采样区域；

根据所述水平采样区域以及随机采样策略，在所述水平采样区域内生成随机采样点，并确定所述随机采样点的水平坐标；

根据所述局部DEM数据以及所述随机采样点的水平坐标，确定所述随机采样点是否在禁飞区；

若是，则根据所述水平采样区域以及随机采样策略重新生成随机采样点，直到重新生成的随机采样点的水平坐标不在禁飞区；

若不是，则确定所述随机采样点的高度坐标。

7.根据权利要求6所述的实时路径规划方法，其特征在于，所述确定所述随机采样点的高度坐标，包括：

根据所述随机采样点的水平坐标，所述局部DEM数据中海拔最高值，所述当前位置的高度坐标以及所述目标点的高度坐标，确定所述随机采样点的高度坐标。

8.根据权利要求5所述的方法，其特征在于，将所述搜索节点存入所述节点列表，包括：

在所述节点列表中确定所述搜索节点对应的父节点；

判断所述搜索节点与对应的父节点之间的路径是否安全；

若安全，将所述搜索节点存入所述节点列表；

若不安全，则舍弃该搜索节点，在采样区域重新生成随机采样点，以确定新的搜索节点。

9.根据权利要求8所述的实时路径规划方法，其特征在于，所述在所述节点列表中确定所述搜索节点对应的父节点，包括：

将所述最小约束节点作为搜索约束节点，并在所述节点列表中确定与所述搜索节点满足所述动力学约束且与所述搜索节点的欧式距离小于重连接距离的至少一个搜索约束节点；

分别确定每一搜索约束节点作为所述搜索节点的临时父节点时，所述搜索节点的代价值；

选择所述搜索节点的代价值最小时所对应的搜索约束节点作为所述搜索节点的父节点。

10.根据权利要求9所述的实时路径规划方法，其特征在于，分别确定每一搜索约束节点作为所述搜索节点的临时父节点时，所述搜索节点的代价值，包括：

根据每一搜索约束节点与所述搜索节点的欧式距离以及每一搜索约束节点的代价值确定所述搜索节点的代价值。

11.根据权利要求8-10任一项所述的实时路径规划方法，其特征在于，将所述搜索节点存入所述节点列表，包括：

将迭代次数加1；

判断是否满足结束搜索节点存入节点列表的条件；

若满足，将所述目标点存入所述节点列表；

否则，将所述搜索节点存入节点列表，并在采样区域重新生成随机采样点，以确定新的搜索节点，并将新的搜索节点存入所述节点列表，直到满足结束搜索节点存入节点列表的条件；

其中，结束搜索节点存入节点列表的条件包括以下至少一个：

所述迭代次数大于最大迭代次数；

所述搜索节点与所述目标点的欧式距离小于搜索截止距离；

所述搜索节点与所述目标点之间的路径为安全路径，且所述搜索节点与所述目标点之间满足所述动力学约束。

12.根据权利要求11所述的实时路径规划方法，其特征在于，所述根据所述节点列表建立路径规划列表，包括：

在所述节点列表中确定所述目标点的父节点；

从所述目标点开始按照节点与父节点的关系顺序，依序提取所述节点列表中的节点，并以所述节点为路径节点建立所述路径规划列表。

13.根据权利要求12所述的实时路径规划方法，其特征在于，根据所述节点列表建立路径规划列表，还包括：

在所述路径规划列表的相邻两个路径节点之间进行等分采样并生成多个等分采样节点；

将多个所述等分采样节点分别***对应的相邻两个路径节点之间，生成包括各个路径节点以及所述等分采样节点的排列顺序的路径规划列表；

以所述路径规划列表中的所述目标点为当前节点开始，执行如下步骤：

步骤A：按照所述排列顺序搜索路径节点以及所述等分采样节点，将与当前节点满足动力学约束且距离最远的路径节点或等分采样节点作为该当前节点的新父节点；

步骤B：判断所述新父节点是否为所述路径规划列表的最后一个所述路径节点；

步骤C：若不是，则以所述新父节点为当前节点，继续执行步骤A；

步骤D：若是，则以所述目标点以及所确定的多个所述新父节点，按照节点与父节点的关系顺序，更新所述路径规划列表。

14.根据权利要求12或13所述的实时路径规划方法，其特征在于，所述根据所述路径规划列表生成所述局部行进路线，包括：

将所述路径规划列表中的路径节点的水平坐标系的坐标转换为地理坐标系的坐标；

根据所述路径规划列表生成所述局部行进路线的输出结果；

等待时间戳更新至发送时刻，发送所述输出结果。

15.根据权利要求14所述的实时路径规划方法，其特征在于，所述输出结果包括：

所述当前行进路线是否安全；以及

是否需要进入辅助路径规划模式；以及

生成的所述路径节点的数量；以及

各所述路径节点的地理坐标系的坐标。

16.一种实时路径规划装置，用于可移动载具，其特征在于，包括：

获取模块，用于获取所述可移动载具的当前行进数据，行进路况信息以及全局DEM数据；

判断模块，用于根据所述当前行进数据，所述全局DEM数据以及所述行进路况信息，判断当前行进路线是否安全；

确定模块，用于在所述当前行进路线不安全时，根据所述可移动载具的当前行进数据以及动力学约束规划局部行进路线；

控制模块，用于控制所述可移动载具根据所述局部行进路线行进；

其中，所述确定模块具体用于：根据所述当前行进数据以及所述动力学约束，生成路径规划的节点列表；

所述根据所述当前行进数据以及所述动力学约束，生成路径规划的节点列表，包括：根据滚动时域控制策略以及所述当前行进数据，确定所述可移动载具的下一时刻的预测位置；将所述预测位置存入所述节点列表；在采样区域生成随机采样点；根据所述动力学约束以及所述随机采样点，确定搜索节点；将所述搜索节点存入所述节点列表；

所述根据所述动力学约束以及所述随机采样点，确定搜索节点，包括：在节点列表中确定与所述随机采样点满足所述动力学约束的至少一个约束节点；分别确定所述至少一个约束节点与所述随机采样点之间的欧式距离；选取最小的欧式距离对应的所述约束节点为最小约束节点；在所述随机采样点以及所述最小约束节点的连线中以连接步长确定搜索节点。

17.一种可移动载具，其特征在于，包括：

包括动力装置与如权利要求16所述的实时路径规划装置，其中，所述实时路径规划装置用于向所述动力装置发送指令，所述动力装置用于根据所述指令产生动力，使得所述可移动载具按照所述实时路径规划装置确定的规划路径行进。

18.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机可读存储介质包括指令，当所述指令在计算机上运行时，使得计算机执行如权利要求1-15中任一项所述的实时路径规划方法。

Images