CN117057156B - 一种陆空两栖飞行器路径搜索方法 - Google Patents

Abstract

本发明公开了一种陆空两栖飞行器路径搜索方法，包括：预处理：建立陆‑空模式的统一的能耗评价标准；初始化：对栅格地图进行初始化，将真实环境离散成格栅地图，在格栅中记录环境的信息；设置路径搜索起点和终点：设置路径规划的目标点标记为Target，以飞行器当前位置为起始点标记为Start，初始化Open list和Closed list，将所有被占据的点装入Closed list，将Start装入Open list，计算代价函数；判断Open list是否为空：若Open list为空，则路径规划失败；若Open list不为空，则进行下一步骤；查询代价最小的节点；路径回溯；本发明旨在提供一种陆空两栖飞行器路径搜索方法，在预处理阶段提出了统一能耗评价标准，合理考量陆空飞行器在不同模式下的运动学特性差异，生成的轨迹更加合理。

Description

一种陆空两栖飞行器路径搜索方法

技术领域

本发明涉及无人机技术领域，尤其涉及一种陆空两栖飞行器路径搜索方法。

背景技术

无人机是一种体积小、高灵敏、隐蔽性强的无人驾驶飞行器，在众多领域中被广泛应用。然而，四旋翼无人机运行时需要持续产生巨大推力以对抗重力，续航问题成为了阻碍其在实际应用中推广的重大难题。陆＝空两栖无人机在保留机动性的同时具备低功耗的陆行能力，极大地提高了无人机的续航能力和实际应用价值。但是，现有的陆空两栖无人机产品缺乏自主飞行的能力。其中，设计一种高效的陆空两栖飞行器路径搜索算法，统一评价飞行器的陆空模式能耗标准，节能、高效、快速地完成轨迹搜索是实现其自主化的关键。

现有的陆空两栖飞行器路径搜索方法有Hybrid A*搜索，拓扑路图搜索等方案。

现有的陆-空两栖飞行器路径搜索方法的主要缺点为陆-空模式能耗标准不统一，轨迹质量评价标准单一，容易进行无意义的陆-空模式切换。

Hybrid A*结合车辆动力学模型进行启发式搜索，生成的路径满足车辆的动力学约束，但未考虑陆-空模式的动力学差异。经过改良后的陆空Hybrid A*虽然考虑了不同模式下的能耗差异，但是缺乏统一的能耗评价标准，最终的路径容易产生无意义的陆/空模式切换。此外，Hybrid A*搜索结合动力学计算轨迹的能耗代价，评价轨迹质量的参数单一，面对陆地障碍物，飞行器会陷入加速飞过障碍物或绕远走过障碍物的选择中，而未考虑不同决策下运行时间的差异。拓扑路图搜索虽然可以有效的重构环境结构信息，但是其生成的轨迹质量跟迭代次数相关，而且不包括飞行器的动力学模型，需要施加额外的约束才能被飞行器执行。

发明内容

本发明的目的在于提出一种陆空两栖飞行器路径搜索方法，在预处理阶段提出了统一能耗评价标准，合理考量陆-空飞行器在不同模式下的运动学特性差异，生成的轨迹更加合理。

为达此目的，本发明采用以下技术方案：

一种陆空两栖飞行器路径搜索方法，包括下述步骤：

预处理：建立陆-空模式的统一的能耗评价标准；

初始化：对栅格地图进行初始化，将真实环境离散成格栅地图，在格栅中记录环境的信息；

设置路径搜索起点和终点：设置路径规划的目标点标记为Target，以飞行器当前位置为起始点标记为Start，初始化Open list和Closed list，将所有被占据的点装入Closed list，将Start装入Open list，计算代价函数f_cost；

判断Open list是否为空：若Open list为空，则路径规划失败；若Open list不为空，则进行下一步骤；

查询代价最小的节点：从加入Open list的陆-空节点中查询启发式代价最小的节点；

路径回溯：从目标点进行路径回溯，查找连接起始点Start和目标点Target的符合陆空两栖飞行器动力学模型的离散路径。

所述预处理的步骤中，包括下列子步骤：

子步骤A1：建立陆行模式下飞行器动力学模型；

子步骤A2：建立陆行模式下飞行器加速度-功耗映射函数；

子步骤A3：建立空行模式下飞行器动力学模型；

子步骤A4：建立空行模式下飞行器加速度功能耗映射函数。

所述子步骤A1的步骤中，具体为：

