一种基于凸优化的非平坦地形下无人车轨迹规划方法技术

本发明专利技术提供一种基于凸优化的非平坦地形下无人车轨迹规划方法，在给定无人车初始时刻与终点时刻位置以及栅格化之后非平坦地形地图的基础上，轨迹规划方法包括以下步骤：首先，建立无人车的能耗模型，以确定无人车前往相邻节点所产生的能耗；其次，设计启发式函数并基于A*搜索算法生成能耗最优初始路径；然后，经过修剪初始路径的路标点生成运动走廊，用以约束最终轨迹位置及运动学特性；最后，设计目标函数，施加多种约束，得到优化后的初始轨迹，进一步通过对生成分段贝塞尔轨迹所用时间进行重分配得到优化后的最终轨迹。本发明专利技术能够保证无人车在三维非平坦地形可行路径存在的前提下生成一条符合运动学特性的时间

【技术实现步骤摘要】
一种基于凸优化的非平坦地形下无人车轨迹规划方法


[0001]本专利技术属于无人车轨迹规划
，具体涉及一种基于凸优化的非平坦地形下无人车轨迹规划方法。

技术介绍

[0002]近年来，随着科技的发展，无人自主机器人在许多领域均发挥着重要的作用，例如地下矿井，精准农业，灾后救援等，无人车则是其中的主要对象。在针对无人车不同方向的诸多研究中，路径规划是必不可少且至关重要的一环。虽然无人车路径规划的研究层出不穷，但非平坦地图下的规划工作则十分稀少。现阶段针对非平坦地图的路径规划研究主要有两种方法，分别是基于图搜索的算法和基于采样的算法。其中前者将空间离散之后为无人车规划折线路径，而后者并未考虑路径规划的最优性。在此基础上，本专利技术充分考虑到实际地表崎岖、不平坦的特性，开展了基于凸优化的非平坦地形下无人车轨迹规划研究，旨在为无人车规划一条既满足运动学特性的要求，又节约能耗和时间的平滑轨迹。

技术实现思路

