CN112762957B - 一种基于多传感器融合的环境建模及路径规划方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合的环境建模及路径规划方法，同时采集无人车上相机视场内的图像和激光点云数据；根据图像和激光点云数据生成稠密点云深度图；以稠密点云深度图中的坐标值为输入，结合径向基神经网络，构建相机视场内的环境模型；以轨迹点的高度和梯度最小为目标、环境特征向量阈值和单步步长为约束条件，确定无人车的路径规划轨迹点序列；本发明将将激光点云数据和相机的图像进行融合作为环境模型构建的输入数据，障碍物的位置信息可以作为输入权重应用到环境建模中，可以增加环境模型中野外凹凸障碍物的定位准确性，而且降低了搜索时间。

Description

一种基于多传感器融合的环境建模及路径规划方法

技术领域

本发明属于无人系统自主感知和障碍规避技术领域，尤其涉及一种基于多传感器融合的环境建模及路径规划方法。

背景技术

社会和科学的飞速发展给智能系统的研究带来了更广泛的技术支持。随着传感器和计算机技术的发展，地面智能移动无人平台的研究不断地深入，无人车系统即其中一个典型代表。无人车因行动灵活、可操纵性强、载荷大等特点，常常用来代替人类执行危险、复杂度高、重复性强的任务，比如侦查、搜救、运输、考古勘探、样本采集、军事打击等。地面移动无人车的研究工作对于未来无人系统的发展是具有广泛价值的。

环境建模和路径规划是无人车自主行驶的基础和核心技术之一。与结构化的城市环境不同，野外环境具有地面起伏、障碍物种类繁多、无路标提示等特点，导致无人车的环境感知和路径规划出错率高，给无人车的环境感知与路径规划带来了巨大挑战。

发明内容

本发明的目的是提供一种基于多传感器融合的环境建模及路径规划方法，以提升无人车在野外环境中环境感知和路径规划的准确性。

本发明采用以下技术方案：一种基于多传感器融合的环境建模及路径规划方法，包括以下步骤：

同时采集无人车上相机视场内的图像和激光点云数据；

根据图像和激光点云数据生成稠密点云深度图；

以稠密点云深度图中坐标值为输入，结合径向基神经网络，构建相机视场内的环境模型；

以轨迹点的高度和梯度最小为目标、环境特征向量阈值和单步步长为约束条件，确定无人车的路径规划轨迹点序列。

进一步地，根据图像和激光点云数据生成稠密点云深度图包括：

将激光点云数据投影到图像上，得到稀疏深度图；

对稀疏深度图和图像进行联合双边滤波，得到稠密深度图；

将稠密深度图转换为稠密点云深度图。

进一步地，当采集到新的图像和激光点云数据时，计算当前时刻与环境模型的构建时刻之间的时间差，当时间差大于等于时间周期阈值时，根据新的图像和激光点云数据重新构建环境模型。

进一步地，当采集到新的图像和激光点云数据时，计算当前时刻与环境模型的构建时刻之间的时间差，当时间差小于时间周期阈值时，根据新的图像和激光点云数据更新径向基神经网络，并根据更新后的径向基神经网络更新环境模型。

进一步地，根据新的图像和激光点云数据更新径向基神经网络包括：

根据新的图像和点云数据，确定径向基神经网络中的每个隐含神经元对新的图像和点云数据的贡献度；

当隐含神经元的贡献度小于贡献度阈值时，丢弃掉该隐含神经元。

进一步地，通过以下方式确定每个隐含神经元对新的图像和点云数据的贡献度：

其中，E(r)表示第r个隐含神经元的贡献度，β_r,n为连接第r个隐含神经元和输出之间的输出权重，n为新的图像和点云数据中点云的数量，S(Ω)表示采样范围的大小，b_r,n为第r个隐含神经元的偏置，X＝(x,y)^T，(x,y)为新的图像和点云数据对应的稠密点云深度图中的横、纵坐标值，W_r,n为第r个隐含神经元的输入权重。

进一步地，根据新的图像和激光点云数据更新径向基神经网络包括：

向已有的隐含神经元中加入新的隐含神经元，新的隐含神经元的加入位置为X_n；其中，加入策略为：