设陆行模式下单侧轮子转速和方向相同，V_c、ω分别为车辆质心的线速度和角速度；L、R分别表示等效后的虚拟左右轮，其速度分别用V₁、V_r表示；d_lr表示虚拟双轮的连线；设等效车辆模型的质心O分布在d_1r的中点上，瞬时旋转中心ICR位于L和R的连线上，转弯半径为R_c；

则，质心O点的角速度ω为：

ω＝(V_r-V_l)/d_lr；

质心O点的线速度V_c为：

V_c＝(V_r+V_l)/2；

联立获得简化模型的逆运动学方程：

单侧轮保持一致，设四个轮子对应的转速为{ω_M1，ω_M2，ω_M3，ω_M4}，轮子半径为R_m，则四轮转速与等效双轮转速应满足以下关系：

圆周运动的转弯半径R_c：

R_c＝V_c/ω；

质心O点的线速度分量为：

对质心O点的线速度分量求导，得到陆行模式下的加速度表达式：

对上式进行小角度近似，即：

其中，Δ_t为单次向前积分时间。

所述子步骤A2的步骤中，具体为：

联立逆运动学方程和动力学方程，得到陆行模式下四轮转速和车辆质心加速度分量的关系：

建立陆行模式下加速度与功率的映射关系：

所述子步骤A3的步骤中，具体为：

飞行模式下的飞行器动力学模型采用四旋翼无人机动力学模型，四旋翼无人机采用X型结构，设F_i、M_i分别为单个电机产生的推力和垂直机架的力矩，且与单个电机转速w_i线性正相关，则：

无人机受四个电机作用下在X、Y轴产生力矩分别为：

受空气阻力的影响四个电机沿Z轴产生力矩为：

M_z＝k(M₁-M₂+M₃-M₄)；其中k为常数；

四个电机在机体平面产生的总推力为：

F＝k_f(M₁+M₂-M₃-M₄)；其中k_f为常数；

根据牛顿第二方程有无人机加速度a及F之间的关系有：

ma＝Fe₃-mg；其中m为无人机质量，g为重力加速度，e₃为机体坐标系Z轴在世界系上的投影；

得出无人机加速度和姿态之间的关系：

其中ψ，θ，φ为欧拉角；

根据刚体欧拉方程，无人机的三轴力矩又与其自身姿态相关，ω为机体坐标系下的角速度：

建立无人机三轴加速度与F、M的映射关系，而F、M与无人机四个旋翼转速又满足以下线性映射：

所述子步骤A4的步骤中，具体为：

联立运动学和动力学方程，进而得到空行模式下飞行器三轴加速度与四个旋翼转速的映射关系，建立空行模式下加速度与功率的转化方程：

在所述设置路径搜索起点和终点的步骤中，计算代价函数f_cost的公式为：

f_cost＝g_cost+h_cost+λ_t(t_g+t_h)；

其中g_cost表示起始点Start经前向积分到当前节点的代价，h_cost表示当前节点到目标点Target的启发式代价函数，λ_t(t_g+t_h)表示时间项惩罚，λ_t为时间惩罚权重，t_g表示前向积分累计时间，t_h表示启发式时间。

在所述查询代价最小的节点的步骤中，包括下述子步骤：

子步骤B1：终点检测：计算当前节点到目标节点Target的欧氏距离D_ct，如果D_ct小于检测阈值，则判定找到目标点，进入路径回溯步骤；否则对当前节点进行前向积分，扩张该节点动力学基础上的邻居；

子步骤B2：判断节点模式：判断节点属于陆行模式还是空行模式，地图中每个栅格都记录了当前栅格中的节点信息，包括了节点的动力学状态信息G_state，G_state包括节点的位置、速度以及控制输入的加速度，然后根据节点Z轴位置判断节点模式，如果节点Z轴高度小于起飞阈值，则判断为陆行模式，进入子步骤B3；否则判定为空行模式，进入子步骤B5；

子步骤B3：陆行模式下的前向积分：前向积分时，对陆行下X最大加速度进行3等分，对Y轴最大加速度进行5等分：

将U_ax和U_ay分别作用在陆行动力学模型上，作用时间为Δt_land，从而得到30个当前节点的邻居节点；

对当前节点所有邻居节点进行第一次级步骤操作，直到所有邻居节点均被遍历进入判断Open list是否为空的步骤：

子步骤B4：陆行模式下对前节点在Z轴正方向上进行一次前向积分，在其f_cost项额外引入模式切换惩罚λ_m，完成后返回判断Open list是否为空的步骤；

子步骤B5：空行模式下的前向积分：在三维空间上进行前向积分，对空行模式下X最大加速度进行3等分，对Y轴最大加速度进行5等分，对Z轴最大加速度进行5等分：

