CN113744317A - 一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法 - Google Patents

Abstract

本发明提供了一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法及装置，所述方法包括以下步骤：步骤1，将输入的无人车的初始位姿和目标位姿利用点云地图进行位姿投影和地形评估；步骤2，分别以无人车初始位姿和目标位姿投影获得的位姿为根节点，利用轨迹库执行改进的双向快速拓展随机树算法，生成初始轨迹；步骤3，将步骤2中生成的初始轨迹基于改进的快速拓展随机树法进行全局轨迹优化；步骤4，将步骤3中生成的轨迹基于有权图的单源最短路径算法进行局部轨迹优化，得到最终轨迹。本发明直接采用点云信息进行轨迹生成，最大程度地保留了环境信息，并避免了繁琐的计算，保证了算法的实时性。

Description

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

技术领域

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

背景技术

许多具有挑战性的应用场景(如搜救、探索、检查或运输)中，自主导航对于无人车的安全高效运行至关重要，它让无人车得以执行或协助对人类来说是重复的、疲惫的甚至是危险的任务。除了定位和建图，轨迹生成是任何自主导航系统的关键功能。

无人车轨迹生成技术，指无人车根据自身传感器对环境感知，从当前点到目标点，自动规划一段安全的运动轨迹。针对阿克曼底盘的轨迹生成主要解决三大问题：1)连通性，即轨迹必须能连接起点和终点；2)可执行性，即轨迹满足阿克曼底盘自身的约束，如最小转弯半径(最大曲率)等；3)最优性，即在完成以上任务的前提下，轨迹的长度、对机器人的能耗应被尽量优化。

在结构路面下，现有算法往往会将三维的环境降维成二维，方便计算；但在非结构路面，如野外环境(崎岖的草坪、充满土包的山路)，现有技术往往只将环境区分为障碍物和可通行区域，例如使用占据栅格法，这样做没有考虑地形对无人车的影响，因此计算出的轨迹既不能保证可执行性，也不能保证在一定标准下(如轨迹的长度、对机器人的能耗)的最优性。

有一些新型技术，允许将环境的点云三维重建，例如用三角形网格表示环境信息。随后，无人车只需在重建后的地图中执行路径搜索，并考虑自身种种约束，便可生成具有可执行性和最优性的轨迹。但这类做法由于需要对环境的点云三维重建，会产生大量的时间消耗，缺乏实时性，同时，三维重建的过程虽然较其他简单的建模方法，较好得保留了环境特征，但仍然会造成部分信息的丢失。

发明内容

针对现有技术不足，本发明公开了一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法。该方法利用多线激光雷达采集的点云数据建成的点云地图，根据无人车当前位姿和目标位姿，实现轨迹生成。

本发明解决其技术问题所采用的技术方案是：一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，该方法包括以下步骤：

步骤1，将输入的无人车的初始位姿和目标位姿利用点云地图进行位姿投影和地形评估；

步骤2，分别以无人车初始位姿和目标位姿投影获得的位姿为根节点，利用事先计算好的轨迹库执行改进的双向快速拓展随机树算法(RRTConnect算法)，生成初始的轨迹；

步骤3，将步骤2中生成的轨迹基于改进的快速拓展随机树法(RRTStar算法)进行全局轨迹优化；

步骤4，将步骤3中生成的轨迹基于有权图的单源最短路径算法(dijkstra算法)进行局部的轨迹优化，得到最终轨迹。

进一步地，所述步骤1中，设无人车位姿由关于地图坐标系的一个大小为3×3的旋转矩阵R＝[X_R,Y_R,Z_R](其中X_R,Y_R,Z_R均是大小为3×1的向量)和一个大小为3×1的平移向量t表示，那么利用点云地图进行位姿投影的具体步骤如下：

步骤111，在点云中找到与t₀最邻近的K个点p_ii，ii＝1,2,...,K，K值根据点云的密度确定，均为3×1的向量，求出它们的重心

和相关的协方差矩阵Cov(t)，二者计算公式如下：

步骤112，求出Cov(t)最小特征值对应的特征向量，记为v₀，则可计算对应投影在地表上的位姿的z方向(与底盘垂直、并从车底指向车顶的方向)的向量n，其计算公式如下：

n＝sign(Z_R·v₀)v₀

其中，sign(·)表示符号函数。

步骤113，再利用Y_R，则可计算对应投影在地表上的位姿的x方向(车辆向前行驶的方向)的向量X_R'，进而可以求得投影后的旋转矩阵R'，它们的计算公式如下：

R'＝[X_R',n×X_R',n]

步骤114，进而，投影在地表上的平移向量t'可由如下公式计算：

基于上述四步，利用点云地图，将无人车初始的位姿投影到了地表上，投影后的位姿可由R'和t'表示。

进一步地，所述步骤1中，设地形评估的结果可由“可穿越性”τ∈[0,1]表示，τ＝0表示改位姿不可达，τ＞0则表示位姿可达，且值越大，表示该位姿的“可穿越性”越良好，那么利用点云地图进行地形评估的步骤如下：

步骤121，根据位姿投影获得的旋转矩阵R'，计算无人车的滚转角φ和俯仰角θ，设

a₁₁～a₃₃为矩阵中的元素，则公式如下：

步骤122，利用点云信息，对以无人车为中心、以r_rob为半径(该值可由对无人车的实际测量获得)的球形区域内所有点云上的点(设有M个点)，分别求粗糙度，记作ρ_i(i＝1,2,...,M)，并设无人车可接受的最大粗糙度为ρ_max，则此位姿下的粗糙度ρ_rob可由如下公式计算：

