一种考虑环境自适应机动的飞行器轨迹规划方法技术

本发明专利技术涉及一种考虑环境自适应机动的飞行器轨迹规划方法，首先，通过路径搜索方法在占据栅格地图中进行全局规划，生成位于较大体积的无障碍物空间的离散路径；随后，对每个路径点周围的安全区域进行建模，并建立低阶MPC轨迹优化模型，进一步生成平滑安全的参考轨迹；最后，基于欧式符号距离场的梯度信息和飞行器的运动状态，建立表征环境自适应行为的机动模型，并建立高阶的MPC轨迹优化模型，生成机动自适应的安全的动态可行的局部轨迹。本发明专利技术利用由“粗”到“细”的三层框架规划轨迹，既有效地提高了飞行器的安全水平，又确保了灵活的机动性，为当前飞行器在复杂环境下自主飞行难以有效平衡快速性与安全性的问题提供了新的解决方法。决方法。决方法。

【技术实现步骤摘要】
一种考虑环境自适应机动的飞行器轨迹规划方法


[0001]本专利技术属于飞行器的轨迹规划领域，具体涉及一种考虑环境自适应机动的飞行器轨迹规划方法。

技术介绍

[0002]近年来，无人机的应用场景日趋复杂，例如工业检测、应急救援、灾后搜救以及矿山勘探等。这些场景对无人机的自主性，安全性以及机动性的要求较高。
[0003]面对未知复杂的现实环境，如何有效地规划出一条合理的轨迹，使得飞行器安全地避开各种障碍物，顺利到达目标位置，而且具备灵活的机动性，是目前飞行器轨迹规划需要深入研究的。现有的方法或是严重依赖于已知环境的先验知识，例如中国专利技术专利CN111707269A《一种三维环境下的无人机路径规划方法》中的方法需要全局障碍物的信息，不适用于未知复杂环境下的任务；或是仅利用与障碍物的距离信息进行避障约束，例如中国专利技术专利CN114594785A《基于混合整数二阶锥优化的无人机避障实时轨迹规划方法》中的方法通过构造与障碍物的距离矩阵、速度矩阵来确定可行运动状态集，飞行器以较快速度飞行时，状态估计、感知和控制上的误差也会增大，降低了安全性；或是基于深度学习，例如中国专利技术专利CN113848974A《一种基于深度强化学习的飞行器轨迹规划方法及系统》中的方法利用长短期记忆网路和深度神经网络组成的模型作为非线性函数拟合器拟合规划策略，方法的性能严重依赖于训练的好坏，且不适用于复杂多变的应用场景。

技术实现思路