将U_ax、U_ay和U_az分别作用在空行动力学模型上，作用时间为Δt_fly，得到150个当前节点的邻居节点；为确保陆空模式下前向积分步长一致Δt_fly、Δt_land满足如下关系||·||表示取模：

依次对当前节点所有邻居节点进行第二次级步骤操作，直到所有邻居节点均被遍历，返回判断Open list是否为空的步骤。

优选地，在所述子步骤B3的步骤中，具体包括下述第一次级步骤：

次级步骤1：判断该节点是否位于Closed list，如果是，则跳过该节点，否则继续进行下一次级步骤；

次级步骤2：计算该邻居节点的g_cost：

将作用在该邻居节点上的控制输入U_ax、U_ay带入加速度与功率的映射方程，计算当前控制输入下的运行功率

则邻居节点的g_cost,currten满足：

g_cost,currten＝P_currten*Δt_land+g_cost,father；其中g_cost,father表示邻居节点父节点的g_cost；

次级步骤3：计算该邻居节点的t_g：

累计前向积分的运行时间t_g，currten：t_g，currten＝t_g，father+Δt_land；其中t_g，father表示邻居节点父节点的t_g；

次级步骤4：计算该邻居节点的t_h：

t_h,currten可根据下式计算：

其中，Dst表示当前节点到Target的欧氏距离，||a||表示加速度沿Dst方向上的最大分量；

次级步骤5：计算该邻居节点的h_cost：

h_cost,currten＝h_cost，1+h_cost，2；其中，h_cost表示当前节点行进到Target的代价，其由两项组成；第一项为不考虑障碍物但考虑动力学下的h_cost，1，第二项为考虑障碍物但不考虑动力学下的h_cost，2；

次级步骤6：计算当前邻居节点的f_cost,currten：

f_cost,currten＝g_cost,currten+h_cost,currten+λ_t(t_g，currten+t_h,currten)；

次级步骤7：判断当前邻居节点是否在Open list，若是则继续判断f_cost,currten是否小于该节点原来的f_cost，若是则更新该节点的父节点为当前节点；否则进入下一次级步骤；

次级步骤8：更新栅格内的节点信息。

进一步地，在所述子步骤B5的步骤中，具体包括下述第二次级步骤：

次级步骤1：判断该节点是否位于Closed list，如果是，则跳过该节点，否则继续进行下一步骤；

次级步骤2：判断当前节点Z轴高度是否小于0，如果是，则跳过该节点，否则继续进行下一次级步骤；

次级步骤3：计算该邻居节点的g_cost：

将作用在该邻居节点上的控制输入U_ax、U_ay、U_az带入第一步所述的加速度与功率的映射方程，计算当前控制输入下的运行功率P_currten、g_cost,currten、t_g，currten、t_h,currten：

g_cost,currten＝P_currten*Δt_land+g_cost,father；

t_g，currten＝t_g，father+Δt_land；

次级步骤4：计算该邻居节点的h_cost：

h_{cost，currten}＝h_cost，1+h_cost，2；

次级步骤5：计算当前邻居节点的f_{cost，currten}：

f_{cost，currten}＝g_{cost，currten}+h_{cost，currten}+λ_t(t_g，currten+t_h，currten)；

次级步骤6：判断当前邻居节点是否在Open list，若是则判断f_{cost，currten}是否小于该节点原来的f_cost，若是则更新该节点的父节点为当前节点，否则进入下一次级步骤；

次级步骤7：更新栅格内的节点信息。

本发明的一个技术方案的有益效果：本发明主要实现了陆-空两栖飞行器在路径搜索阶段的统一规划，在预处理阶段提出了一种陆-空飞行器统一能耗评价标准，合理考量陆-空飞行器在不同模式下的运动学特性差异，生成的轨迹更加合理。在启发式搜索阶段，引入能量最优、时间最优的决策因子，使生成的轨迹更适合多变的应用场景。采用一种陆行模式前向积分后额外进行一次空行模式的向上扩张的策略，使算法在扩展优先节点的同时具有三维搜索能力，提升了算法的实时性的同时避免了不必要的模式切换。为启发式函数引入两种估计指标，提高了启发式函数的精确度，有效减小的搜索过程中扩展的节点数量。

附图说明

图1是本发明一个实施例的步骤流程示意图；

图2是本发明一个实施例可变结构的陆空两栖四旋翼无人机的结构示意图；

图3是本发明一个实施例陆行模式下的飞行器动力学模型示意图；

图4是本发明一个实施例四旋翼无人机动力学模型示意图；

图5是本发明一个实施例四旋翼无人机平面示意图。

具体实施方式

下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。