步骤123，设无人车可接收的最大滚转角为φ_max，最大俯仰角为θ_max，最小俯仰角为θ_min，若对应的ρ_rob,θ,φ其中有一者超出限度，则强制“可穿越性”τ赋值为零，表示该位姿不可行，否则τ由以下公式计算获得：

其中，w_ρ+w_φ+w_θ＝1且w_ρ,w_φ,w_θ＞0为各项权重。

进一步地，所述步骤122中，计算点云上单个点p_i的粗糙度ρ_i的具体步骤如下：

步骤1221，在点云中找到与p_i距离小于r_p(由人为指定)的点p_j，设有M个，即j＝1,2,...,M，均为3×1的向量，求出它们的重心

和相关的协方差矩阵Cov(p_i)，二者计算公式如下：

步骤1222，求出Cov(p_i)最小特征值对应的特征向量，记为n_i，则n_i的物理意义为p_j所在平面的法线。随后对上述M个点中与p_i距离小于

的点，分别由如下公式计算它们到该平面的距离d_ij：

步骤1223，为去除离群点，对所求出的d_ij排序，设有N个(N≤M)，取大小排行正好15％和75％的点，分别记作d_ijmin和d_ijmax，则p_i的粗糙度ρ_i由如下公式给出：

ρ_i＝|d_ijmax-d_ijmin|。

进一步地，所述步骤2中，设无人车机体坐标系原点o为后轮车轴的中心，向前行驶方向为x轴正方向，与底盘垂直、并从车底指向车顶的方向为z轴正方向，依照右手系确定y轴，轨迹库中的每一条轨迹由相对无人车机体坐标系的变化(Δx,Δy,Δψ,Δκ)组成，Δx和Δy分别为相对于无人车x轴和y轴变化的距离，Δψ无人车变化的偏航角，Δκ为曲率变化，设轨迹库中轨迹的初始曲率都为零，那么生成轨迹库的具体步骤如下：

步骤211，取半径r_t、偏航角变化值δψ、正整数k，轨迹库中的Δψ取0,±δψ,±2δψ,...,±kδψ，生成总共2k+1条轨迹，同时，为保证轨迹库中的轨迹都可自由相互连接，Δκ取零；

步骤212，对每一个Δψ，取以o为圆心、r_t为半径且在x轴正半平面上的圆弧，对圆弧上的每个点(x_i3,y_i3)，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x_i3,y_i3,Δψ,0)的轨迹，其中i3表示将圆弧离散化为点后，每个点的下标，i为自然数；

步骤213，分别计算由圆弧上的每个点(x_i3,y_i3)生成轨迹的最大曲率值κ_m，选择κ_m最小的那条作为对应Δψ的轨迹，即：若使κ_m最小的轨迹对应圆弧上点为(x_m,y_m)，则对应此Δψ的轨迹库中的轨迹为(Δx_m,Δy_m,Δψ,0)。

进一步地，所述所述步骤212中，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x_i,y_i,Δψ,0)轨迹的具体步骤如下：

步骤2121，设阿克曼底盘运动学方程如下，系统输入u为曲率的导数，速度恒为1：

其中，κ为曲率值，ψ为无人车偏航角；

那么，在输入为零，且状态为零处将系统线性化，得到运动学方程变为：

步骤2122，为保证轨迹的光滑性，设输入为关于时间t的二次多项式，即u＝at²+bt+c，其中a,b,c为多项式系数，并设总运动时间为T，则由积分可得：

步骤2123，根据末状态(x_i3,y_i3,Δψ,0)，列出一个四元一次线性方程组：

求解可得未知数(a,b,c,T)，即得从状态(0,0,0,0)到(x_i,y_i,Δψ,0)轨迹。

进一步地，所述步骤2中改进的双向快速拓展随机树算法的步骤具体如下：

步骤221，建立两棵搜索树，分别以初始位姿和目标位姿投影获得的位姿为根节点，分别记作ST和GT；

步骤222，任选轨迹库中的一条轨迹TrajSeg和ST中的一个节点Node，在Node的坐标系下，运用轨迹TrajSeg，获得新的位姿Tseg；

步骤223，将获得的新位姿Tseg利用点云进行位姿投影和地形评估，获得Tseg'和对应的τ_Tseg'，τ_Tseg'为对应的地形评估结果，若τ_Tseg'＞0，将其加入ST，记作Node_new，父节点为Node；否则，转步骤222；

步骤224，计算GT中每个节点与Node_new的欧式距离，若存在距离小于指定距离r_c的节点，则连接ST与GT，若连接成功，算法结束，轨迹生成；否则转步骤225；

步骤225，求GT中与Node_new欧式距离最近的节点Node_nearest，并在Node_nearest的坐标系下，运用所有轨迹库中的轨迹，获得新的位姿；

步骤226，利用点云，将新的位姿进行位姿投影和地形评估，获得这些新位姿投影后的位姿和对应的τ，τ为地形评估结果的“可穿越性”，取其中与Node_new欧式距离最近且τ＞0的位姿，记作T_best，将其加入GT，父节点为Node_nearest，转步骤227；若不存在这样的位姿，转步骤227；

步骤227，交换ST和GT，转步骤222。

进一步地，所述步骤224中连接ST和GT的具体步骤如下：

步骤2241，计算GT中每个节点与Node_new的欧式距离，若存在距离小于指定距离r_c的节点，记作Node_try；

步骤2242，设Node_try的旋转矩阵为R_try，平移向量为t_try；Node_new的旋转矩阵为R_new，平移向量为t_new；按如下公式求出Node_try在Node_new坐标系下的(Δx,Δy,Δψ,Δκ)，Δx和Δy分别为相对于无人车x轴和y轴变化的距离，Δψ无人车变化的偏航角，Δκ为曲率变化：