[0003]针对无人车存在执行机构饱和约束等问题，本专利技术提供一种基于凸优化的非平坦地形下无人车轨迹规划方法，能够在节约能耗和时间的同时，为无人车规划一条既满足运动学特性要求的平滑轨迹。本专利技术主要应用于给定无人车初始时刻与终点时刻位置以及栅格化之后非平坦地形地图基础上的时间
‑
能耗最优轨迹规划。
[0004]为达到上述目的，本专利技术采用的技术方案为：
[0005]一种基于凸优化的非平坦地形下无人车轨迹规划方法，包括以下步骤：
[0006](1)通过栅格化给定非平坦地图模型，建立无人车在任意方向相邻节点之间运动的能耗模型；
[0007](2)在步骤(1)的基础上设计启发式函数，并通过A*搜索算法生成一条能耗最优的初始路径；
[0008](3)通过修剪步骤(2)中初始路径的路标点以生成运动走廊；
[0009](4)在步骤(3)的基础上设计目标函数，并通过施加不同约束将其转换到二次约束二次规划的框架下求解得到优化后初始轨迹，并进一步调整时间参数以生成最终轨迹。
[0010]进一步地，所述步骤(1)中，
[0011]在从当前点n
c
处运动到邻居节点n
n
处的过程中，消耗的能量E
t
(n
c
,n
n
)为：
[0012][0013]其中，m表示无人车的质量，g为重力加速度，μ为动摩擦系数，φ为邻域节点之间的倾角，φ
max
为最大爬坡倾角，φ
b
为临界倾角，φ
min
为最小临界倾角，由斜坡的静摩擦系数μ
s
确定：φ
min
＝
‑
arctan(μ
s
)；s(n
i
,n
j
)表示节点n
i
和n
j
之间的欧氏距离；
[0014]其中，无人车的在饱和约束及安全约束共同作用下的最大爬坡倾角为φ
max
＝max(φ
f
,φ
s
)，其中，在静摩擦力的作用下保持无人车静止的最大爬坡倾角φ
s
＝arctan(μ
s
‑
μ)，其中μ
s
为斜坡的静摩擦系数，无人车在存在牵引力的前提下最大爬坡倾角φ
f
为：
[0015][0016]其中，F
max
为最大牵引力；
[0017]无人车在n
c
处原地转向的能耗模型为：
[0018]E
r
(n
c
,ψ)＝μmg
·
(π
‑
ψ)
·
r
[0019]其中，ψ∈[0,π]是无人车从n
i
到n
j
经过n
c
时的转向角，n
c
是n
i
的邻居，n
j
是n
c
的邻居，r＞0，是无人车的原地转向半径；
[0020]从节点n
r
到当前点n
c
真实消耗的能量为：
[0021][0022]进一步地，所述步骤(2)中的启发式函数h(n
c
)为：
[0023][0024]其中，Δ(n
i
,n
j
)表示节点n
i
和n
j
之间的垂直距离。
[0025]通过所述A*搜索算法生成一条初始路径。
[0026]进一步地，所述步骤(3)包括在x
‑
y平面内为修剪过路标点的路径生成凸的运动走廊；运动走廊的长度由子路径的长度决定，宽度通过人为设定。
[0027]进一步地，所述步骤(4)中，所述目标函数为轨迹f(t)三阶导数的平方，即：
[0028][0029]其中，是包含2个方向(η∈{x,y})、m个分段(i＝1,2,
…
,m)的n阶多项式(j＝0,1,
…
,n)轨迹所有控制点的向量，Q0是目标函数的海森矩阵；
[0030]其中，对于任一方向轨迹f
η
(t)，有：
[0031][0032]其中，η∈{x,y}表示方向，T1,T2,
…
,T
m
为每段轨迹的终端时刻，无人车走完整条轨迹一共耗时T＝T
m
‑
T0，δt
i
＝T
i
‑
T
i
‑1则是一个比例系数，表示每段轨迹所用时长，用于将真实的不同时刻统一至区间[0,1]在等式约束中，限制轨迹在初末时刻状态，为：
[0033]f
η
(T0)＝p
η0
,f
η
(T
M
)＝p
ηe
[0034]f
′
η
(T0)＝v
η0
,f
′
η
(T
M
)＝v
ηe
[0035]f
″
η
(T0)＝a
η0
,f
″
η
(T
M
)＝a
ηe
[0036]其中，p,v,a分别表示无人车的位置、速度、加速度，{
·
}0和{
·
}
e
表示初始时刻和终止时刻的状态，η∈{x,y}表示方向；
[0037]使得最终轨迹的平滑需要使相邻曲线之间的各阶导数连续，即：
[0038][0039][0040][0041]其中，和分别为第i段贝塞尔轨迹的初始时刻和末端时刻，i＝1,2,
…
,m
‑
1；<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于凸优化的非平坦地形下无人车轨迹规划方法，其特征在于，包括以下步骤：(1)通过栅格化给定非平坦地图模型，建立无人车在任意方向相邻节点之间运动的能耗模型；(2)在步骤(1)的基础上设计启发式函数，并通过A*图搜索算法生成一条能耗最优的初始路径；(3)通过修剪步骤(2)中初始路径的路标点以生成运动走廊；(4)在步骤(3)的基础上设计目标函数，并通过施加不同约束将其转换到二次约束二次规划的框架下求解得到优化后初始轨迹，并进一步调整时间参数以生成最终轨迹。2.根据权利要求1所述的一种基于凸优化的非平坦地形下无人车轨迹规划方法，其特征在于：所述步骤(1)中，在从当前点n
c
处运动到邻居节点n
n
处的过程中，消耗的能量E
t
(n
c
,n
n
)为：其中，m表示无人车的质量，g为重力加速度，μ为动摩擦系数，φ为邻域节点之间的倾角，φ
max
为最大爬坡倾角，φ
b
为临界倾角，φ
min
为最小临界倾角，由斜坡的静摩擦系数μ
s
确定：φ
min
＝
‑
arctan(μ
s
)，s(n
i
,n
j
)表示节点n
i
和n
j
之间的欧氏距离；其中，无人车的在饱和约束及安全约束共同作用下的最大爬坡倾角为φ
max
＝max(φ
f
,φ
s
)，其中，在静摩擦力的作用下保持无人车静止的最大爬坡倾角φ
s
＝arctan(μ
s
‑
μ)，其中μ
s
为斜坡的静摩擦系数，无人车在存在牵引力的前提下最大爬坡倾角φ
f
为：其中，F
max
为最大牵引力；无人车在n
c
处原地转向的能耗模型为：E
r
(n
c
,ψ)＝μmg
·
(π
‑
ψ)
·
r其中，ψ∈[0,π]是无人车从n
i
到n
j
经过n
c
时的转向角，n
c
是n
i
的邻居，n
j
是n
c
的邻居，r＞0，是无人车的原地转向半径；从节点n
r
到当前点n
c
真实消耗的能量为：3.根据权利要求2所述的一种基于凸优化的非平坦地形下无人车轨迹规划方法，其特征在于：所述步骤(2)中的启发式函数h(n
c
)为：
其中，Δ(n
i
,n
j
)表示节点n
i
和n
j
之间的垂直距离；通过所述A*搜索算法生成一条初始路径。4.根据权利要3所述的一种基于凸优化的非平坦地形下无人车轨迹规划方法，其特征在于：所述步骤(3)包括在x
‑
y平面内为修剪过路标点的路径生成凸的运动走廊；运动走廊的长度由子路径的长度决定，宽度通过人为设定。5.根据权利要求4所述的一种基于凸...

【专利技术属性】
技术研发人员：郑建英，陈绪宁，胡庆雷，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：