下面详细描述本发明的实施例，所述实施例的示例在附图中示出。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

参阅图1至图5所示，一种陆空两栖飞行器路径搜索方法，包括下述步骤：

预处理：建立陆-空模式的统一的能耗评价标准；

初始化：对栅格地图进行初始化，将真实环境离散成格栅地图，在格栅中记录环境的信息；

设置路径搜索起点和终点：设置路径规划的目标点标记为Target，以飞行器当前位置为起始点标记为Start，初始化Open list和Closed list，将所有被占据的点装入Closed list，将Start装入Open list，计算代价函数f_cost；

判断Open list是否为空：若Open list为空，则路径规划失败；若Open list不为空，则进行下一步骤；

查询代价最小的节点：从加入Open list的陆-空节点中查询启发式代价最小的节点；

路径回溯：从目标点进行路径回溯，查找连接起始点Start和目标点Target的符合陆空两栖飞行器动力学模型的离散路径。

本发明提出了一种陆-空飞行器统一能耗评价标准，结合四轮滑动转向驱动车辆动力学模型以及X型四旋翼无人机动力学模型，计算飞行器在不同模式下的加速度与功率映射函数，作为轨迹搜索时的代价评价标准，实现了陆-空飞行器在路径搜索阶段的统一规划。

在启发式代价中引入时间惩罚项，通过调节时间惩罚权重λ_t，可以优化陆空两栖车辆在面临节能优先与时间优先决策时的倾向性问题，弥补了Hybrid A*算法的时间规划能力，提升了算法的鲁棒性，扩展了陆空两栖车辆自主化的应用场景。

同时，采用复合式启发式函数，包括考虑动力学不考虑碰撞的启发式代价以及考虑碰撞不考虑动力学的启发式代价，优化了算法运行速度，提高了算法的实时性。

本发明主要实现了陆-空两栖飞行器在路径搜索阶段的统一规划。在预处理阶段提出了一种陆-空飞行器统一能耗评价标准，合理考量陆-空飞行器在不同模式下的运动学特性差异，生成的轨迹更加合理。在启发式搜索阶段，引入能量最优、时间最优的决策因子，使生成的轨迹更适合多变的应用场景。采用一种陆行模式前向积分后额外进行一次空行模式的向上扩张的策略，使算法在扩展优先节点的同时具有三维搜索能力，提升了算法的实时性的同时避免了不必要的模式切换。为启发式函数引入两种估计指标，提高了启发式函数的精确度，有效减小的搜索过程中扩展的节点数量。

具体地，所述预处理的步骤中，包括下列子步骤：

子步骤A1：建立陆行模式下飞行器动力学模型；

子步骤A2：建立陆行模式下飞行器加速度-功耗映射函数；

子步骤A3：建立空行模式下飞行器动力学模型；

子步骤A4：建立空行模式下飞行器加速度功能耗映射函数。

优选的，所述子步骤A1的步骤中，具体为：

设陆行模式下单侧轮子转速和方向相同，V_c、ω分别为车辆质心的线速度和角速度；L、R分别表示等效后的虚拟左右轮，其速度分别用V₁、V_r表示；d_lr表示虚拟双轮的连线；设等效车辆模型的质心0分布在d_lr的中点上，瞬时旋转中心ICR位于L和R的连线上，转弯半径为R_c；

则，质心0点的角速度ω为：

ω＝(V_r-V_l)/d_lr；

质心0点的线速度V_c为：

V_c＝(V_r+V_l)/2；

联立获得简化模型的逆运动学方程：

单侧轮保持一致，设四个轮子对应的转速为{ω_M1，ω_M2，ω_M3，ω_M4}，轮子半径为R_m，则四轮转速与等效双轮转速应满足以下关系：

圆周运动的转弯半径R_c：

R_c＝V_c/ω；

质心O点的线速度分量为：

对质心O点的线速度分量求导，得到陆行模式下的加速度表达式：

对上式进行小角度近似，即：

其中，Δ_t为单次向前积分时间。

具体地，所述子步骤A2的步骤中，具体为：

联立逆运动学方程和动力学方程，得到陆行模式下四轮转速和车辆质心加速度分量的关系：

在确定负载的情况下，陆行电机不同转速下的实际功率可以通过实验进行测量，进而可以计算在指定加速度下的陆行功率，建立陆行模式下加速度与功率的映射关系：

优选的，所述子步骤A3的步骤中，具体为：

飞行模式下的飞行器动力学模型采用四旋翼无人机动力学模型，四旋翼无人机采用X型结构，通过四个螺旋桨来提供推力以及扭力，设F_i、M_i分别为单个电机产生的推力和垂直机架的力矩，且与单个电机转速w_i线性正相关，则：