其中，X_n为新的图像和点云数据，W_r为第r个隐含神经元的输入权重，ε_n为距离阈值，e_n为X_n的输出误差，X为环境模型构建时的(x,y)，κ为重叠因子，e_min为贡献度阈值。

本发明的另一种技术方案：一种基于多传感器融合的环境建模及路径规划装置，包括：

采集模块，用于同时采集无人车上相机视场内的图像和激光点云数据；

生成模块，用于根据图像和激光点云数据生成稠密点云深度图；

构建模块，用于以稠密点云深度图中坐标值为输入，结合径向基神经网络，构建相机视场内的环境模型；

规划模块，用于以轨迹点的高度和梯度最小为目标、环境特征向量阈值和单步步长为约束条件，确定无人车的路径规划轨迹点序列。

本发明的另一种技术方案：一种基于多传感器融合的环境建模及路径规划装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，其特征在于，处理器执行计算机程序时实现上述的一种基于多传感器融合的环境建模及路径规划方法。

本发明的另一种技术方案：一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现上述的一种基于多传感器融合的环境建模及路径规划方法。

本发明的有益效果是：本发明将将激光点云数据和相机的图像进行融合作为环境模型构建的输入数据，障碍物的位置信息可以作为输入权重应用到环境建模中，可以增加环境模型中野外凹凸障碍物的定位准确性；通过考虑目的地距离、环境高度和环境梯度，对每一个搜索的位置进行评估，得到准确路径规划轨迹点序列，不仅能缩小搜索范围，而且降低了搜索时间。

附图说明

图1为本发明实施例一种基于多传感器融合的环境建模及路径规划方法的流程图；

图2为本发明实施例中稠密点云深度图示意图；

图3为本发明实施例中径向基神经网络的结构示意图；

图4为本发明实施例无人车基于多约束的轨迹生成示意图；

图5为本发明实施例一种基于多传感器融合的环境建模及路径规划装置的结构示意图；

图6为本发明另一实施例一种基于多传感器融合的环境建模及路径规划装置的结构示意图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

在目前的研究中，对于无人车的路径规划多是基于简单或全局已知环境下进行的，在复杂野外环境中的方案相对空白。无人车能够进行自主决策并进行障碍规避离不开一个良好的感知系统。单传感器感知环境只适用于特定环境，局限性大。

因此，本发明在研究无人车自主感知和规避技术的基础上，提出一种多传感器融合的环境建模算法和多约束优化下的路径规划方法，通过研究设计对三维环境的数学建模技术以及基于数学模型的路径规划技术使自主无人车在复杂野外环境中能够进行在线决策和安全行驶。本发明通过多传感器融合准确地感知野外复杂环境并对其建模，再规划无人车的行驶路径，尤其涉及对野外复杂环境下的环境建模和路径规划方法。

如图1所示，本发明实施例公开了一种基于多传感器融合的环境建模及路径规划方法，包括以下步骤：步骤S110、同时采集无人车上相机视场内的图像和激光点云数据，即进行相机和激光雷达的多源信息融合，可以为后期的建模提供准确的输入；步骤S120、根据图像和激光点云数据生成稠密点云深度图；步骤S130、以稠密点云深度图中坐标值为输入，结合径向基神经网络，构建相机视场内的环境模型；步骤S140、以轨迹点的高度和梯度最小为目标、环境特征向量阈值和单步步长为约束条件，确定无人车的路径规划轨迹点序列。

本发明将将激光点云数据和相机的图像进行融合作为环境模型构建的输入数据，障碍物的位置信息可以作为输入权重应用到环境建模中，可以增加环境模型中野外凹凸障碍物的定位准确性；通过考虑目的地距离、环境高度和环境梯度，对每一个搜索的位置进行评估，得到准确路径规划轨迹点序列，不仅能缩小搜索范围，而且降低了搜索时间。

在本发明实施例中，针对复杂野外场景的环境建模和路径规划具体步骤如下。