Δx＝t_x,Δy＝t_y,

其中，a₁₁～a₃₃,t_x,t_y,t_z均表示矩阵中的元素；

步骤2243，根据(Δx,Δy,Δψ,Δκ)和阿克曼运动学方程局部线性化后的解析解法，获得轨迹的方程，若该轨迹的最大曲率小于无人车可接受的最大曲率κ_max，则对该轨迹每隔指定时间Δt采样一个位姿，利用位姿投影和地形评估，获得新的位姿，若所有地形评估所得的τ均大于零，连接成功，将所有位姿加入ST；一旦轨迹的最大曲率大于κ_max或地形评估中存在τ＝0，则连接失败。

进一步地，所述步骤3中基于改进的快速拓展随机树法进行全局轨迹优化的步骤具体如下：

步骤31，在生成的轨迹中任取一个路径节点，在以该路径点为球心、r_near为半径的球形区域内任取点云中的一个点p；

步骤32，将节点沿该节点与p连线的方向平移至p，获得位姿P，对P进行位姿投影和地形评估，若所得地形评估结果τ_P＝0，转步骤31，否则转步骤33；

步骤33，找到以P位置为球心、指定半径为r_s的球形区域内ST或GT中的节点，用步骤224中连接ST和GT的方法从这些节点连接P；

步骤34，若存在一个节点Q可以连接P，且以Q作为父节点，生成从根节点到P的轨迹消耗时间最短，则将P加入ST或GT，父节点为Q；否则，转步骤31；

步骤35，对以P位置为球心、r_s为半径的球形区域内ST或GT中的节点，再反过来用步骤224中连接ST和GT的方法从P连接这些节点；

步骤36，对其中的一个节点Node_i，若连接成功，且以P作为父节点可以减少从根节点到Node_i的时间消耗，则改变Node_i的父节点为P；

步骤37，累计总的被改变父节点的节点个数N_rewire，若N_rewire大于指定阈值N_max，则全局优化完成；否则，转步骤31。

进一步地，所述步骤4中基于有权图的单源最短路径算法进行局部轨迹优化的步骤具体如下：

步骤41，对经过全局优化后的轨迹，除了起点和终点外的所有节点，分别沿各自的y轴平移指定距离±δd，获得两个新的位姿，再将两个新的位姿进行投影和地形评估，获得两个新节点，称为主节点的左子节点和右子节点，如此，对一条具有k_w个节点的轨迹，获得3k_w-4个节点；

步骤42，为了确定新节点的方向和曲率，对不临近起点和终点的节点，以左子节点为例，画三个圆，即经过上一节点的左子节点、该左子节点、下一节点的左子节点的圆，经过上一节点的主节点、该左子节点、下一节点的主节点的圆，经过上一节点的右子节点、该左子节点、下一节点的右子节点的圆；那么该节点的方向就是三个圆在该左子节点的切线方向的平均值，即沿该左子节点自身的z轴，将该左子节点的x轴方向旋转至三个圆在该左子节点的切线方向的平均值；该左子节点的曲率即为三圆曲率的平均值；对于临近起点或终点节点的左子节点，方向和曲率都由经过上一节点的主节点、该左子节点、下一节点的主节点的圆确定；

步骤43，由于除起点终点外的节点各自都分为了三个节点，对其中一个主节点，利用阿克曼运动学方程局部线性化后的解析解法，便可生成其与下一个主节点的九条轨迹，对连接两点node₁和node₂的轨迹赋予如下公式所示的代价ct，便可依据有权图的单源最短路径算法进行图搜索，获得最优路径；其中node₁和node₂即经过步骤41得到的3k_w-4个节点中的任意两个节点；

其中，w_len+w_cur+w_tra＝1且w_len,w_cur,w_tra＞0，w_len,w_cur,w_tra分别为时间消耗、最大曲率大小、可通行性好坏的权重，T,κ_m,τ分别表示该轨迹的时间消耗、最大曲率值和node₂处地形评估的结果，T_min,T_max,κ_max分别表示图中所有轨迹段的最短最长时间消耗、无人车可接受的最大曲率阈值，若最大曲率值κ_m＞κ_max或node₂处地形评估的结果τ＝0，则强制令ct＝∞，表示该轨迹段是不可被执行的；

步骤44，若图搜索后所得最优路径中的节点仍然全是主节点，则局部优化结束，返回轨迹；否则，对那些从主节点“被优化”到左子节点或者右子节点的节点，下次迭代时平移的δd变为原来的γ倍，0＜γ＜1，直到所有节点的δd小于设定阈值δd_min；若所有节点的δd小于设定阈值δd_min，则局部优化结束，返回轨迹，否则以该次优化后的轨迹为初始轨迹，转步骤41。

本发明的有益效果如下：

(1)本发明生成轨迹的节点间距小于车辆长度，并利用点云进行位姿投影，因此所得的轨迹能很好地依附于地形表面。

(2)本发明生成轨迹节点间的连接采用局部线性化的阿克曼运动学模型，曲率采用三次多项式计算，保证轨迹充分光滑；轨迹生成的过程中考虑了阿克曼底盘车辆的姿态约束、最小转弯半径约束，具有良好的可执行性。

(3)本发明直接采用点云信息进行轨迹生成，最大程度地保留了环境信息，并避免了繁琐的计算，保证了算法的实时性。

(4)本发明生成的轨迹，在获得初始轨迹后，对轨迹进行了全局优化和局部优化，保证了所得轨迹的最优性。