无人机受四个电机作用下在X、Y轴产生力矩分别为：

受空气阻力的影响四个电机沿Z轴产生力矩为：

M_z＝k(M₁-M₂+M₃-M₄)；其中k为常数；

四个电机在机体平面产生的总推力为：

F＝k_f(M₁+M₂-M₃-M₄)；其中k_f为常数；

根据牛顿第二方程有无人机加速度a及F之间的关系有：

ma＝Fe₃-mg；其中m为无人机质量，g为重力加速度，e₃为机体坐标系Z轴在世界系上的投影；

得出无人机加速度和姿态之间的关系：

其中ψ，θ，φ为欧拉角；

根据刚体欧拉方程，无人机的三轴力矩又与其自身姿态相关，ω为机体坐标系下的角速度：

建立无人机三轴加速度与F、M的映射关系，而F、M与无人机四个旋翼转速又满足以下线性映射：

具体地，所述子步骤A4的步骤中，具体为：

联立运动学和动力学方程，进而得到空行模式下飞行器三轴加速度与四个旋翼转速的映射关系，旋翼电机在不同转速下的功率曲线可以通过实验进行测量，建立空行模式下加速度与功率的转化方程：

对栅格地图进行初始化，Hybrid A*将真实环境离散成栅格地图，每个栅格索引唯一且大小为0.2m*0.2m*0.2m，栅格中记录环境的障碍物信息、节点状态和扩展属性等。首先，设置规划器重要参数，如地图大小、陆空模式下加速度上限、时间惩罚权重、模式切换代价。然后，根据参数设置三维地图边界，将每个节点初始化为无障碍、未被探索的状态。最后，读取传感器点云数据，将环境中的障碍物存入栅格地图。

优选的，在所述设置路径搜索起点和终点的步骤中，计算代价函数f_cost的公式为：

f_cost＝g_cost+h_cost+λ_t(t_g+t_h)

其中g_cost表示起始点Start经前向积分到当前节点的代价，h_cost表示当前节点到目标点Target的启发式代价函数，λ_t(t_g+t_h)表示时间项惩罚，λ_t为时间惩罚权重，t_g表示前向积分累计时间，t_h表示启发式时间，起始节点Start的f_cost可以设置为无穷大。

具体地，在所述查询代价最小的节点的步骤中，该步骤在传统Hybrid A*的基础上引入了第一步中所述的陆空模式加速度-功率映射方程，基于此方程实现了三维地图中复合动力学模型下的Hybrid A*节点扩张，同时考虑到飞行器在陆行模式下功耗低、在空行模式下耗时少，因此在f_cost中引入重要的时间代价惩罚项，通过设置权重使飞行器可以调节能耗最低和时耗最低的倾向性，以应对复杂场景下的不同任务，包括下述子步骤：

子步骤B1：终点检测：计算当前节点到目标节点Target的欧氏距离D_ct，如果D_ct小于检测阈值，则判定找到目标点，进入路径回溯步骤；否则对当前节点进行前向积分，扩张该节点动力学基础上的邻居；

子步骤B2：判断节点模式：判断节点属于陆行模式还是空行模式，地图中每个栅格都记录了当前栅格中的节点信息，包括了节点的动力学状态信息G_state，G_state包括节点的位置、速度以及控制输入的加速度，然后根据节点Z轴位置判断节点模式，如果节点Z轴高度小于起飞阈值，则判断为陆行模式，进入子步骤B3；否则判定为空行模式，进入子步骤B5；

子步骤B3：陆行模式下的前向积分：为减少轨迹搜索时间，陆行模式下在地面上进行二维扩张，同时对当前节点在Z轴正方向上进行一次扩张，确保算法具有起飞绕过地面障碍物的能力；前向积分时，对陆行下X最大加速度进行3等分，对Y轴最大加速度进行5等分：

将U_ax和U_ay分别作用在陆行动力学模型上，作用时间为Δt_land，从而得到30个当前节点的邻居节点；

对当前节点所有邻居节点进行第一次级步骤操作，直到所有邻居节点均被遍历进入判断Open list是否为空的步骤：

子步骤B4：陆行模式下对前节点在Z轴正方向上进行一次前向积分，其运算流程与Step5类似，为抑制频繁的陆空模式切换，在其f_cost项额外引入模式切换惩罚λ_m，完成后返回判断Open list是否为空的步骤；

子步骤B5：空行模式下的前向积分：在三维空间上进行前向积分，对空行模式下X最大加速度进行3等分，对Y轴最大加速度进行5等分，对Z轴最大加速度进行5等分：