[0004]本专利技术为了解决上述问题，提供了一种考虑环境自适应机动的飞行器轨迹规划方法，其能为无人机在较大的无障碍物空间规划轨迹，并能够根据环境中障碍物的分布和系统的状态，自适应地调节飞行速度，既提高了安全性又确保了灵活的机动性。
[0005]为达到上述目的，本专利技术采用如下的技术方案：
[0006]本专利技术的一种考虑环境自适应机动的飞行器轨迹规划方法包括路径搜索方法，低阶MPC轨迹优化和高阶MPC轨迹优化。
[0007]所述的路径搜索方法利用导航定位所提供的位姿信息以及表征环境的欧式符号距离场和占据栅格地图，规划出一条离散的路径；随后，基于该路径，构建所述的低阶MPC轨迹优化模型，解优化后生成一条参考轨迹；最后，利用该参考轨迹，构建所述的高阶MPC轨迹优化模型，解优化后生成一条局部的期望轨迹并提供给底层的控制器。
[0008]所述的路径搜索方法在占据栅格地图中进行全局规划，具体包括如下步骤：
[0009]步骤A1：根据飞行器所携带传感器的测量信息获得当前飞行器的位姿信息以及周围环境的点云数据，并建立占据栅格地图和欧式符号距离场；
[0010]步骤A2：创建两个表OpenSet和CloseSet，用于存储搜索过程中的节点，将起始节点添加到OpenSet中；
[0011]步骤A3：从非空的OpenSet表中取出总体代价最小的节点n
min
，判断是否等于目标
节点，若是则获取一系列离散路径点并结束，否则获取n
min
的邻居节点，并将n
min
从OpenSet中移除，然后添加至CloseSet中；
[0012]其中总体代价f＝λ1g
e
+λ2g
v
+λ3g
f
+g
p
，其中g
e
表示当前节点与父节点之间欧式距离代价，g
v
表示当前节点周围无障碍物空间体积的代价，g
f
表示当前节点与目标点之间的启发式距离代价，g
p
表示当前节点的父节点的总体代价，λ
i
(i＝1,2,3)表示权重；
[0013]步骤A4：计算当前节点的邻居节点的g
e
和g
v
，采用欧式距离计算g
e
：其中n
n,i
(i＝x,y,z)为当前节点的邻居节点在惯性系下的三维坐标，n
c,i
(i＝x,y,z)为当前节点在惯性系下的三维坐标；g
v
的数学表达式为其中r
thr
为安全阈值，r(n
n
)为邻居节点与欧式符号距离场中最近障碍物的距离，n
n
为邻居节点在惯性系下的三维坐标；
[0014]步骤A5：计算当前节点的邻居节点的g
f
的数学表达式为：
[0015]Δ＝n
n
‑
g
[0016][0017][0018][0019]其中，g为目标节点在惯性系下的三维坐标，Δ为两个坐标的向量差，Δ
m
为三个坐标轴距离中的最短距离，Δ
n
为三个坐标轴距离中的次短距离，Δ
{x,y,z}\{m,n}
为三个坐标轴距离中的最长距离，min为最小值函数，|
·
|为绝对值函数，β为调节系数；
[0020]步骤A6：若邻居节点在CloseSet中或者通过障碍物，则丢弃之，不做处理；否则对该邻居节点继续执行以下流程：
[0021](i)如果不在OpenSet中，则将其添加至OpenSet，当前节点n
c
为其父节点；(ii)如果当前节点已经在OpenSet中，则通过f判断这条路径是否为代价更小的路径，若是，则更新邻居节点的信息；
[0022]步骤A7：重复上述步骤A3
‑
A6，直至结束。
[0023]进一步，所述的低阶MPC轨迹优化的模型建立为：
[0024][0025]s.t.x
k+1
＝f
d
(x
k
,u
k
)
[0026]x0＝x0[0027]u
k
∈Ω,x
k
∈X
[0028]其中，J
u
(
·
)表示平滑惩罚函数，J
s
(
·
)表示跟踪离散路径的惩罚函数，J
c
(
·
)表示静态避障惩罚函数，J
v
(
·
)表示路径点周围的无障碍空间体积惩罚函数，f
d
(
·
)为状态转移函数，x
k
为第k个系统状态，u
k
为第k个系统输入，x0是起始状态，N表示离散路径点的个数，λ
u
,λ
s
,λ
c
,λ
v
均为加权系数，Ω和X表示状态集合。
[0029]所述的低阶MPC轨迹优化模型中，第j段参考轨迹的平滑、跟踪和静态避障代价函数定义为：
[0030]J
u
(u
k+1
,u
k
)＝||u
k+1
‑
u
k
||2[0031][0032][0033]其中，c<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种考虑环境自适应机动的飞行器轨迹规划方法，其特征在于，包括路径搜索方法，低阶MPC轨迹优化和高阶MPC轨迹优化；所述的路径搜索方法利用导航定位所提供的位姿信息以及表征环境的欧式符号距离场和占据栅格地图，规划出一条离散的路径；随后，基于该路径，构建低阶MPC轨迹优化模型，解优化后生成一条参考轨迹；最后，利用该参考轨迹，构建高阶MPC轨迹优化模型，解优化后生成一条局部的期望轨迹并提供给底层的控制器。2.根据权利要求1所述的一种考虑环境自适应机动的飞行器轨迹规划方法，其特征在于，所述的路径搜索方法在占据栅格地图中进行全局规划，具体包括如下步骤：步骤A1：根据飞行器所携带传感器的测量信息获得当前飞行器的位姿信息以及周围环境的点云数据，并建立占据栅格地图和欧式符号距离场；步骤A2：创建两个表OpenSet和CloseSet，用于存储搜索过程中的节点，将起始节点添加到OpenSet中；步骤A3：从非空的OpenSet表中取出总体代价最小的节点n
min
，判断是否等于目标节点，若是则获取一系列离散路径点并结束，否则获取n
min
的邻居节点，并将n
min
从OpenSet中移除，然后添加至CloseSet中；其中，总体代价f＝λ1g
e
+λ2g
v
+λ3g
f
+g
p
，其中g
e
表示当前节点与父节点之间欧式距离代价，g
v
表示当前节点周围无障碍物空间体积的代价，g
f
表示当前节点与目标点之间的启发式距离代价，g
p
表示当前节点的父节点的总体代价，λ
i
(i＝1,2,3)表示权重；步骤A4：计算当前节点的邻居节点的g
e
和g
v
，采用欧式距离计算g
e
：其中n
n,i
(i＝x,y,z)为当前节点的邻居节点在惯性系下的三维坐标，n
c,i
(i＝x,y,z)为当前节点在惯性系下的三维坐标；g
v
的数学表达式为其中r
thr
为安全阈值，r(n
n
)为邻居节点与欧式符号距离场中最近障碍物的距离，n
n
为邻居节点在惯性系下的三维坐标；步骤A5：计算当前节点的邻居节点的g
f
的数学表达式为：Δ＝n
n
‑
ggg其中，g为目标节点在惯性系下的三维坐标，Δ为两个坐标的向量差，Δ
m
为三个坐标轴距离中的最短距离，Δ
n
为三个坐标轴距离中的次短距离，Δ
{x,y,z}\{m,n}
为三个坐标轴距离中的最长距离，min为最小值函数，|
·
|为绝对值函数，β为调节系数；
步骤A6：若邻居节点在CloseSet中或者通过障碍物，则丢弃之，不做处理；否则对该邻居节点继续执行一下流程：(i)如果不在OpenSet中，则将其添加至OpenSet，当前节点n
c
为其父节点；(ii)如果当前节点已经在OpenSet中，则通过f判断这条路径是否为代价更小的路径，若是，则更新邻居节点的信息；步骤A7：重复上述步骤A3
‑
A6，直至结束。3.根据权利要求1所述的一种考虑环境自适应机动的飞行器轨迹规划方法，其特征在于，所述的低阶MPC轨迹优化的模型建立为：s.t.x
k+1
＝f
d
(x
k
,u
k
)x0＝x0u
k
∈Ω,x
k
∈X其中，J
u
(
·
)表示平滑惩罚函数，J
s
(
·
)表示跟踪离散路径的惩罚函数，J
c
(
·
)表示静态避障惩罚函数，J
v
(
·
)表示路径点周围的无障碍空间体积惩...

【专利技术属性】
技术研发人员：郭克信，刘书林，余翔，郭雷，
申请(专利权)人：北京航空航天大学杭州创新研究院，
类型：发明
国别省市：