基于径向基神经网络的多传感器融合环境建模。环境建模一般可分为构建环境的地图模型和数学模型，相比之下，基于数学建模的环境建模速度更快，适合实时的无人车自主导航。基于数学模型的环境建模可以通过神经网络来训练回归模型。在本发明中提出的多传感器融合环境建模的方法利用将传感器信息放入神经网络训练回归数学模型。

无人车首先利用三维激光雷达、单目相机以及微型惯性测量单元进行野外环境感知，再对运动情况下的野外环境序贯建模。激光雷达的点云数据信息加上IMU的姿态角信息可以实现对野外凹凸障碍物的检测与定位，然后障碍物的位置信息可以作为输入权重应用到环境建模中，激光雷达融合图像信息可以实现对稀疏点云的深度恢复以提高点云分辨率，将稠密点云作为环境建模时的输入可以提高环境建模的精度。最后利用RBF-ELM神经网络进行在线的回归训练，可以得到环境的数学模型。在连续帧的序贯处理中，通过加入或丢弃神经元来自动更新模型。

多约束优化下的轨迹生成。本发明研究野外复杂环境的场景下的无人车的轨迹生成。得到环境模型后，不仅要避开高度较高的凸障碍物，还要避开低于地平面的凹障碍物。因此，为了使无人车能安全到达目的地，并且在轨迹生成时让无人车在运动过程中尽可能的行驶在高度低、梯度小的平坦区域，受到启发式方法的思想，构建同时考虑目的地距离、环境高度和环境梯度的代价函数，对每一个搜索的位置进行评估，得到好的位置，再从这个位置进行搜索直到目标。不仅能缩小搜索范围，而且理论上是时间优的。在该算法中，基于野外环境数学模型高度、梯度以及距目标距离等因素构建代价函数，通过优化策略生成参考航迹点。

野外环境下的障碍物种类复杂繁多，但对于无人车的避障来说，不需要知道具体的障碍物是什么。因此，主要根据是否低于地平面将其分为凸障碍物和凹障碍物。凸障碍物包括一些土堆、树木、土块等等，凹障碍物包括一些土坑、壕沟等。准确的检测并定位到这些障碍物对于环境建模至关重要，它可以提供环境建模初始的输入权重。

本实施例主要使用激光雷达、单目相机和IMU来检测凹凸障碍物。激光雷达是野外无人车自主感知与规避系统中的核心传感器，通过计算发送波束和接收波束之间的时间差，从而确定目标的方位和距离。雷达安装在无人车车体上。本实施例选择使用分辨率较高同时尽可能节约成本的速腾聚创RS-32B激光雷达。IMU是无人系统中经常使用的一种传感器。在系统中主要作用是估计车体的姿态角，随时校正激光雷达点云。使无人车在崎岖起伏的地面也能准确检测到障碍物，而不会受到无人车倾斜带来的影响。本实施例选择了高精度的惯性测量单元STIM300。

整个系统中，用激光雷达点云数据来训练环境的数学模型。但激光雷达的点云有一个特点，就是随着距离的增加，点云会越来越稀疏。点云稀疏意味着随着距离的增加丢失了很多信息，这对于环境建模是非常不利的，可能会得到错误的环境模型，导致无人车在行驶过程中撞到障碍物。为了避免建模错误，采用雷达采集的点云数据和相机采集的图像进行融合，对深度进行恢复，得到更加准确的建模输入。

具体的，根据图像和激光点云数据生成稠密点云深度图包括：将激光点云数据投影到图像上，得到稀疏深度图；对稀疏深度图和图像进行联合双边滤波，得到稠密深度图；将稠密深度图转换为稠密点云深度图。

更为具体的，首先，当激光本雷达和相机的相对位置一旦确定，可以通过一些联合标定算法获得两者之间的旋转平移矩阵R_lc(旋转矩阵)和T_lc(平移矩阵)。同时，需要保证点云数据流和图像数据流时间上保持同步，所以需要同时采集激光雷达数据和相机拍摄的图像。由于相机的视场角一般只在正前方一定范围，而激光雷达点云是360°的视场范围，因此，需要滤掉一部分点云，只保留相机视场内的部分。这样做虽然牺牲了一部分视场，但可以在前方视场内有效的融合点云和图像的信息，来准确的感知环境。