将U_ax、U_ay和U_az分别作用在空行动力学模型上，作用时间为Δt_fly，得到150个当前节点的邻居节点；为确保陆空模式下前向积分步长一致Δt_fly、Δt_land满足如下关系||·||表示取模：

依次对当前节点所有邻居节点进行第二次级步骤操作，直到所有邻居节点均被遍历，返回判断Open list是否为空的步骤。

优选的，在所述子步骤B3的步骤中，具体包括下述第一次级步骤：

次级步骤1：判断该节点是否位于Closed list，如果是，则跳过该节点，否则继续进行下一次级步骤；

次级步骤2：计算该邻居节点的g_cost：

将作用在该邻居节点上的控制输入U_ax、U_ay带入加速度与功率的映射方程，计算当前控制输入下的运行功率P_currten：

则邻居节点的g_{cost，currten}满足：

g_cost,currten＝P_currten*Δt_land+g_cost,father；其中g_cost,father表示邻居节点父节点的g_cost；

次级步骤3：计算该邻居节点的t_g：

累计前向积分的运行时间t_g，currten：t_g，currten＝t_g，father+Δt_land；其中t_g，father表示邻居节点父节点的t_g；

次级步骤4：计算该邻居节点的t_h：

t_h,currten可根据下式计算：

其中，Dst表示当前节点到Target的欧氏距离，||a||表示加速度沿Dst方向上的最大分量；

次级步骤5：计算该邻居节点的h_cost：

h_cost,currten＝h_cost，1+h_cost，2；其中，h_cost表示当前节点行进到Target的代价，其由两项组成；第一项为不考虑障碍物但考虑动力学下的h_cost，1，第二项为考虑障碍物但不考虑动力学下的h_cost，2；h_cost，1项可以通过速度约束下的最优边界值问题进行求解；求解的结果与边界约束时间t_h,currten相关；h_cost，2项可以通过无动力学A*找到一条当前邻居节点到目标点的最近路径，计算路径长度作为当前邻居节点的h_cost，2；

次级步骤6：计算当前邻居节点的f_cost,currten：

f_cost,currten＝g_cost,currten+h_cost,currten+λ_t(t_g，currten+t_h,currten)；

次级步骤7：判断当前邻居节点是否在Open list，若是则继续判断f_cost,currten是否小于该节点原来的f_cost，若是则更新该节点的父节点为当前节点；否则进入下一次级步骤；

次级步骤8：更新栅格内的节点信息。

优选的，在所述子步骤B5的步骤中，具体包括下述第二次级步骤：

次级步骤1：判断该节点是否位于Closed list，如果是，则跳过该节点，否则继续进行下一步骤；

次级步骤2：判断当前节点Z轴高度是否小于0，如果是，则跳过该节点，否则继续进行下一次级步骤；

次级步骤3：计算该邻居节点的g_cost：

将作用在该邻居节点上的控制输入U_ax、U_ay、U_az带入第一步所述的加速度与功率的映射方程，计算当前控制输入下的运行功率P_currten、g_{cost，currten}、t_g，currten、t_h，currten：

g_{cost，currten}＝P_currten*Δt_land+g_{cost，father}；

t_g，currten＝t_g，father+Δt_land；

次级步骤4：计算该邻居节点的h_cost：

h_{cost，currten}＝h_cost，1+h_cost，2；

次级步骤5：计算当前邻居节点的f_{cost，currten}：

f_{cost，currten}＝g_{cost，currten}+h_{cost，currten}+λ_t(t_g，currten+t_h，currten)；

次级步骤6：判断当前邻居节点是否在Open list，若是则判断f_{cost，currten}是否小于该节点原来的f_cost，若是则更新该节点的父节点为当前节点，否则进入下一次级步骤；

次级步骤7：更新栅格内的节点信息。

在本说明书的描述中，参考术语“实施例”、“示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上结合具体实施例描述了本发明的技术原理。这些描述只是为了解释本发明的原理，而不能以任何方式解释为对本发明保护范围的限制。基于此处的解释，本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方式，这些方式都将落入本发明的保护范围之内。

Claims (3)

1.一种陆空两栖飞行器路径搜索方法，其特征在于，包括下述步骤：

预处理：建立陆-空模式的统一的能耗评价标准；

初始化：对栅格地图进行初始化，将真实环境离散成格栅地图，在格栅中记录环境的信息；

设置路径搜索起点和终点：设置路径规划的目标点标记为Target，以飞行器当前位置为起始点标记为Start，初始化Open list和Closed list，将所有被占据的点装入Closedlist，将Start装入Open list，计算代价函数f_cost；