(5)轨迹生成可使用局部点云地图，可以在全局环境未知的情况下使用。

附图说明

图1为本发明整体原理步骤图；

图2为地形评估算法流程图；

图3为改进的双向快速拓展随机树算法流程图；

图4为基于改进的快速拓展随机树法的全局轨迹优化算法流程图；

图5为基于有权图的单源最短路径算法的局部轨迹优化算法流程图；

图6为改进的双向快速拓展随机树算法生成轨迹的效果示意图；

图7为基于改进的快速拓展随机树法的全局轨迹优化算法的效果示意图；

图8为基于有权图的单源最短路径算法的局部轨迹优化算法的效果示意图。

具体实施方式

本方法针对非结构路面建模困难、计算量大导致算法实时性不强的问题，以及占据栅格法等粗略建图导致生成轨迹缺乏可执行性和最优性的问题，直接利用点云地图提供的信息，在执行改进的双向快速拓展随机树法(RRTConnect算法)的同时进行位姿投影与地形评估，并基于改进的快速拓展随机树法(RRTStar算法)进行全局轨迹优化、基于有权图的单源最短路径算法(dijkstra算法)进行局部的轨迹优化，完成具有阿克曼底盘无人车的轨迹生成。下文中，结合附图和实施例对本发明作进一步阐述。

图1为本发明整体原理步骤图，本发明提出了一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，包括以下步骤：

步骤1，将输入的无人车的初始位姿和目标位姿利用点云地图进行位姿投影和地形评估；

步骤2，分别以无人车初始位姿和目标位姿投影获得的位姿为根节点，利用事先计算好的轨迹库执行改进的双向快速拓展随机树算法(RRTConnect算法)，生成初始的轨迹；

步骤3，将步骤2中生成的轨迹基于改进的快速拓展随机树法(RRTStar算法)进行全局轨迹优化；

步骤4，将步骤3中生成的轨迹基于有权图的单源最短路径算法(dijkstra算法)进行局部的轨迹优化，得到最终轨迹。

进一步地，所述步骤1中，设无人车位姿由关于地图坐标系的一个大小为3×3的旋转矩阵R＝[X_R,Y_R,Z_R]和一个大小为3×1的平移向量t表示，那么利用点云地图进行位姿投影的具体步骤如下：

步骤111，在点云中找到与t最邻近的K个点p_i(i＝1,2,...,K)(本实例中，取K＝25)，均为3×1的向量，求出它们的重心

和相关的协方差矩阵Cov(t)，二者计算公式如下：

步骤112，求出Cov(t)最小特征值对应的特征向量，记为v₀，则可计算对应投影在地表上的位姿的z方向(与底盘垂直、并从车底指向车顶的方向)的向量n，其计算公式如下：

n＝sign(Z_R·v₀)v₀

其中，sign(·)表示符号函数。

步骤113，再利用Y_R，则可计算对应投影在地表上的位姿的x方向(车辆向前行驶的方向)的向量X_R'，进而可以求得投影后的旋转矩阵R'，它们的计算公式如下：

R'＝[X_R',n×X_R',n]

步骤114，进而，投影在地表上的平移向量t'可由如下公式计算：

基于上述四步，利用点云地图，将无人车初始的位姿投影到了地表上，投影后的位姿可由R'和t'表示。

图2为地形评估算法流程图。设地形评估的结果可由“可穿越性”τ∈[0,1]表示，τ＝0表示改位姿不可达，τ＞0则表示位姿可达，且值越大，表示该位姿的“可穿越性”越良好，那么利用点云地图进行地形评估的步骤如下：

步骤121，根据位姿投影获得的旋转矩阵R'，计算无人车的滚转角φ和俯仰角θ，设

则公式如下：

步骤122，利用点云信息，对以无人车为中心、以r_rob为半径(该值可由对无人车的实际测量获得)的球形区域内所有点云上的点(设有M个点)，分别求粗糙度，记作ρ_i(i＝1,2,...,M)，并设无人车可接受的最大粗糙度为ρ_max，本实例中，取ρ_max＝0.08，则此位姿下的粗糙度ρ_rob可由如下公式计算：

步骤123，设无人车可接收的最大滚转角为

最大俯仰角为θ_max，最小俯仰角为

若对应的ρ_rob,θ,φ其中有一者超出限度，则强制“可穿越性”τ赋值为零，表示该位姿不可行，否则τ由以下公式计算获得：

其中，w_ρ+w_φ+w_θ＝1且w_ρ,w_φ,w_θ＞0为各项权重，优选的，本实例中取w_ρ＝0.8,w_φ＝0.1,w_θ＝0.1。

进一步地，所述步骤122中，计算点云上单个点p_i的粗糙度ρ_i的具体步骤如下：

步骤1221，在点云中找到与p_i距离小于r_p(本实例中取r_p＝0.3m)的点p_j，设有M个，即j＝1,2,...,M，均为3×1的向量，求出它们的重心

和相关的协方差矩阵Cov(p_i)，二者计算公式如下：

步骤1222，求出Cov(p_i)最小特征值对应的特征向量，记为n_i，则n_i的物理意义为p_j所在平面的法线。随后对上述M个点中与p_i距离小于

的点，分别由如下公式计算它们到该平面的距离d_ij：

步骤1223，为去除离群点，对所求出的d_ij排序，设有N个(N≤M)，取大小排行正好15％和75％的点，分别记作d_ijmin和d_ijmax，则p_i的粗糙度ρ_i由如下公式给出：

ρ_i＝|d_ijmax-d_ijmin|。