经过一系列预处理之后，可以将激光雷达点云投影到图像上，如

所示，其中，每个3D点都有一个像素坐标一一对应，且具有深度值d，这时可以得到一幅稀疏的点云深度图，即上述的稀疏深度图。其中，X、Y、Z分别为点云数据中的三轴坐标，u、v为图像坐标系中的坐标，

为相机的内参矩阵。

图像的总像素远大于3D激光雷达点，为了方便滤波，先将图像中的空白像素点(即没有被点云数据覆盖的像素点)的深度赋为0，对投影得到的深度图和原始图像(相机的)，可以借助联合双边滤波的思想对点云(此处的点云指的是投影之后的)上采样以恢复点云深度(所有投影之后的像素点，都得到一个新的深度值)，滤波公式为：

其中，i、j分别表示在稀疏的点云深度图中和原始的相机图像中对应位置的像素点，I_i和I_j表示对应位置的像素值，d_j表示j处的深度值，

表示i处的深度输出结果，δ表示i的邻域范围，σ_s和σ_c分别是距离项和颜色项的标准差，W_i是i处的归一化参数，

这说明，i处的深度值是由它的邻域深度值加权平均得到的，权值一部分是由邻域坐标距i的距离决定，一部分是由邻域和i处的颜色相似性决定。这样做的前提就是认为离的越近的像素深度越接近，颜色差别越小的像素深度越接近。最终得到了稠密点云深度图如图2所示，该稠密点云深度图将和相机采集的原始图像有着相同的分辨率，大大丰富了点云数据的信息。

稠密的深度图可以借助相机的内参矩阵，转换到相机坐标系下的3D点云，公式为：

其中，x、y、z分别表示在雷达坐标系下点的三轴坐标值。这种方法充分融合了激光雷达点云的深度信息和图像的颜色信息，为环境建模提供了更加准确的输入信息。

在得到融合后的点云作为输入后，本发明使用RBF神经网络来训练回归模型z＝f(x,y)，即通过该回归模型可以构建环境模型。RBF(Radial Basis Function)神经网络是一种局部逼近的神经网络，具有唯一最佳逼近(克服局部极小值问题)、训练简洁、学习收敛速度快等良好性能，可以满足实时性要求。RBF网络能够以任意精度逼近任意连续的非线性网络。

RBF神经网络实质上是一种单隐层前馈神经网络，如图3所示，包括输入层、隐含层和输出层。其中，输入层由N个感知单元组成，表示信源节点输入；隐含层由L个径向基神经元组成，一般是高斯核函数，混合高斯模型可以拟合任意的函数分布，它的作用是从输入空间到隐函层空间之间进行非线性变换；输出层是各个隐含层输出的线性加权组合，为激活函数提供线性响应。这样，网络由输入到输出的映射是非线性的，而网络输出对可调参数而言却又是线性的。这样径向基神经网络不仅可以近似任意的函数，而且网络的权值可以由线性方程组解出，从而大大加快了学习的速度且避免了局部极小值问题。

若此时有N个输入样本(X_i,t_i)，其中X_i＝[x_i1,x_i2,…,x_im]^T，t_i＝[t_i1,t_i2,…,t_in]^T，即X_i是一个m维向量，t_i是一个n维向量，具体到本实施例中，X_i＝(x_i,y_i)，t_i＝z_i。

则具有L个核函数的RBF网络的输出为：

其中，β_i(i＝1,…,L)是连接第i个核函数与输出神经元之间的输出权重，φ_i(X)是第i个隐含神经元的激活函数，取高斯核函数，φ_i(X)＝exp(b_i||X-W_i||²)，W_i＝[w_i1,w_i2,…,w_im]^T是第i个隐含神经元的输入权重，b_i＝[b_i1,b_i2,…,b_im]^T是第i个隐含神经元的偏置。

该神经网络学习的目的就是为了最小化输出误差，如

所示，也就是要满足公式

也可以通过矩阵形式表示成公式Hβ＝T，其中，H是隐含层单元的输出，β是输出权重，T是期望输出，定义分别如下式所示：

训练的目的就是找到输入权重

偏置

和输出权重

使得满足式：

即等同于最小化损失函数E,其定义如下式：