判断Open list是否为空：若Open list为空，则路径规划失败；若Open list不为空，则进行下一步骤；

查询代价最小的节点：从加入Open list的陆＝空节点中查询启发式代价最小的节点；

路径回溯：从目标点进行路径回溯，查找连接起始点Start和目标点Target的符合陆空两栖飞行器动力学模型的离散路径；

所述预处理的步骤中，包括下列子步骤：

子步骤Al：建立陆行模式下飞行器动力学模型；

子步骤A2：建立陆行模式下飞行器加速度-功耗映射函数；

子步骤A3：建立空行模式下飞行器动力学模型；

子步骤A4：建立空行模式下飞行器加速度功能耗映射函数；

所述子步骤A1的步骤中，具体为：

设陆行模式下单侧轮子转速和方向相同，V_c、ω分别为车辆质心的线速度和角速度；L、R分别表示等效后的虚拟左右轮，其速度分别用V₁、V_r表示；d_Ir表示虚拟双轮的连线；设等效车辆模型的质心O分布在d_Ir的中点上，瞬时旋转中心ICR位于L和R的连线上，转弯半径为R_c；

则，质心O点的角速度ω为：

ω＝(V_r-V_l)/d_lr；

质心O点的线速度V_c为：

V_c＝(V_r+V_l)/2；

联立获得简化模型的逆运动学方程：

单侧轮保持一致，设四个轮子对应的转速为{ω_M1，ω_M2，ω_M3，ω_M4}，轮子半径为R_m，则四轮转速与等效双轮转速应满足以下关系：

圆周运动的转弯半径R_c：

R_c＝V_c/ω；

质心O点的线速度分量为：

对质心O点的线速度分量求导，得到陆行模式下的加速度表达式：

对上式进行小角度近似，即：

其中，Δ_t为单次向前积分时间；

所述子步骤A2的步骤中，具体为：

联立逆运动学方程和动力学方程，得到陆行模式下四轮转速和车辆质心加速度分量的关系：

建立陆行模式下加速度与功率的映射关系：

所述子步骤A3的步骤中，具体为：

飞行模式下的飞行器动力学模型采用四旋翼无人机动力学模型，四旋翼无人机采用X型结构，设F_i、M_i分别为单个电机产生的推力和垂直机架的力矩，且与单个电机转速w_i线性正相关，则：

无人机受四个电机作用下在X、Y轴产生力矩分别为：

受空气阻力的影响四个电机沿Z轴产生力矩为：

M_z＝k(M₁-M₂+M₃-M₄)；其中k为常数；

四个电机在机体平面产生的总推力为：

F＝k_f(M₁+M₂-M₃-M₄)；其中k_f为常数；

根据牛顿第二方程有无人机加速度a及F之间的关系有：

ma＝Fe₃-mg；其中m为无人机质量，g为重力加速度，e₃为机体坐标系Z轴在世界系上的投影；

得出无人机加速度和姿态之间的关系：

其中ψ，θ，φ为欧拉角；

根据刚体欧拉方程，无人机的三轴力矩又与其自身姿态相关，ω为机体坐标系下的角速度：

建立无人机三轴加速度与F、M的映射关系，而F、M与无人机四个旋翼转速又满足以下线性映射：

所述子步骤A4的步骤中，具体为：

联立运动学和动力学方程，进而得到空行模式下飞行器三轴加速度与四个旋翼转速的映射关系，建立空行模式下加速度与功率的转化方程：

在所述设置路径搜索起点和终点的步骤中，计算代价函数f_cost的公式为：

f_cost＝g_cost+h_cost+λ_t(t_g+t_h)；

其中g_cost表示起始点Start经前向积分到当前节点的代价，h_cost表示当前节点到目标点Target的启发式代价函数，λ_t(t_g+t_h)表示时间项惩罚，λ_t为时间惩罚权重，t_g表示前向积分累计时间，t_h表示启发式时间；

在所述查询代价最小的节点的步骤中，包括下述子步骤：

子步骤B1：终点检测：计算当前节点到目标节点Target的欧氏距离D_ct,如果D_ct小于检测阈值，则判定找到目标点，进入路径回溯步骤；否则对当前节点进行前向积分，扩张该节点动力学基础上的邻居；

子步骤B2：判断节点模式：判断节点属于陆行模式还是空行模式，地图中每个栅格都记录了当前栅格中的节点信息，包括了节点的动力学状态信息G_state，G_state包括节点的位置、速度以及控制输入的加速度，然后根据节点Z轴位置判断节点模式，如果节点Z轴高度小于起飞阈值，则判断为陆行模式，进入子步骤B3；否则判定为空行模式，进入子步骤B5；