进一步地，所述步骤2中，设无人车机体坐标系原点o为后轮车轴的中心，向前行驶方向为x轴正方向，与底盘垂直、并从车底指向车顶的方向为z轴正方向，依照右手系确定y轴，轨迹库中的每一条轨迹由相对无人车机体坐标系的变化(Δx,Δy,Δψ,Δκ)组成(Δκ为曲率变化，设轨迹库中轨迹的初始曲率都为零)，那么生成轨迹库的步骤如下：

步骤211，取合适的半径r_t(本实例中取r_t＝0.4)、合适的偏航角变化值δψ(优选为0.1rad)、合适的正整数k(本实例中取δψ＝0.1rad,k＝5)，轨迹库中的Δψ便可取0,±δψ,±2δψ,...,±kδψ，将生成总共2k+1条轨迹，同时，为保证轨迹库中的轨迹都可自由相互连接，Δκ取零；

步骤212，对每一个Δψ，取以o为圆心、r_t为半径且在x轴正半平面上的圆弧，对圆弧上的每个点(x_i,y_i)，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x_i,y_i,Δψ,0)的轨迹；

步骤213，分别计算由圆弧上的每个点(x_i,y_i)生成轨迹的最大曲率值κ_m，选择κ_m最小的那条作为对应Δψ的轨迹，即：若使κ_m最小的轨迹对应圆弧上点为(x_m,y_m)，则对应此Δψ的轨迹库中的轨迹为(Δx_m,Δy_m,Δψ,0)。

进一步地，所述所述步骤212中，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x_i,y_i,Δψ,0)轨迹的具体步骤如下：

步骤2121，设阿克曼底盘运动学方程如下，系统输入u为曲率的导数，速度恒为1：

那么，在输入为零，且状态为零处将系统线性化，得到运动学方程变为：

步骤2122，为保证轨迹的光滑性，设输入为关于时间t的二次多项式，即u＝at²+bt+c，并设总运动时间为T，则由积分可得：

步骤2123，根据末状态(x_i,y_i,Δψ,0)，可列出一个四元一次线性方程组：

求解可得未知数(a,b,c,T)，即得从状态(0,0,0,0)到(x_i,y_i,Δψ,0)轨迹。

图3为改进的双向快速拓展随机树算法(RRTConnect算法)流程图，步骤如下：

步骤221，建立两棵搜索树，分别以初始位姿和目标位姿投影获得的位姿为根节点，分别记作ST和GT；

步骤222，任选轨迹库中的一条轨迹TrajSeg和ST中的一个节点Node，在Node的坐标系下，运用轨迹TrajSeg，获得新的位姿Tseg；

步骤223，将获得的新位姿Tseg利用点云进行位姿投影和地形评估，获得Tseg'和对应的τ_Tseg'，若τ_Tseg'＞0，将其加入ST，记作Node_new，父节点为Node；否则，转步骤222；

步骤224，计算GT中每个节点与Node_new的欧式距离，若存在距离小于r_c(本实例中取r_c＝1.2m)的节点，则尝试连接ST与GT，若连接成功，算法结束，轨迹生成；否则转步骤225；

步骤225，求GT中与Node_new欧式距离最近的节点Node_nearest，并在Node_nearest的坐标系下，运用所有轨迹库中的轨迹，获得若干新的位姿；

步骤226，利用点云，将获得的若干新位姿进行位姿投影和地形评估，获得这些新位姿投影后的位姿和对应的τ，取其中与Node_new欧式距离最近且τ＞0的位姿，记作T_best，将其加入GT，父节点为Node_nearest，转步骤227；若不存在这样的位姿，转步骤227；

步骤227，交换ST和GT，转步骤222。

进一步地，所述步骤224中，尝试连接ST和GT的具体步骤如下：

步骤2241，计算GT中每个节点与Node_new的欧式距离，若存在距离小于r_c的节点，记作Node_try；

步骤2242，设Node_try的旋转矩阵为R_try，平移向量为t_try；Node_new的旋转矩阵为R_new，平移向量为t_new；按如下公式求出Node_try在Node_new坐标系下的(Δx,Δy,Δψ,Δκ)：

步骤2243，根据(Δx,Δy,Δψ,Δκ)和阿克曼运动学方程局部线性化后的解析解法，获得轨迹的方程，若该轨迹的最大曲率小于无人车可接受的最大曲率κ_max，则对该轨迹每隔Δt(本实例中取Δt＝0.4s)时间采样一个位姿，利用位姿投影和地形评估，获得新的位姿，若所有地形评估所得的τ均大于零，连接成功，将所有位姿加入ST；一旦轨迹的最大曲率大于κ_max或地形评估中存在τ＝0，则连接失败。

图4为基于改进的快速拓展随机树法(RRTStar算法)的全局轨迹优化算法流程图。其具体步骤如下：

步骤31，在生成的轨迹中任取一个路径节点，在以该路径点为球心、r_near为半径(优选的，取三倍的r_t)的球形区域内任取点云中的一个点p；

步骤32，将节点沿该节点与p连线的方向平移至p，获得位姿P，对P进行位姿投影和地形评估，若所得地形评估结果τ_P＝0，转步骤31，否则转步骤33；

步骤33，找到以P位置为球心、r_s为半径(本实例中取r_s＝6r_t)的球形区域内ST或GT中的节点，尝试用步骤224中连接ST和GT的方法从这些节点连接P；

步骤34，若存在一个节点Q可以连接P，且以Q作为父节点，生成从根节点到P的轨迹消耗时间最短，则将P加入ST或GT，父节点为Q；否则，转步骤31；

步骤35，对以P位置为球心、r_s为半径的球形区域内ST或GT中的节点，再反过来尝试用步骤(2)中连接ST和GT的方法从P连接这些节点；

