一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法与流程

1.本发明涉及自主导航技术领域，具体涉及一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法及装置。


背景技术：

2.许多具有挑战性的应用场景(如搜救、探索、检查或运输)中，自主导航对于无人车的安全高效运行至关重要，它让无人车得以执行或协助对人类来说是重复的、疲惫的甚至是危险的任务。除了定位和建图，轨迹生成是任何自主导航系统的关键功能。
3.无人车轨迹生成技术，指无人车根据自身传感器对环境感知，从当前点到目标点，自动规划一段安全的运动轨迹。针对阿克曼底盘的轨迹生成主要解决三大问题：1)连通性，即轨迹必须能连接起点和终点；2)可执行性，即轨迹满足阿克曼底盘自身的约束，如最小转弯半径(最大曲率)等；3)最优性，即在完成以上任务的前提下，轨迹的长度、对机器人的能耗应被尽量优化。
4.在结构路面下，现有算法往往会将三维的环境降维成二维，方便计算；但在非结构路面，如野外环境(崎岖的草坪、充满土包的山路)，现有技术往往只将环境区分为障碍物和可通行区域，例如使用占据栅格法，这样做没有考虑地形对无人车的影响，因此计算出的轨迹既不能保证可执行性，也不能保证在一定标准下(如轨迹的长度、对机器人的能耗)的最优性。
5.有一些新型技术，允许将环境的点云三维重建，例如用三角形网格表示环境信息。随后，无人车只需在重建后的地图中执行路径搜索，并考虑自身种种约束，便可生成具有可执行性和最优性的轨迹。但这类做法由于需要对环境的点云三维重建，会产生大量的时间消耗，缺乏实时性，同时，三维重建的过程虽然较其他简单的建模方法，较好得保留了环境特征，但仍然会造成部分信息的丢失。


技术实现要素：