在本实施例中，径向基神经网络的训练前先确定隐含层的个数L，一般来说隐含层个数L越多，函数的近似就越精确，但同时考虑运算速度应选取一个合适的值。通过障碍物检测与定位，可以确定输入权重

即障碍物的位置就是高斯核函数的中心，剩下的高斯核的中心随机确定。隐含神经元的偏置

即高斯核函数的方差可以通过K最近邻算法(KNN)得到。当

和

都确定时，那么，训练这个RBF神经网络被转换为求解线性系统的优化问题，如公式：

此时，输出权重β可以被唯一确定，而且是最优解，即

其中，H⁺是H矩阵的广义逆矩阵，可以通过正交投影或者SVD的方法来求得广义逆矩阵。

在本实施例中，输入样本中X_i＝(x_i,y_i)，t_i＝z_i，最终可以得到z关于x、y的函数，即

根据该公式结合输入的稠密点云深度图中各坐标值，可以生成环境模型。

在本发明实施例中，上述内容仅仅是在一个时刻时，计算环境模型的过程，但是在车辆运动过程中，环境在实时的变化，神经网络的输入也在实时的更新，若每一帧都重新检测障碍物并构建环境模型，对于运算的消耗会比较大，难以满足实时性的要求。因此，本实施例提出一种连续帧的序贯建模方法，可以在时序的输入下实现对模型的更新，而不用重新建模，节省计算成本。在本实施例中，通过计算神经元的“显著性”来分析隐含层神经元的重要性，再确定新加入的输入信息是否需要新增加神经元，或者丢弃掉不需要的神经元，最后更新输出权值。即当时间差小于时间周期阈值时，根据新的图像和激光点云数据更新径向基神经网络，并根据更新后的径向基神经网络更新环境模型。

更新环境模型的方法有很多，本实施例中根据新的图像和激光点云数据更新径向基神经网络包括：根据新的图像和点云数据，确定径向基神经网络中每个隐含神经元对新的图像和点云数据的贡献度；当隐含神经元的贡献度小于贡献度阈值时，丢弃掉该隐含神经元。通过以下方式确定每个隐含神经元对新的图像和点云数据的贡献度：

其中，E(r)表示第r个隐含神经元的贡献度，β_r,n为连接第r个隐含神经元和输出之间的输出权重，n为新的图像和点云数据中点云，S(Ω)表示采样范围的大小，b_r,n为第r个隐含神经元的偏置，X＝(x,y)^T，(x,y)为新的图像和点云数据对应的稠密点云深度图中点的横、纵坐标值，W_r,n为第r个隐含神经元的输入权重，dX是积分时候的微元算子。

另外，还包括增加隐含神经元的情况，根据新的图像和激光点云数据更新径向基神经网络还包括向已有的隐含神经元中加入新的隐含神经元，新的隐含神经元的加入位置为X_n；其中，加入策略为：

其中，X_n为新的图像和点云数据，W_r为第r个隐含神经元的输入权重，ε_n为距离阈值，e_n为X_n的输出误差，X为环境模型构建时刻的(x,y)，κ为重叠因子，e_min为贡献度阈值。

假设现在的RBF神经网络有L个隐含神经元，输入为

那么经过神经网络输出为

其中，β_k表示连接第k个隐含神经元和输出之间的输出权重,φ_k(X)表示第k个隐含神经元对于输入X的函数响应，φ_k(X)＝exp(b_k||X-W_k||²)，W_k表示输入权重，代表着神经元的位置，b_k表示神经元的偏置，代表着神经元的宽度。

经过环境建模得到的神经网络，对于输入X_i，对应的网络输入为：

如果此时第k个隐含神经元被移除，对应的神经网络的输入为：

因此对于输入X_i，因为移除了第k个隐含神经元导致的输出误差如下式所示：

那么对于有n个点的点云

来说，对于移除第k个隐含神经元导致总的输出误差为：

E(k)＝||(E(k,1),E(k,2),...,E(k,n))^T||，

即如下式：