步骤36，对其中的一个节点Node_i，若连接成功，且以P作为父节点可以减少从根节点到Node_i的时间消耗，则改变Node_i的父节点为P；

步骤37，累计总的被改变父节点的节点个数N_rewire，若N_rewire大于阈值N_max(人为指定)，则全局优化完成；否则，转步骤31。

图5为基于有权图的单源最短路径算法(dijkstra算法)的局部轨迹优化算法流程图。其具体步骤如下

步骤41，对经过全局优化后的轨迹，除了起点和终点外的所有节点，分别沿各自的y轴平移±δd(本实例中，其初始值取0.06m)，获得两个新的位姿，再将两个新的位姿进行投影和地形评估，获得两个新节点，称为主节点的左子节点和右子节点，如此，对一条具有k个节点的轨迹，可获得3k-4个节点；

步骤42，为了确定新节点的方向和曲率，对不临近起点和终点的节点，以左子节点为例，画三个圆，即经过上一节点的左子节点、该左子节点、下一节点的左子节点的圆，经过上一节点的主节点、该左子节点、下一节点的主节点的圆，经过上一节点的右子节点、该左子节点、下一节点的右子节点的圆(都在该左子节点的坐标系下)；那么该节点的方向就是三个圆在该左子节点的切线方向的平均值，即沿该左子节点自身的z轴，将该左子节点的x轴方向旋转至三个圆在该左子节点的切线方向的平均值；该左子节点的曲率即为三圆曲率的平均值；对于临近起点或终点节点的左子节点，方向和曲率都由经过上一节点的主节点、该左子节点、下一节点的主节点的圆确定；

步骤43，由于除起点终点外的节点各自都分为了三个节点，对其中一个主节点，利用阿克曼运动学方程局部线性化后的解析解法，便可生成其与下一个主节点的九条轨迹，对连接两点node₁和node₂的轨迹赋予如下公式所示的代价ct，便可依据有权图的单源最短路径算法(dijkstra算法)进行图搜索，获得最优路径；

其中，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,τ分别表示该轨迹的时间消耗、最大曲率值和node₂处地形评估的结果，T_min,T_max,κ_max分别表示图中所有轨迹段的最短最长时间消耗、无人车可接受的最大曲率阈值，若最大曲率值κ_m＞κ_max或node₂处地形评估的结果τ＝0，则强制令ct＝∞，表示该轨迹段是不可被执行的；

步骤44，若图搜索后所得最优路径中的节点仍然全是主节点，则局部优化结束，返回轨迹；否则，对那些从主节点“被优化”到左子节点或者右子节点的节点，下次迭代时平移的δd变为原来的γ倍(0＜γ＜1，本实例中取γ＝0.9)，直到所有节点的δd小于阈值δd_min(本实例取δd_min＝0.02m)；若所有节点的δd小于阈值δd_min，则局部优化结束，返回轨迹，否则以该次优化后的轨迹为初始轨迹，转步骤41。

图6为改进的双向快速拓展随机树算法生成轨迹的效果示意图。图7为基于改进的快速拓展随机树法的全局轨迹优化算法的效果示意图。图8为基于有权图的单源最短路径算法的局部轨迹优化算法的效果示意图。图中方形片状物表示点云，带箭头的线段为生成轨迹的节点，箭头的方向表示在该节点时，无人车x轴正方向的朝向。本发明生成的轨迹十分贴合地形，经过全局优化的轨迹，大大降低了轨迹的时间消耗，经过局部优化的轨迹，大大降低了轨迹的曲率，算法具有实时性、最优性。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (10)

1.一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述方法包括以下步骤：

步骤1，将输入的无人车的初始位姿和目标位姿利用点云地图进行位姿投影和地形评估；

步骤2，分别以无人车初始位姿和目标位姿投影获得的位姿为根节点，利用轨迹库执行改进的双向快速拓展随机树算法，生成初始轨迹；

步骤3，将步骤2中生成的初始轨迹基于改进的快速拓展随机树法进行全局轨迹优化；

步骤4，将步骤3中生成的轨迹基于有权图的单源最短路径算法进行局部轨迹优化，得到最终轨迹。

2.如权利要求1所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤1中，设无人车的初始位姿由关于地图坐标系的一个大小为3×3的旋转矩阵R＝[X_R,Y_R,Z_R]和一个大小为3×1的平移向量t₀表示，其中X_R,Y_R,Z_R均是大小为3×1的向量，那么利用点云地图进行位姿投影的具体步骤如下：

步骤111，在点云中找到与t₀最邻近的K个点p_ii，ii＝1,2,...,K，K值根据点云的密度确定，均为3×1的向量，求出它们的重心

和相关的协方差矩阵Cov(t)，二者计算公式如下：

步骤112，求出Cov(t)最小特征值对应的特征向量，记为v₀，计算对应投影在地表上的位姿的z方向的向量n，其计算公式如下：

n＝sign(Z_R·v₀)v₀

其中，sign(·)表示符号函数；

步骤113，再利用Y_R，计算对应投影在地表上的位姿的x方向的向量X_R'，进而求得投影后的旋转矩阵R'，计算公式如下：

R'＝[X_R',n×X_R',n]

步骤114，投影在地表上的平移向量t'采用如下公式计算：

基于上述步骤，利用点云地图，将无人车的初始位姿投影到了地表上，投影后的位姿由R'和t'表示。

3.如权利要求2所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤1中，设地形评估的结果由τ表示，τ∈[0,1]，τ＝0表示改位姿不可达，τ＞0则表示位姿可达；

那么利用点云地图进行地形评估的具体步骤如下：