6.针对现有技术不足，本发明公开了一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法。该方法利用多线激光雷达采集的点云数据建成的点云地图，根据无人车当前位姿和目标位姿，实现轨迹生成。
7.本发明解决其技术问题所采用的技术方案是：一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，该方法包括以下步骤：
8.步骤1，将输入的无人车的初始位姿和目标位姿利用点云地图进行位姿投影和地形评估；
9.步骤2，分别以无人车初始位姿和目标位姿投影获得的位姿为根节点，利用事先计算好的轨迹库执行改进的双向快速拓展随机树算法(rrtconnect算法)，生成初始的轨迹；
10.步骤3，将步骤2中生成的轨迹基于改进的快速拓展随机树法(rrtstar算法)进行全局轨迹优化；
11.步骤4，将步骤3中生成的轨迹基于有权图的单源最短路径算法(dijkstra算法)进行局部的轨迹优化，得到最终轨迹。
12.进一步地，所述步骤1中，设无人车位姿由关于地图坐标系的一个大小为3
×
3的旋转矩阵r＝[x
r
,y
r
,z
r
](其中x
r
,y
r
,z
r
均是大小为3
×
1的向量)和一个大小为3
×
1的平移向量t表示，那么利用点云地图进行位姿投影的具体步骤如下：
[0013]
步骤111，在点云中找到与t0最邻近的k个点p
ii
，ii＝1,2,...,k，k值根据点云的密度确定，均为3
×
1的向量，求出它们的重心和相关的协方差矩阵cov(t)，二者计算公式如下：
[0014][0015][0016]
步骤112，求出cov(t)最小特征值对应的特征向量，记为v0，则可计算对应投影在地表上的位姿的z方向(与底盘垂直、并从车底指向车顶的方向)的向量n，其计算公式如下：
[0017]
n＝sign(z
r
·
v0)v0[0018]
其中，sign(
·
)表示符号函数。
[0019]
步骤113，再利用y
r
，则可计算对应投影在地表上的位姿的x方向(车辆向前行驶的方向)的向量x
r
'，进而可以求得投影后的旋转矩阵r'，它们的计算公式如下：
[0020][0021]
r'＝[x
r
',n
×
x
r
',n]
[0022]
步骤114，进而，投影在地表上的平移向量t'可由如下公式计算：
[0023][0024]
基于上述四步，利用点云地图，将无人车初始的位姿投影到了地表上，投影后的位姿可由r'和t'表示。
[0025]
进一步地，所述步骤1中，设地形评估的结果可由“可穿越性”τ∈[0,1]表示，τ＝0表示改位姿不可达，τ＞0则表示位姿可达，且值越大，表示该位姿的“可穿越性”越良好，那么利用点云地图进行地形评估的步骤如下：
[0026]
步骤121，根据位姿投影获得的旋转矩阵r'，计算无人车的滚转角φ和俯仰角θ，设a
11
～a
33
为矩阵中的元素，则公式如下：
[0027][0028]
步骤122，利用点云信息，对以无人车为中心、以r
rob
为半径(该值可由对无人车的实际测量获得)的球形区域内所有点云上的点(设有m个点)，分别求粗糙度，记作ρ
i
(i＝1,
2,...,m)，并设无人车可接受的最大粗糙度为ρ
max
，则此位姿下的粗糙度ρ
rob
可由如下公式计算：
[0029][0030]
步骤123，设无人车可接收的最大滚转角为φ
max
，最大俯仰角为θ
max
，最小俯仰角为θ
min
，若对应的ρ
rob
,θ,φ其中有一者超出限度，则强制“可穿越性”τ赋值为零，表示该位姿不可行，否则τ由以下公式计算获得：
[0031][0032]
其中，w
ρ
w
φ
w
θ
＝1且w
ρ
,w
φ
,w
θ
＞0为各项权重。
[0033]
进一步地，所述步骤122中，计算点云上单个点p
i
的粗糙度ρ
i
的具体步骤如下：
[0034]
步骤1221，在点云中找到与p
i
距离小于r
p
(由人为指定)的点p
j
，设有m个，即j＝1,2,...,m，均为3
×
1的向量，求出它们的重心和相关的协方差矩阵cov(p
i
)，二者计算公式如下：
[0035][0036][0037]
步骤1222，求出cov(p
i
)最小特征值对应的特征向量，记为n
i
，则n
i
的物理意义为p
j
所在平面的法线。随后对上述m个点中与p
i
距离小于的点，分别由如下公式计算它们到该平面的距离d
ij
：
[0038][0039]
步骤1223，为去除离群点，对所求出的d
ij
排序，设有n个(n≤m)，取大小排行正好15％和75％的点，分别记作d
ijmin
和d
ijmax
，则p
i
的粗糙度ρ
i
由如下公式给出：
[0040]
ρ
i
＝|d
ijmax
‑
d
ijmin
|。
[0041]
进一步地，所述步骤2中，设无人车机体坐标系原点o为后轮车轴的中心，向前行驶方向为x轴正方向，与底盘垂直、并从车底指向车顶的方向为z轴正方向，依照右手系确定y轴，轨迹库中的每一条轨迹由相对无人车机体坐标系的变化(δx,δy,δψ,δκ)组成，δx和δy分别为相对于无人车x轴和y轴变化的距离，δψ无人车变化的偏航角，δκ为曲率变化，设轨迹库中轨迹的初始曲率都为零，那么生成轨迹库的具体步骤如下：
[0042]
步骤211，取半径r
t
、偏航角变化值δψ、正整数k，轨迹库中的δψ取0,
±
δψ,
±
2δψ,...,
±
kδψ，生成总共2k 1条轨迹，同时，为保证轨迹库中的轨迹都可自由相互连接，δκ取零；
[0043]
步骤212，对每一个δψ，取以o为圆心、r
t
为半径且在x轴正半平面上的圆弧，对圆
弧上的每个点(x
i3
,y
i3
)，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x
i3
,y
i3
,δψ,0)的轨迹，其中i3表示将圆弧离散化为点后，每个点的下标，i为自然数；
[0044]
步骤213，分别计算由圆弧上的每个点(x
i3
,y
i3
)生成轨迹的最大曲率值κ
m
，选择κ
m
最小的那条作为对应δψ的轨迹，即：若使κ
m
最小的轨迹对应圆弧上点为(x
m
,y
m
)，则对应此δψ的轨迹库中的轨迹为(δx
m
,δy
m
,δψ,0)。
[0045]
进一步地，所述所述步骤212中，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x
i
,y
i
,δψ,0)轨迹的具体步骤如下：
[0046]
步骤2121，设阿克曼底盘运动学方程如下，系统输入u为曲率的导数，速度恒为1：
[0047][0048]
其中，κ为曲率值，ψ为无人车偏航角；
[0049]
那么，在输入为零，且状态为零处将系统线性化，得到运动学方程变为：
[0050][0051]
步骤2122，为保证轨迹的光滑性，设输入为关于时间t的二次多项式，即u＝at2 bt c，其中a,b,c为多项式系数，并设总运动时间为t，则由积分可得：
[0052][0053]
步骤2123，根据末状态(x
i3
,y
i3
,δψ,0)，列出一个四元一次线性方程组：
[0054][0055]
求解可得未知数(a,b,c,t)，即得从状态(0,0,0,0)到(x
i
,y
i
,δψ,0)轨迹。
[0056]
进一步地，所述步骤2中改进的双向快速拓展随机树算法的步骤具体如下：
[0057]
步骤221，建立两棵搜索树，分别以初始位姿和目标位姿投影获得的位姿为根节
点，分别记作st和gt；
[0058]
步骤222，任选轨迹库中的一条轨迹trajseg和st中的一个节点node，在node的坐标系下，运用轨迹trajseg，获得新的位姿tseg；
[0059]
步骤223，将获得的新位姿tseg利用点云进行位姿投影和地形评估，获得tseg'和对应的τ
tseg'
，τ
tseg'
为对应的地形评估结果，若τ
tseg'
＞0，将其加入st，记作node
new
，父节点为node；否则，转步骤222；
[0060]
步骤224，计算gt中每个节点与node
new
的欧式距离，若存在距离小于指定距离r
c
的节点，则连接st与gt，若连接成功，算法结束，轨迹生成；否则转步骤225；
[0061]
步骤225，求gt中与node
new
欧式距离最近的节点node
nearest
，并在node
nearest
的坐标系下，运用所有轨迹库中的轨迹，获得新的位姿；
[0062]
步骤226，利用点云，将新的位姿进行位姿投影和地形评估，获得这些新位姿投影后的位姿和对应的τ，τ为地形评估结果的“可穿越性”，取其中与node
new
欧式距离最近且τ＞0的位姿，记作t
best
，将其加入gt，父节点为node
nearest
，转步骤227；若不存在这样的位姿，转步骤227；
[0063]
步骤227，交换st和gt，转步骤222。
[0064]
进一步地，所述步骤224中连接st和gt的具体步骤如下：
[0065]
步骤2241，计算gt中每个节点与node
new
的欧式距离，若存在距离小于指定距离r
c
的节点，记作node
try
；
[0066]
步骤2242，设node
try
的旋转矩阵为r
try
，平移向量为t
try
；node
new
的旋转矩阵为r
new
，平移向量为t
new
；按如下公式求出node
try
在node
new
坐标系下的(δx,δy,δψ,δκ)，δx和δy分别为相对于无人车x轴和y轴变化的距离，δψ无人车变化的偏航角，δκ为曲率变化：
[0067][0068]
δx＝t
x
,δy＝t
y
,
[0069]
其中，a
11
～a
33
,t
x
,t
y
,t
z
均表示矩阵中的元素；
[0070]
步骤2243，根据(δx,δy,δψ,δκ)和阿克曼运动学方程局部线性化后的解析解法，获得轨迹的方程，若该轨迹的最大曲率小于无人车可接受的最大曲率κ
max
，则对该轨迹每隔指定时间δt采样一个位姿，利用位姿投影和地形评估，获得新的位姿，若所有地形评估所得的τ均大于零，连接成功，将所有位姿加入st；一旦轨迹的最大曲率大于κ
max
或地形评估中存在τ＝0，则连接失败。
[0071]
进一步地，所述步骤3中基于改进的快速拓展随机树法进行全局轨迹优化的步骤具体如下：
[0072]
步骤31，在生成的轨迹中任取一个路径节点，在以该路径点为球心、r
near
为半径的球形区域内任取点云中的一个点p；
[0073]
步骤32，将节点沿该节点与p连线的方向平移至p，获得位姿p，对p进行位姿投影和地形评估，若所得地形评估结果τ
p
＝0，转步骤31，否则转步骤33；
[0074]
步骤33，找到以p位置为球心、指定半径为r
s
的球形区域内st或gt中的节点，用步骤224中连接st和gt的方法从这些节点连接p；
[0075]
步骤34，若存在一个节点q可以连接p，且以q作为父节点，生成从根节点到p的轨迹消耗时间最短，则将p加入st或gt，父节点为q；否则，转步骤31；
[0076]
步骤35，对以p位置为球心、r
s
为半径的球形区域内st或gt中的节点，再反过来用步骤224中连接st和gt的方法从p连接这些节点；
[0077]
步骤36，对其中的一个节点node
i
，若连接成功，且以p作为父节点可以减少从根节点到node
i
的时间消耗，则改变node
i
的父节点为p；
[0078]
步骤37，累计总的被改变父节点的节点个数n
rewire
，若n
rewire
大于指定阈值n
max
，则全局优化完成；否则，转步骤31。
[0079]
进一步地，所述步骤4中基于有权图的单源最短路径算法进行局部轨迹优化的步骤具体如下：
[0080]
步骤41，对经过全局优化后的轨迹，除了起点和终点外的所有节点，分别沿各自的y轴平移指定距离
±
δd，获得两个新的位姿，再将两个新的位姿进行投影和地形评估，获得两个新节点，称为主节点的左子节点和右子节点，如此，对一条具有k
w
个节点的轨迹，获得3k
w
‑
4个节点；
[0081]
步骤42，为了确定新节点的方向和曲率，对不临近起点和终点的节点，以左子节点为例，画三个圆，即经过上一节点的左子节点、该左子节点、下一节点的左子节点的圆，经过上一节点的主节点、该左子节点、下一节点的主节点的圆，经过上一节点的右子节点、该左子节点、下一节点的右子节点的圆；那么该节点的方向就是三个圆在该左子节点的切线方向的平均值，即沿该左子节点自身的z轴，将该左子节点的x轴方向旋转至三个圆在该左子节点的切线方向的平均值；该左子节点的曲率即为三圆曲率的平均值；对于临近起点或终点节点的左子节点，方向和曲率都由经过上一节点的主节点、该左子节点、下一节点的主节点的圆确定；
[0082]
步骤43，由于除起点终点外的节点各自都分为了三个节点，对其中一个主节点，利用阿克曼运动学方程局部线性化后的解析解法，便可生成其与下一个主节点的九条轨迹，对连接两点node1和node2的轨迹赋予如下公式所示的代价ct，便可依据有权图的单源最短路径算法进行图搜索，获得最优路径；其中node1和node2即经过步骤41得到的3k
w
‑
4个节点中的任意两个节点；
[0083][0084]
其中，w
len
w
cur
w
tra
＝1且w
len
,w
cur
,w
tra
＞0，w
len
,w
cur
,w
tra
分别为时间消耗、最大曲率大小、可通行性好坏的权重，t,κ
m
,τ分别表示该轨迹的时间消耗、最大曲率值和node2处地形评估的结果，t
min
,t
max
,κ
max
分别表示图中所有轨迹段的最短最长时间消耗、无人车可接受的最大曲率阈值，若最大曲率值κ
m
＞κ
max
或node2处地形评估的结果τ＝0，则强制令ct＝∞，表示该轨迹段是不可被执行的；
[0085]
步骤44，若图搜索后所得最优路径中的节点仍然全是主节点，则局部优化结束，返回轨迹；否则，对那些从主节点“被优化”到左子节点或者右子节点的节点，下次迭代时平移的δd变为原来的γ倍，0＜γ＜1，直到所有节点的δd小于设定阈值δd
min
；若所有节点的δd小
于设定阈值δd
min
，则局部优化结束，返回轨迹，否则以该次优化后的轨迹为初始轨迹，转步骤41。
[0086]
本发明的有益效果如下：
[0087]
(1)本发明生成轨迹的节点间距小于车辆长度，并利用点云进行位姿投影，因此所得的轨迹能很好地依附于地形表面。
[0088]
(2)本发明生成轨迹节点间的连接采用局部线性化的阿克曼运动学模型，曲率采用三次多项式计算，保证轨迹充分光滑；轨迹生成的过程中考虑了阿克曼底盘车辆的姿态约束、最小转弯半径约束，具有良好的可执行性。
[0089]
(3)本发明直接采用点云信息进行轨迹生成，最大程度地保留了环境信息，并避免了繁琐的计算，保证了算法的实时性。
[0090]
(4)本发明生成的轨迹，在获得初始轨迹后，对轨迹进行了全局优化和局部优化，保证了所得轨迹的最优性。
[0091]
(5)轨迹生成可使用局部点云地图，可以在全局环境未知的情况下使用。
附图说明
[0092]
图1为本发明整体原理步骤图；
[0093]
图2为地形评估算法流程图；
[0094]
图3为改进的双向快速拓展随机树算法流程图；
[0095]
图4为基于改进的快速拓展随机树法的全局轨迹优化算法流程图；
[0096]
图5为基于有权图的单源最短路径算法的局部轨迹优化算法流程图；
[0097]
图6为改进的双向快速拓展随机树算法生成轨迹的效果示意图；
[0098]
图7为基于改进的快速拓展随机树法的全局轨迹优化算法的效果示意图；
[0099]
图8为基于有权图的单源最短路径算法的局部轨迹优化算法的效果示意图。
具体实施方式
[0100]
本方法针对非结构路面建模困难、计算量大导致算法实时性不强的问题，以及占据栅格法等粗略建图导致生成轨迹缺乏可执行性和最优性的问题，直接利用点云地图提供的信息，在执行改进的双向快速拓展随机树法(rrtconnect算法)的同时进行位姿投影与地形评估，并基于改进的快速拓展随机树法(rrtstar算法)进行全局轨迹优化、基于有权图的单源最短路径算法(dijkstra算法)进行局部的轨迹优化，完成具有阿克曼底盘无人车的轨迹生成。下文中，结合附图和实施例对本发明作进一步阐述。
[0101]
图1为本发明整体原理步骤图，本发明提出了一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，包括以下步骤：
[0102]
步骤1，将输入的无人车的初始位姿和目标位姿利用点云地图进行位姿投影和地形评估；
[0103]
步骤2，分别以无人车初始位姿和目标位姿投影获得的位姿为根节点，利用事先计算好的轨迹库执行改进的双向快速拓展随机树算法(rrtconnect算法)，生成初始的轨迹；
[0104]
步骤3，将步骤2中生成的轨迹基于改进的快速拓展随机树法(rrtstar算法)进行全局轨迹优化；
[0105]
步骤4，将步骤3中生成的轨迹基于有权图的单源最短路径算法(dijkstra算法)进行局部的轨迹优化，得到最终轨迹。
[0106]
进一步地，所述步骤1中，设无人车位姿由关于地图坐标系的一个大小为3
×
3的旋转矩阵r＝[x
r
,y
r
,z
r
]和一个大小为3
×
1的平移向量t表示，那么利用点云地图进行位姿投影的具体步骤如下：
[0107]
步骤111，在点云中找到与t最邻近的k个点p
i
(i＝1,2,...,k)(本实例中，取k＝25)，均为3
×
1的向量，求出它们的重心和相关的协方差矩阵cov(t)，二者计算公式如下：
[0108][0109][0110]
步骤112，求出cov(t)最小特征值对应的特征向量，记为v0，则可计算对应投影在地表上的位姿的z方向(与底盘垂直、并从车底指向车顶的方向)的向量n，其计算公式如下：
[0111]
n＝sign(z
r
·
v0)v0[0112]
其中，sign(
·
)表示符号函数。
[0113]
步骤113，再利用y
r
，则可计算对应投影在地表上的位姿的x方向(车辆向前行驶的方向)的向量x
r
'，进而可以求得投影后的旋转矩阵r'，它们的计算公式如下：
[0114][0115]
r'＝[x
r
',n
×
x
r
',n]
[0116]
步骤114，进而，投影在地表上的平移向量t'可由如下公式计算：
[0117][0118]
基于上述四步，利用点云地图，将无人车初始的位姿投影到了地表上，投影后的位姿可由r'和t'表示。
[0119]
图2为地形评估算法流程图。设地形评估的结果可由“可穿越性”τ∈[0,1]表示，τ＝0表示改位姿不可达，τ＞0则表示位姿可达，且值越大，表示该位姿的“可穿越性”越良好，那么利用点云地图进行地形评估的步骤如下：
[0120]
步骤121，根据位姿投影获得的旋转矩阵r'，计算无人车的滚转角φ和俯仰角θ，设则公式如下：
[0121][0122]
步骤122，利用点云信息，对以无人车为中心、以r
rob
为半径(该值可由对无人车的实际测量获得)的球形区域内所有点云上的点(设有m个点)，分别求粗糙度，记作ρ
i
(i＝1,2,...,m)，并设无人车可接受的最大粗糙度为ρ
max
，本实例中，取ρ
max
＝0.08，则此位姿下的
粗糙度ρ
rob
可由如下公式计算：
[0123][0124]
步骤123，设无人车可接收的最大滚转角为最大俯仰角为θ
max
，最小俯仰角为若对应的ρ
rob
,θ,φ其中有一者超出限度，则强制“可穿越性”τ赋值为零，表示该位姿不可行，否则τ由以下公式计算获得：
[0125][0126]
其中，w
ρ
w
φ
w
θ
＝1且w
ρ
,w
φ
,w
θ
＞0为各项权重，优选的，本实例中取w
ρ
＝0.8,w
φ
＝0.1,w
θ
＝0.1。
[0127]
进一步地，所述步骤122中，计算点云上单个点p
i
的粗糙度ρ
i
的具体步骤如下：
[0128]
步骤1221，在点云中找到与p
i
距离小于r
p
(本实例中取r
p
＝0.3m)的点p
j
，设有m个，即j＝1,2,...,m，均为3
×
1的向量，求出它们的重心和相关的协方差矩阵cov(p
i
)，二者计算公式如下：
[0129][0130][0131]
步骤1222，求出cov(p
i
)最小特征值对应的特征向量，记为n
i
，则n
i
的物理意义为p
j
所在平面的法线。随后对上述m个点中与p
i
距离小于的点，分别由如下公式计算它们到该平面的距离d
ij
：
[0132][0133]
步骤1223，为去除离群点，对所求出的d
ij
排序，设有n个(n≤m)，取大小排行正好15％和75％的点，分别记作d
ijmin
和d
ijmax
，则p
i
的粗糙度ρ
i
由如下公式给出：
[0134]
ρ
i
＝|d
ijmax
‑
d
ijmin
|。
[0135]
进一步地，所述步骤2中，设无人车机体坐标系原点o为后轮车轴的中心，向前行驶方向为x轴正方向，与底盘垂直、并从车底指向车顶的方向为z轴正方向，依照右手系确定y轴，轨迹库中的每一条轨迹由相对无人车机体坐标系的变化(δx,δy,δψ,δκ)组成(δκ为曲率变化，设轨迹库中轨迹的初始曲率都为零)，那么生成轨迹库的步骤如下：
[0136]
步骤211，取合适的半径r
t
(本实例中取r
t
＝0.4)、合适的偏航角变化值δψ(优选为0.1rad)、合适的正整数k(本实例中取δψ＝0.1rad,k＝5)，轨迹库中的δψ便可取0,
±
δψ,
±
2δψ,...,
±
kδψ，将生成总共2k 1条轨迹，同时，为保证轨迹库中的轨迹都可自由相互连接，δκ取零；
[0137]
步骤212，对每一个δψ，取以o为圆心、r
t
为半径且在x轴正半平面上的圆弧，对圆弧上的每个点(x
i
,y
i
)，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x
i
,y
i
,δψ,0)的轨迹；
[0138]
步骤213，分别计算由圆弧上的每个点(x
i
,y
i
)生成轨迹的最大曲率值κ
m
，选择κ
m
最小的那条作为对应δψ的轨迹，即：若使κ
m
最小的轨迹对应圆弧上点为(x
m
,y
m
)，则对应此δψ的轨迹库中的轨迹为(δx
m
,δy
m
,δψ,0)。
[0139]
进一步地，所述所述步骤212中，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x
i
,y
i
,δψ,0)轨迹的具体步骤如下：
[0140]
步骤2121，设阿克曼底盘运动学方程如下，系统输入u为曲率的导数，速度恒为1：
[0141][0142]
那么，在输入为零，且状态为零处将系统线性化，得到运动学方程变为：
[0143][0144]
步骤2122，为保证轨迹的光滑性，设输入为关于时间t的二次多项式，即u＝at2 bt c，并设总运动时间为t，则由积分可得：
[0145][0146]
步骤2123，根据末状态(x
i
,y
i
,δψ,0)，可列出一个四元一次线性方程组：
[0147][0148]
求解可得未知数(a,b,c,t)，即得从状态(0,0,0,0)到(x
i
,y
i
,δψ,0)轨迹。
[0149]
图3为改进的双向快速拓展随机树算法(rrtconnect算法)流程图，步骤如下：
[0150]
步骤221，建立两棵搜索树，分别以初始位姿和目标位姿投影获得的位姿为根节点，分别记作st和gt；
[0151]
步骤222，任选轨迹库中的一条轨迹trajseg和st中的一个节点node，在node的坐标系下，运用轨迹trajseg，获得新的位姿tseg；
[0152]
步骤223，将获得的新位姿tseg利用点云进行位姿投影和地形评估，获得tseg'和对应的τ
tseg
'，若τ
tseg
'＞0，将其加入st，记作node
new
，父节点为node；否则，转步骤222；
[0153]
步骤224，计算gt中每个节点与node
new
的欧式距离，若存在距离小于r
c
(本实例中取r
c
＝1.2m)的节点，则尝试连接st与gt，若连接成功，算法结束，轨迹生成；否则转步骤225；
[0154]
步骤225，求gt中与node
new
欧式距离最近的节点node
nearest
，并在node
nearest
的坐标系下，运用所有轨迹库中的轨迹，获得若干新的位姿；
[0155]
步骤226，利用点云，将获得的若干新位姿进行位姿投影和地形评估，获得这些新位姿投影后的位姿和对应的τ，取其中与node
new
欧式距离最近且τ＞0的位姿，记作t
best
，将其加入gt，父节点为node
nearest
，转步骤227；若不存在这样的位姿，转步骤227；
[0156]
步骤227，交换st和gt，转步骤222。
[0157]
进一步地，所述步骤224中，尝试连接st和gt的具体步骤如下：
[0158]
步骤2241，计算gt中每个节点与node
new
的欧式距离，若存在距离小于r
c
的节点，记作node
try
；
[0159]
步骤2242，设node
try
的旋转矩阵为r
try
，平移向量为t
try
；node
new
的旋转矩阵为r
new
，平移向量为t
new
；按如下公式求出node
try
在node
new
坐标系下的(δx,δy,δψ,δκ)：
[0160][0161][0162]
步骤2243，根据(δx,δy,δψ,δκ)和阿克曼运动学方程局部线性化后的解析解法，获得轨迹的方程，若该轨迹的最大曲率小于无人车可接受的最大曲率κ
max
，则对该轨迹每隔δt(本实例中取δt＝0.4s)时间采样一个位姿，利用位姿投影和地形评估，获得新的位姿，若所有地形评估所得的τ均大于零，连接成功，将所有位姿加入st；一旦轨迹的最大曲率大于κ
max
或地形评估中存在τ＝0，则连接失败。
[0163]
图4为基于改进的快速拓展随机树法(rrtstar算法)的全局轨迹优化算法流程图。其具体步骤如下：
[0164]
步骤31，在生成的轨迹中任取一个路径节点，在以该路径点为球心、r
near
为半径(优选的，取三倍的r
t
)的球形区域内任取点云中的一个点p；
[0165]
步骤32，将节点沿该节点与p连线的方向平移至p，获得位姿p，对p进行位姿投影和地形评估，若所得地形评估结果τ
p
＝0，转步骤31，否则转步骤33；
[0166]
步骤33，找到以p位置为球心、r
s
为半径(本实例中取r
s
＝6r
t
)的球形区域内st或gt中的节点，尝试用步骤224中连接st和gt的方法从这些节点连接p；
[0167]
步骤34，若存在一个节点q可以连接p，且以q作为父节点，生成从根节点到p的轨迹消耗时间最短，则将p加入st或gt，父节点为q；否则，转步骤31；
[0168]
步骤35，对以p位置为球心、r
s
为半径的球形区域内st或gt中的节点，再反过来尝试用步骤(2)中连接st和gt的方法从p连接这些节点；
[0169]
步骤36，对其中的一个节点node
i
，若连接成功，且以p作为父节点可以减少从根节点到node
i
的时间消耗，则改变node
i
的父节点为p；
[0170]
步骤37，累计总的被改变父节点的节点个数n
rewire
，若n
rewire
大于阈值n
max
(人为指定)，则全局优化完成；否则，转步骤31。
[0171]
图5为基于有权图的单源最短路径算法(dijkstra算法)的局部轨迹优化算法流程图。其具体步骤如下
[0172]
步骤41，对经过全局优化后的轨迹，除了起点和终点外的所有节点，分别沿各自的y轴平移
±
δd(本实例中，其初始值取0.06m)，获得两个新的位姿，再将两个新的位姿进行投影和地形评估，获得两个新节点，称为主节点的左子节点和右子节点，如此，对一条具有k个节点的轨迹，可获得3k
‑
4个节点；
[0173]
步骤42，为了确定新节点的方向和曲率，对不临近起点和终点的节点，以左子节点为例，画三个圆，即经过上一节点的左子节点、该左子节点、下一节点的左子节点的圆，经过上一节点的主节点、该左子节点、下一节点的主节点的圆，经过上一节点的右子节点、该左子节点、下一节点的右子节点的圆(都在该左子节点的坐标系下)；那么该节点的方向就是三个圆在该左子节点的切线方向的平均值，即沿该左子节点自身的z轴，将该左子节点的x轴方向旋转至三个圆在该左子节点的切线方向的平均值；该左子节点的曲率即为三圆曲率的平均值；对于临近起点或终点节点的左子节点，方向和曲率都由经过上一节点的主节点、该左子节点、下一节点的主节点的圆确定；
[0174]
步骤43，由于除起点终点外的节点各自都分为了三个节点，对其中一个主节点，利用阿克曼运动学方程局部线性化后的解析解法，便可生成其与下一个主节点的九条轨迹，对连接两点node1和node2的轨迹赋予如下公式所示的代价ct，便可依据有权图的单源最短路径算法(dijkstra算法)进行图搜索，获得最优路径；
[0175][0176]
其中，w
len
w
cur
w
tra
＝1且w
len
,w
cur
,w
tra
＞0分别为时间消耗、最大曲率大小、可通行性好坏的权重，本实例中，取w
len
＝0.25,w
cur
＝0.25,w
tra
＝0.5，t,κ
m
,τ分别表示该轨迹的时间消耗、最大曲率值和node2处地形评估的结果，t
min
,t
max
,κ
max
分别表示图中所有轨迹段的最短最长时间消耗、无人车可接受的最大曲率阈值，若最大曲率值κ
m
＞κ
max
或node2处地形评估的结果τ＝0，则强制令ct＝∞，表示该轨迹段是不可被执行的；
[0177]
步骤44，若图搜索后所得最优路径中的节点仍然全是主节点，则局部优化结束，返回轨迹；否则，对那些从主节点“被优化”到左子节点或者右子节点的节点，下次迭代时平移的δd变为原来的γ倍(0＜γ＜1，本实例中取γ＝0.9)，直到所有节点的δd小于阈值δd
min
(本实例取δd
min
＝0.02m)；若所有节点的δd小于阈值δd
min
，则局部优化结束，返回轨迹，否则以该次优化后的轨迹为初始轨迹，转步骤41。
[0178]
图6为改进的双向快速拓展随机树算法生成轨迹的效果示意图。图7为基于改进的快速拓展随机树法的全局轨迹优化算法的效果示意图。图8为基于有权图的单源最短路径算法的局部轨迹优化算法的效果示意图。图中方形片状物表示点云，带箭头的线段为生成
轨迹的节点，箭头的方向表示在该节点时，无人车x轴正方向的朝向。本发明生成的轨迹十分贴合地形，经过全局优化的轨迹，大大降低了轨迹的时间消耗，经过局部优化的轨迹，大大降低了轨迹的曲率，算法具有实时性、最优性。
[0179]
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。