子步骤B3：陆行模式下的前向积分：前向积分时，对陆行下X最大加速度进行3等分，对Y轴最大加速度进行5等分：

将U_ax和U_ay分别作用在陆行动力学模型上，作用时间为Δt_land，从而得到30个当前节点的邻居节点；

对当前节点所有邻居节点进行第一次级步骤操作，直到所有邻居节点均被遍历进入判断Open list是否为空的步骤：

子步骤B4：陆行模式下对前节点在Z轴正方向上进行一次前向积分，在其f_cost项额外引入模式切换惩罚λ_m，完成后返回判断Open list是否为空的步骤；

子步骤B5：空行模式下的前向积分：在三维空间上进行前向积分，对空行模式下X最大加速度进行3等分，对Y轴最大加速度进行5等分，对Z轴最大加速度进行5等分：

将U_ax、U_ay和U_az分别作用在空行动力学模型上，作用时间为Δt_fly，得到150个当前节点的邻居节点；为确保陆空模式下前向积分步长一致Δt_fly、Δt_land满足如下关系||·||表示取模：

依次对当前节点所有邻居节点进行第二次级步骤操作，直到所有邻居节点均被遍历，返回判断Open list是否为空的步骤。

2.根据权利要求1所述的一种陆空两栖飞行器路径搜索方法，其特征在于，在所述子步骤B3的步骤中，具体包括下述第一次级步骤：

次级步骤1：判断该节点是否位于Closed list，如果是，则跳过该节点，否则继续进行下一次级步骤；

次级步骤2：计算该邻居节点的g_cost：

将作用在该邻居节点上的控制输入U_ax、U_ay带入加速度与功率的映射方程，计算当前控制输入下的运行功率P_currten：

则邻居节点的g_cost,currten满足：

g_cost,currten＝P_currten*Δt_land+g_cost,father；其中g_cost,father表示邻居节点父节点的g_cost；

次级步骤3：计算该邻居节点的t_g：

累计前向积分的运行时间t_g，currten：t_g，currten＝t_g，father+Δt_land；其中t_g，father表示邻居节点父节点的t_g；

次级步骤4：计算该邻居节点的t_h：

t_h,currten可根据下式计算：

其中，Dst表示当前节点到Target的欧氏距离，||a||表示加速度沿Dst方向上的最大分量；

次级步骤5：计算该邻居节点的h_cost：

h_{cost，currten}＝h_cost，1+h_cost，2；其中，h_cost表示当前节点行进到Target的代价，其由两项组成；第一项为不考虑障碍物但考虑动力学下的h_cost，1，第二项为考虑障碍物但不考虑动力学下的h_cost，2；

次级步骤6：计算当前邻居节点的f_{cost，currten}：

f_{cost，currten}＝g_{cost，currten}+h_{cost，currten}+λ_t(t_g，currten+t_h，currten)；

次级步骤7：判断当前邻居节点是否在Open list，若是则继续判断f_{cost，currten}是否小于该节点原来的f_cost，若是则更新该节点的父节点为当前节点；否则进入下一次级步骤；

次级步骤8：更新栅格内的节点信息。

3.根据权利要求1所述的一种陆空两栖飞行器路径搜索方法，其特征在于，在所述子步骤B5的步骤中，具体包括下述第二次级步骤：

次级步骤1：判断该节点是否位于Closed list，如果是，则跳过该节点，否则继续进行下一步骤；

次级步骤2：判断当前节点Z轴高度是否小于0，如果是，则跳过该节点，否则继续进行下一次级步骤；

次级步骤3：计算该邻居节点的g_cost：

将作用在该邻居节点上的控制输入U_ax、U_ay、U_az带入第一步所述的加速度与功率的映射方程，计算当前控制输入下的运行功率P_currten、g_{cost，currten}、t_g，currten、t_h，currten：

g_{cost，currten}＝P_currten*Δt_land+g_{cost，father}；

t_g，currten＝t_g，father+Δt_land；

次级步骤4：计算该邻居节点的h_cost：

h_{cost，currten}＝h_cost，1+h_cost，2；

次级步骤5：计算当前邻居节点的f_{cost，currten}：

f_{cost，currten}＝g_{cost，currten}+h_{cost，currten}+λ_t(t_g，currten+t_h，currten)；

次级步骤6：判断当前邻居节点是否在Open list，若是则判断f_{cost，currten}是否小于该节点原来的f_cost，若是则更新该节点的父节点为当前节点，否则进入下一次级步骤；

次级步骤7：更新栅格内的节点信息。