步骤121，根据投影后的旋转矩阵R'，计算无人车的滚转角φ和俯仰角θ，设

a₁₁～a₃₃为矩阵中的元素，则公式如下：

步骤122，利用点云信息，对以无人车为中心、以r_rob为半径的球形区域内所有点云上的点p_i分别求粗糙度，记作ρ_i，i＝1,2,...,M，M为整数，表示点云上点的个数，并设无人车可接受的最大粗糙度为ρ_max，则此位姿下的粗糙度ρ_rob可由如下公式计算：

步骤123，设无人车可接收的最大滚转角为φ_max，最大俯仰角为θ_max，最小俯仰角为θ_min，若对应的ρ_rob,θ,φ其中有一者超出限度，则强制“可穿越性”τ赋值为零，表示该位姿不可行，否则τ由以下公式计算获得：

其中，w_ρ,w_φ,w_θ为各项权重，w_ρ+w_φ+w_θ＝1且w_ρ,w_φ,w_θ＞0。

4.如权利要求3所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤122中ρ_i的计算具体包括以下步骤：

步骤1221，在点云中找到与p_i距离小于设定距离r_p的点p_j，设有M个，即j＝1,2,...,M，均为3×1的向量，求出它们的重心

和相关的协方差矩阵Cov(p_i)，二者计算公式如下：

步骤1222，求出Cov(p_i)最小特征值对应的特征向量，记为n_i，则n_i的物理意义为p_j所在平面的法线；随后对上述M个点中与p_i距离小于

的点，分别由如下公式计算它们到所述平面的距离d_ij：

步骤1223，为去除离群点，对所求出的d_ij排序，设有N个点与p_i距离小于

N≤M，取大小排行正好15％和75％的点，分别记作d_ijmin和d_ijmax，则p_i的粗糙度ρ_i由如下公式给出：

ρ_i＝|d_ijmax-d_ijmin|。

5.如权利要求1所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤2中，设无人车机体坐标系原点o为后轮车轴的中心，向前行驶方向为x轴正方向，与底盘垂直、并从车底指向车顶的方向为z轴正方向，依照右手系确定y轴，轨迹库中的每一条轨迹由相对无人车机体坐标系的变化(Δx,Δy,Δψ,Δκ)组成，Δx和Δy分别为相对于无人车x轴和y轴变化的距离，Δψ无人车变化的偏航角，Δκ为曲率变化，设轨迹库中轨迹的初始曲率都为零，那么生成轨迹库的具体步骤如下：

步骤211，取半径r_t、偏航角变化值δψ、正整数k，轨迹库中的Δψ取0,±δψ,±2δψ,...,±kδψ，生成总共2k+1条轨迹，同时，为保证轨迹库中的轨迹都可自由相互连接，Δκ取零；

步骤212，对每一个Δψ，取以o为圆心、r_t为半径且在x轴正半平面上的圆弧，对圆弧上的每个点(x_i3,y_i3)，用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x_i3,y_i3,Δψ,0)的轨迹，其中i3表示将圆弧离散化为点后，每个点的下标，i为自然数；

步骤213，分别计算由圆弧上的每个点(x_i3,y_i3)生成轨迹的最大曲率值κ_m，选择κ_m最小的那条作为对应Δψ的轨迹，即：若使κ_m最小的轨迹对应圆弧上点为(x_m,y_m)，则对应此Δψ的轨迹库中的轨迹为(Δx_m,Δy_m,Δψ,0)。

6.如权利要求5所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤212中用阿克曼运动学方程局部线性化后的解析解法，求得从状态(0,0,0,0)到(x_i,y_i,Δψ,0)轨迹的具体步骤如下：

步骤2121，设阿克曼底盘运动学方程如下，系统输入u为曲率的导数，速度恒为1：

其中，κ为曲率值，ψ为无人车偏航角；

那么，在输入为零，且状态为零处将系统线性化，得到运动学方程变为：

步骤2122，为保证轨迹的光滑性，设输入为关于时间t的二次多项式，即u＝at²+bt+c，其中a,b,c为多项式系数，并设总运动时间为T，则由积分可得：

步骤2123，根据末状态(x_i3,y_i3,Δψ,0)，列出一个四元一次线性方程组：

求解可得未知数(a,b,c,T)，即得从状态(0,0,0,0)到(x_i,y_i,Δψ,0)轨迹。

7.如权利要求1所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤2中改进的双向快速拓展随机树算法的步骤具体如下：

步骤221，建立两棵搜索树，分别以初始位姿和目标位姿投影获得的位姿为根节点，分别记作ST和GT；

步骤222，任选轨迹库中的一条轨迹TrajSeg和ST中的一个节点Node，在Node的坐标系下，运用轨迹TrajSeg，获得新的位姿Tseg；

步骤223，将获得的新位姿Tseg利用点云进行位姿投影和地形评估，获得Tseg'和对应的τ_Tseg'，τ_Tseg'为对应的地形评估结果，若τ_Tseg'＞0，将其加入ST，记作Node_new，父节点为Node；否则，转步骤222；

步骤224，计算GT中每个节点与Node_new的欧式距离，若存在距离小于指定距离r_c的节点，则连接ST与GT，若连接成功，算法结束，轨迹生成；否则转步骤225；

步骤225，求GT中与Node_new欧式距离最近的节点Node_nearest，并在Node_nearest的坐标系下，运用所有轨迹库中的轨迹，获得新的位姿；

步骤226，利用点云，将新的位姿进行位姿投影和地形评估，获得这些新位姿投影后的位姿和对应的τ，τ为地形评估结果的“可穿越性”，取其中与Node_new欧式距离最近且τ＞0的位姿，记作T_best，将其加入GT，父节点为Node_nearest，转步骤227；若不存在这样的位姿，转步骤227；