直接求取上式，不仅要用到所有的n个输入信息，而且通常在训练环境模型时不会记录以往的数据，实时运算非常难以实现。所以，利用输入的点云是服从均匀分布这一特点，在采样范围Ω内，该分布的分布概率密度函数如

所示。其中，Ω表示采样范围，S(Ω)表示采样范围的大小。将采样范围Ω划分为M个小范围Δ_j，j＝1,...,M，Δ_j的大小表示为S(Δ_j)。那么，在每一个Δ_j里大约有n·p(X_j)·S(Δ_j)个点，X_j表示在Δ_j选择的任意一个输入点。

假设认为在每一个小范围Δ_j内所有输入在第k个隐含神经元的函数响应都是相同的，那么E(k)公式可以写成下式：

当采样范围无限细分时，即M趋于无穷时，可以得到：

E(k)表示第k个隐含神经元对整体输出的贡献大小。此时，将E(k)定义为第k个神经元的“显著性”。通过“显著性”可以判断第k个神经元对最新输入的重要性，有了“显著性”的定义，就可以引入神经元的“加入”和“丢弃”策略。

对于已经训练好的神经网络，有L个隐含神经元，当有新的输入X_n加入时，定义“加入”策略如下式所示：

其中，W_r表示离X_n最近的隐含神经元的输入权重，即该神经元的位置。ε_n是距离阈值，e_min是判断显著性的阈值，e_n是输入X_n的输出误差。这些参数的定义如下式：

对于满足条件的，可以加入第L+1个神经元。

的第一个准则是为了保证只有输入距现有神经元足够远才会新加入神经元；第二个准则是为了确保新加入的神经元的“显著性”大于需要的近似精度e_min，该神经元的参数定义如下：

其中，κ是重叠因子，用来决定神经元的相应宽度。由上式可知，对于满足“显著性”条件而新加入的神经元，其位置就选在X_n＝(x_n,y_n)处，其宽度等于新加入神经元距最近神经元的距离乘以重叠因子，其输出权重等于X_n的输出误差。

更新完权值参数后，需要判断离的最近的第r个神经元是否满足“显著性条件”，如果小于e_min，应该丢弃掉，节省存储空间和运算成本。具体公式如下式所示：

在判断完“加入”策略和“丢弃”策略之后，假设现在隐含层神经元个数变成L'个，而且每个隐含层的输入权重和偏置已经确定，这一帧的输入点云的总个数为N'，那么新的隐含层响应矩阵H'可以确定，真实样本的期望输出T'也可以确定，如下式所示：

那么根据