步骤227，交换ST和GT，转步骤222。

8.如权利要求7所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤224中连接ST和GT的具体步骤如下：

步骤2241，计算GT中每个节点与Node_new的欧式距离，若存在距离小于指定距离r_c的节点，记作Node_try；

步骤2242，设Node_try的旋转矩阵为R_try，平移向量为t_try；Node_new的旋转矩阵为R_new，平移向量为t_new；按如下公式求出Node_try在Node_new坐标系下的(Δx,Δy,Δψ,Δκ)，Δx和Δy分别为相对于无人车x轴和y轴变化的距离，Δψ无人车变化的偏航角，Δκ为曲率变化：

Δx＝t_x,Δy＝t_y,

Δκ＝0

其中，a₁₁～a₃₃,t_x,t_y,t_z均表示矩阵中的元素；

步骤2243，根据(Δx,Δy,Δψ,Δκ)和阿克曼运动学方程局部线性化后的解析解法，获得轨迹的方程，若该轨迹的最大曲率小于无人车可接受的最大曲率κ_max，则对该轨迹每隔指定时间Δt采样一个位姿，利用位姿投影和地形评估，获得新的位姿，若所有地形评估所得的τ均大于零，连接成功，将所有位姿加入ST；一旦轨迹的最大曲率大于κ_max或地形评估中存在τ＝0，则连接失败。

9.如权利要求7所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤3中基于改进的快速拓展随机树法进行全局轨迹优化的步骤具体如下：

步骤31，在生成的轨迹中任取一个路径节点，在以该路径点为球心、r_near为半径的球形区域内任取点云中的一个点p；

步骤32，将节点沿该节点与p连线的方向平移至p，获得位姿P，对P进行位姿投影和地形评估，若所得地形评估结果τ_P＝0，转步骤31，否则转步骤33；

步骤33，找到以P位置为球心、指定半径为r_s的球形区域内ST或GT中的节点，用步骤224中连接ST和GT的方法从这些节点连接P；

步骤34，若存在一个节点Q可以连接P，且以Q作为父节点，生成从根节点到P的轨迹消耗时间最短，则将P加入ST或GT，父节点为Q；否则，转步骤31；

步骤35，对以P位置为球心、r_s为半径的球形区域内ST或GT中的节点，再反过来用步骤224中连接ST和GT的方法从P连接这些节点；

步骤36，对其中的一个节点Node_i，若连接成功，且以P作为父节点可以减少从根节点到Node_i的时间消耗，则改变Node_i的父节点为P；

步骤37，累计总的被改变父节点的节点个数N_rewire，若N_rewire大于指定阈值N_max，则全局优化完成；否则，转步骤31。

10.如权利要求1所述的一种非结构路面下只依赖点云的阿克曼底盘轨迹生成方法，其特征在于，所述步骤4中基于有权图的单源最短路径算法进行局部轨迹优化的步骤具体如下：

步骤41，对经过全局优化后的轨迹，除了起点和终点外的所有节点，分别沿各自的y轴平移指定距离±δd，获得两个新的位姿，再将两个新的位姿进行投影和地形评估，获得两个新节点，称为主节点的左子节点和右子节点，如此，对一条具有k_w个节点的轨迹，获得3k_w-4个节点；

步骤42，为了确定新节点的方向和曲率，对不临近起点和终点的节点，以左子节点为例，画三个圆，即经过上一节点的左子节点、该左子节点、下一节点的左子节点的圆，经过上一节点的主节点、该左子节点、下一节点的主节点的圆，经过上一节点的右子节点、该左子节点、下一节点的右子节点的圆；那么该节点的方向就是三个圆在该左子节点的切线方向的平均值，即沿该左子节点自身的z轴，将该左子节点的x轴方向旋转至三个圆在该左子节点的切线方向的平均值；该左子节点的曲率即为三圆曲率的平均值；对于临近起点或终点节点的左子节点，方向和曲率都由经过上一节点的主节点、该左子节点、下一节点的主节点的圆确定；

步骤43，由于除起点终点外的节点各自都分为了三个节点，对其中一个主节点，利用阿克曼运动学方程局部线性化后的解析解法，便可生成其与下一个主节点的九条轨迹，对连接两点node₁和node₂的轨迹赋予如下公式所示的代价ct，便可依据有权图的单源最短路径算法进行图搜索，获得最优路径；其中node₁和node₂即经过步骤41得到的3k_w-4个节点中的任意两个节点；

其中，w_len+w_cur+w_tra＝1且w_len,w_cur,w_tra＞0，w_len,w_cur,w_tra分别为时间消耗、最大曲率大小、可通行性好坏的权重，T,κ_m,τ分别表示该轨迹的时间消耗、最大曲率值和node₂处地形评估的结果，T_min,T_max,κ_max分别表示图中所有轨迹段的最短最长时间消耗、无人车可接受的最大曲率阈值，若最大曲率值κ_m＞κ_max或node₂处地形评估的结果τ＝0，则强制令ct＝∞，表示该轨迹段是不可被执行的；

步骤44，若图搜索后所得最优路径中的节点仍然全是主节点，则局部优化结束，返回轨迹；否则，对那些从主节点“被优化”到左子节点或者右子节点的节点，下次迭代时平移的δd变为原来的γ倍，0＜γ＜1，直到所有节点的δd小于设定阈值δd_min；若所有节点的δd小于设定阈值δd_min，则局部优化结束，返回轨迹，否则以该次优化后的轨迹为初始轨迹，转步骤41。

Images