可以得到更新后的输出权重β'如β'＝(H')⁺T'所示。

对于运动中的车辆，通常随着时间的推移，场景变化会比较大，通过上面参数更新的方法可能会随着时间推移误差越来越大。为了保证环境建模的准确性和可靠性，在经过周期为Q的时间后，需要通过障碍物检测与定位重新确定隐含神经元的输入权重和偏置，然后重复上述的操作。

即随着时间而变化的，根据每个时刻的信息去更新或构建新的环境模型。当采集到新的图像和激光点云数据时，计算当前时刻与环境模型的构建时刻之间的时间差，当时间差大于等于时间周期阈值时，根据新的图像和激光点云数据重新构建环境模型，即重复执行上述步骤。

城市环境的无人车在考虑避障因素时，往往只需考虑高度信息，比如高于车轮半径的物体视为障碍物。因为城市环境地面平坦，基本没有坡度，障碍物类型也比较单一。但野外环境地面起伏，不仅有高于地面的凸障碍物，还有低于地面的凹障碍物。在某些情况下，虽然高度高，但梯度比较平缓，对无人车是可以通行的；在其他一些情况，虽然高度低，但梯度值很大，比如突然出现的凹坑，这对无人车的安全行驶的致命的。因此，在考虑野外环境的避障因素时应该综合考虑这些因素。而前面的环境建模模块得到的环境模型可以得到环境的高度信息，对模型求导可以得到环境的梯度信息，因此，在这一节基于环境的数学模型，综合考虑无人车任务目标和机动代价，构建了多约束下的代价函数。在无人车的任务中，在已知目的地坐标后，无人车应当根据环境的高度和梯度信息，找一条尽可能离目标点近，且参考轨迹点的高度和梯度综合值比较小的路径。为此，假设已经得到环境的数学模型f(X)，那么可以构建轨迹生成的代价函数：

其中，X_i＝(x_i,y_i)表示优化的参考轨迹点坐标，X_i-1是上一个优化得到的轨迹点坐标，K是实时生成的轨迹点的总个数，P^*是目的地的位置坐标，α₁和α₂是两个参数系数，表示目的地距离和环境信息的重要性权重，ξ表示单步步长，g(X)是在X点处综合考虑高度和梯度信息的环境特征向量，如

所示，ε表示可以安全行驶的环境特征向量阈值。

最终优化的目的是为了获得参考轨迹点序列

其中，

野外无人车基于多约束的轨迹生成示意图如图4所示。环境感知模块的得到的环境模型是高低起伏的。最终规划的路径总是尽可能的靠近目的地。同时，尽可能生成轨迹点在平坦的区域，避开高度和梯度高的区域，通过设定合适大小的阈值ε，可以保证无人车有一定的安全包络，而且行驶在平坦区域对无人车有着更小的机动代价，更加保证行驶的安全性。

参考轨迹生成问题可以视为一个带不等式约束的非线性优化问题：

其中，h₁(X)＝||X_i-X_i-1||-ξ，h₂(X)＝||g(X)||-ε。

对于带不等式约束的非线性优化问题，通过解析法直接求出最优解的方式是很少见的，通常是通过迭代的方法逐步接近最优解。而在非线性优化的迭代解法中，序列二次规划法是非常常用的方法，在各个领域取得了广泛应用。其基本思想是把原非线性优化问题转为一系列二次规划子问题求解，通过迭代获得最优解。对于一个带不等约束的优化问题，可以通过构建拉格朗日函数，使其满足KKT条件后进行泰勒展开近似，那么第k次迭代的优化问题可以转化为求解一个二次规划问题，那么第k次迭代X_k的下降搜索方向d_k可以由下述公式得到：

通过给定初始迭代步长δ_k，那么下一个迭代点X_k+1可以通过d_k求出，X_k+1＝X_k+δ_kd_k。

表明，代价函数每一个单步步长ξ优化得到每一个参考轨迹点

通过K步的优化，可以得到参考轨迹序列

参考轨迹序列可以发送给控制模块，控制无人车跟踪这条实时规划出来的安全路径。

本发明该公开了另一种技术方案：一种基于多传感器融合的环境建模及路径规划装置，如图5所示，包括：采集模块210，用于同时采集无人车上相机视场内的图像和激光点云数据；生成模块220，用于根据图像和激光点云数据生成稠密点云深度图；构建模块230，用于以稠密点云深度图中的坐标值为输入，结合径向基神经网络，构建相机视场内的环境模型；确定模块240，用于以轨迹点的高度和梯度最小为目标、环境特征向量阈值和单步步长为约束条件，确定无人车的路径规划轨迹点序列。

需要说明的是，上述模块之间的信息交互、执行过程等内容，由于与本发明方法实施例基于同一构思，其具体功能及带来的技术效果，具体可参见方法实施例部分，此处不再赘述。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将所述装置的内部结构划分成不同的功模块，以完成以上描述的全部或者部分功能。实施例中的各功能模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。上述系统中模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

本发明另一实施例还公开了一种基于多传感器融合的环境建模及路径规划装置，如图6所示，包括存储器31、处理器32以及存储在存储器31中并可在处理器32上运行的计算机程序33，处理器32执行计算机程序时实现上述的一种基于多传感器融合的环境建模及路径规划方法。

本发明另一实施例还公开了一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现上述的一种基于多传感器融合的环境建模及路径规划方法。

所述计算机可读介质至少可以包括：能够将计算机程序代码携带到拍照装置/终端设备的任何实体或装置、记录介质、计算机存储器、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质。例如U盘、移动硬盘、磁碟或者光盘等。在某些司法管辖区，根据立法和专利实践，计算机可读介质不可以是电载波信号和电信信号。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

Claims (8)

1.一种基于多传感器融合的环境建模及路径规划方法，其特征在于，包括以下步骤：

同时采集无人车上相机视场内的图像和激光点云数据；

根据所述图像和激光点云数据生成稠密点云深度图；

以所述稠密点云深度图中坐标值为输入，结合径向基神经网络，构建所述相机视场内的环境模型；

以轨迹点的高度和梯度最小为目标、环境特征向量阈值和单步步长为约束条件，确定所述无人车的路径规划轨迹点序列；

当采集到新的图像和激光点云数据时，计算当前时刻与环境模型的构建时刻之间的时间差，当所述时间差大于等于时间周期阈值时，根据新的图像和激光点云数据重新构建环境模型；

当采集到新的图像和激光点云数据时，计算当前时刻与环境模型的构建时刻之间的时间差，当所述时间差小于所述时间周期阈值时，根据新的图像和激光点云数据更新所述径向基神经网络，并根据更新后的径向基神经网络更新环境模型。

2.如权利要求1所述的一种基于多传感器融合的环境建模及路径规划方法，其特征在于，根据所述图像和激光点云数据生成稠密点云深度图包括：

将所述激光点云数据投影到所述图像上，得到稀疏深度图；

对所述稀疏深度图和所述图像进行联合双边滤波，得到稠密深度图；

将所述稠密深度图转换为稠密点云深度图。

3.如权利要求1所述的一种基于多传感器融合的环境建模及路径规划方法，其特征在于，根据新的图像和激光点云数据更新所述径向基神经网络包括：

根据新的图像和点云数据，确定所述径向基神经网络中每个隐含神经元对新的图像和点云数据的贡献度；

当所述隐含神经元的贡献度小于贡献度阈值时，丢弃掉该隐含神经元。

4.如权利要求3所述的一种基于多传感器融合的环境建模及路径规划方法，其特征在于，通过以下方式确定每个隐含神经元的对新的图像和点云数据的贡献度：

其中，E(r)表示第r个隐含神经元的贡献度，β_r,n为连接第r个隐含神经元和输出之间的输出权重，n为新的图像和点云数据中点云的数量，S(Ω)表示采样范围的大小，b_r,n为第r个隐含神经元的偏置，X＝(x,y)^T，(x,y)为新的图像和点云数据对应的稠密点云深度图中的横、纵坐标值，W_r,n为第r个隐含神经元的输入权重。

5.如权利要求1所述的一种基于多传感器融合的环境建模及路径规划方法，其特征在于，根据新的图像和激光点云数据更新所述径向基神经网络包括：

向已有的隐含神经元中加入新的隐含神经元，新的隐含神经元的加入位置为X_n；其中，加入策略为：

其中，X_n为新的图像和点云数据，W_r为第r个隐含神经元的输入权重，ε_n为距离阈值，e_n为X_n的输出误差，X为所述环境模型构建时的(x,y)，κ为重叠因子，e_min为贡献度阈值。

6.一种基于多传感器融合的环境建模及路径规划装置，其特征在于，用于实现权利要求1-5任一所述的一种基于多传感器融合的环境建模及路径规划方法，包括：

采集模块，用于同时采集无人车上相机视场内的图像和激光点云数据；

生成模块，用于根据所述图像和激光点云数据生成稠密点云深度图；

构建模块，用于以所述稠密点云深度图中坐标值为输入，结合径向基神经网络，构建所述相机视场内的环境模型；

规划模块，用于以轨迹点的高度和梯度最小为目标、环境特征向量阈值和单步步长为约束条件，确定所述无人车的路径规划轨迹点序列；

当采集到新的图像和激光点云数据时，计算当前时刻与环境模型的构建时刻之间的时间差，当所述时间差大于等于时间周期阈值时，根据新的图像和激光点云数据重新构建环境模型；

当采集到新的图像和激光点云数据时，计算当前时刻与环境模型的构建时刻之间的时间差，当所述时间差小于所述时间周期阈值时，根据新的图像和激光点云数据更新所述径向基神经网络，并根据更新后的径向基神经网络更新环境模型。

7.一种基于多传感器融合的环境建模及路径规划装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述的一种基于多传感器融合的环境建模及路径规划方法。

8.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一种基于多传感器融合的环境建模及路径规划方法。

Images