一种非结构化道路状态参数估计方法及系统

1.本发明涉及智能车辆环境感知技术领域，特别是关于一种非结构化道路状态参数估计方法及系统。


背景技术：

2.汽车智能化是汽车行业发展的重要趋势，环境感知技术是智能车辆技术的核心技术之一。环境感知技术通过获取和分析传感器数据，为决策、控制等技术提供环境信息。然而在矿区、越野场景这类非结构化道路上行驶时，智能车辆需要面对路面类型多变、坡度起伏明显等问题，这无疑对智能车辆的运动控制、安全行驶带来极大的挑战。因此提前预知前方道路地面状态参数对保障智能车辆正常安全行驶具有重要意义。利用车载传感器实时估计车辆前方的道路状态，车辆便能够根据相关状态参数及时调整行驶姿态，实现最优的路径规划和运动控制。
3.目前的各种车载传感器的地面状态的估计方法主要存在以下几点问题：
4.(1)基于深度相机的方法对光照条件要求严格，且受相机深度估计值影响较大，在复杂环境条件下鲁棒性差；
5.(2)基于三维点云的方法仅仅依赖空间几何信息，无法做到精确的地面分割，且对于远处点云稀疏区域，参数估计误差较大；
6.(3)目前存在的道路状态参数估计方法中所使用的平面分块策略均未考虑实际道路情况，鲁棒性差，大大降低了后续参数估计的准确度；
7.(4)基于车辆行驶参数的方法属于接触式估计方法，只有当车辆行驶到当前路面时才能对其地面状态进行精准的估计，无法提前预测车辆前方行驶区域的地面状态参数，达不到提前规划的目的。


技术实现要素：

8.本发明的目的在于提供一种非结构化道路状态参数估计方法及系统来克服或至少减轻现有技术的上述缺陷中的至少一个。
9.为实现上述目的，本发明提供一种非结构化道路状态参数估计方法，其包括：
10.步骤1，将采集到的点云数据与图像数据进行时间同步和空间同步，并获得激光雷达坐标系oxyz中的点云数据中的某个点云l(x
l
，y
l
，z
l
)到像素坐标系ouv中图像数据的某个像素c(uc，vc)的映射关系；
11.步骤2，多传感器融合地面分割，输出点云地面分割结果和图像地面分割结果
12.步骤3，地面点云数据增强，其具体包括：
13.步骤31，依据步骤1获得的映射关系将步骤2获得的点云地面分割结果中
的每个点云投影至图像地面分割结果的像素上，并搜索出不存在投影点云的像素，其在图像ouv坐标系中的坐标表示为(u
x
，v
x
)；
14.步骤32，根据步骤31搜索获得的(u
x
，v
x
)及其邻域信息，对所述像素(u
x
，v
x
)的深度补全；
15.步骤33，将(u
x
，v
x
，d
x
)通过步骤1获得的映射关系(1)反投影至激光雷达坐标系oxyz中，生成新的地面点云，实现对地面点云数据的增强。
16.步骤4，道路自适应分块拟合，实现路面建模；
17.步骤5，道路参数估计：基于路面平面模型，通过计算相邻两平面的法向量夹角即可求得相邻平面的相对纵向/横向坡度；通过计算地面点云到对应拟合平面之间的距离，并采用距离的均方根作为路面粗糙度的评价指标；通过拟合每个分块平面的道路边界，进行道路曲率估计。
18.进一步地，所述步骤32采用式(6)、式(7)和式(8)描述的双向线性插值的方法对所述像素(u
x
，v
x
)的深度补全：
[0019][0020][0021][0022]
式中，(u
t
，v
x
)表示沿着图像坐标系u轴负方向的距离像素(u
x
，v
x
)的最近的存在投影点云的像素，是该像素的深度值；(u
t+1
，v
x
)表示沿着图像坐标系u轴正方向的距离像素(u
x
，v
x
)的最近的存在投影点云的像素，是该像素的深度值；(u
x
，v
t
)表示沿着图像坐标系v轴负方向的距离像素(u
x
，v
x
)的最近的存在投影点云的像素，是该像素的深度值；(u
x
，v
t+1
)表示沿着图像坐标系轴负方向的距离像素(u
x
，v
x
)的最近的存在投影点云的像素，是该像素的深度值；表示u轴方向上的像素深度线性插值，表示v轴方向上的像素深度线性插值，d
x
为最后加权计算得到的像素(u
x
，v
x
)的深度值，ku和kv分别为横向和纵向深度估计值的权重。
[0023]
进一步地，所述步骤33中的ku和kv依据式(9)获得：
[0024][0025]
进一步地，所述步骤4具体包括：
[0026]
步骤41，将步骤3获取的增强后的地面点云投影至道路侧向，并对投影至道路侧向的地面点云数据进行分块。
[0027]
步骤42，对分块后的地面点云分别进行平面拟合，获得依据道路实际坡度自适应分段拟合出来的多个道路平面。
[0028]
进一步地，所述步骤41具体包括：
[0029]
步骤411，将步骤2获得的地面点云投影至激光雷达坐标系的xoz平面；
[0030]
步骤412，构建最佳分段数优化目标函数(10)：
[0031]
f(n)＝f(b)+k*n
ꢀꢀ
(10)
[0032]
式中，f(b)表示最佳分段位置优化目标函数，n表示分段数，k表示过拟合抑制参数；
[0033]
步骤413，初始采样，通过贝叶斯优化框架(11)，针对该非凸、非连续的目标函数f(n)进行全局优化，找到最佳分段数n：
[0034][0035]
式中，d表示初始采样集合，p( )表示概率分布函数；
[0036]
步骤414，采用残差平方和来定义最佳分段位置优化目标函数(12)：
[0037]
f(b)＝ssr(b)
ꢀꢀ
(12)
[0038]
式中，b＝[b0，b1,b2，...，bn]表示最佳分段位置，ssr( )表示残差平方和函数；
[0039]
步骤415，将上述最佳分段数优化目标函数(10)和最佳分段位置优化目标函数(12)通过差分进化算法进行优化，其优化步骤如下：
[0040]
a.初始化采样，定义b0为中在激光雷达坐标系的x轴上的最小值，bn为最大值；若分段数为n，则需要优化的分段点数目为n-1，采用式(13)提供的均匀采样的方式在解空间采样m组样本sm，一般设置m＝(5～10)n，获得原始样本：
[0041][0042]
式中，m∈{1，2，...，m},表示样本的索引值，n∈{1，2，...，n-1},表示每个样本的维度索引值，rand( )表示随机采样函数，和分别表示解空间中第n维的上下边界；
[0043]
b.用式(14)对所述步骤a获得的样本进行变异操作，获得变异样本：
[0044][0045]
式中，表示变异样本，分别代表每次迭代过程中随机选取的原始样本，g表示当前迭代次数，f表示缩放因子；
[0046]
c.利用式(15)将部分所述步骤a获取的所述原始样本与所述步骤b获取的所述变异样本的对应分量按照二项式交叉生成试验样本
[0047][0048]
其中，cr为交叉算子，rand( )表示随机采样函数；
[0049]
d.根据目标函数来计算原始样本和试验样本的适应度，按照式(16)选取结果较好的样本进行保留，进入下一次迭代：
[0050][0051]
式中，f( )表示最佳分段位置优化目标函数；
[0052]
最后，当迭代次数达到设定阈值时，选取最佳适应度的样本作为输出，获得最佳分段位置b
best
。
[0053]
步骤416，根据获取的最佳分段位置b
best
，将步骤3输出的增强后的地面点云进行分块操作。
[0054]
进一步地，所述步骤42采用随机采样一致性算法对步骤41提供的分块点云进行拟合，获得每块点云的最佳平面方程：
[0055]ai
x+biy+ciz+di＝0
ꢀꢀ
(17)
[0056]
式中，i∈[1，2，...，n]，ai、bi、ci、di为随机采样一致性算法计算得到的拟合平面的四个参数。
[0057]
进一步地，所述步骤2具体包括：
[0058]
步骤21，将步骤1输出的图像数据输入经训练好的实时语义分割网络中，输出图像地面分割结果中每个像素的概率值为p(c)；
[0059]
步骤22，步骤1输出的点云数据输入经过训练好的点云语义分割网络中输出点云地面分割结果中每个点的概率值为p(l)；
[0060]
步骤23，将获取的和采用概率图模型进行建模；
[0061]
步骤24，根据步骤23的建立的概率图模型，重新估计概率图模型中点云节点和像素节点的属于地面的概率值和其计算公式如下：
[0062][0063][0064]
步骤25，根据步骤24估计的每个节点(包含点云节点和像素节点)的概率值按照式(7)获得的qi，判断每个节点i是否属于地面：
[0065][0066]
式中，阈值t需要根据真实数据进行调整，对于不同的非结构化场景给予不同的大小，1代表属于地面，0代表不属于地面；
[0067]
步骤26，根据qi的值更新点云地面分割结果和图像地面分割结果
[0068]
进一步地，所述步骤23具体包括：
[0069]
步骤231，将所述点云地面分割结果中的第i个点云节点描述为li，其点云节点值被描述为p(li)；将所述图像地面分割结果中的第j个像素节点描述为cj，其像素节点值被描述为p(cj)；
[0070]
步骤232，先使用k近邻方法，寻找与所述点云地面分割结果中的点云节点li最近邻的k个点云节点lk，其地面点云概率值用p(lk)描述，再将点云节点li与点云节点
lk建立节点连接，最后计算点云节点li与点云节点lk与之间的距离dis(li，lk)，将dis(li，lk)的倒数作为点云节点li与点云节点lk的连接边的权重参数；
[0071]
步骤233，先采用八领域搜索，确定点云地面分割结果中的像素节点cj附近的8个相邻的像素节点cm，其地面像素概率值用p(cj)描述，然后，将连接像素节点ci与像素节点cj建立节点连接，再设置像素节点ci与像素节点cj之间的距离dis(cj，cm)，将dis(cj，cm)的倒数作为像素节点ci与像素节点cj的连接边的权重参数：
[0072]
步骤234，利用步骤1中给出的空间映射关系将每个点云节点和像素节点建立一一对应的节点连接，并将这两种节点的连接边的权重参数设置为第三预设值。
[0073]
进一步地，所述步骤233按如下情形的差异设置像素节点ci与像素节点cj之间的距离dis(cj，cm)：
[0074]
情形一：当像素节点cj与像素节点cm属于直线相邻时，将dis(cj，cm)设置为第一预设值；
[0075]
情形二：当像素节点cj与像素节点cm属于对角相邻时，将dis(cj，cm)设置为第二预设值。
[0076]
本发明还提供一种采用如上所述非结构化道路状态参数估计方法的系统。
[0077]
本发明由于采取以上技术方案，其具有以下优点：
[0078]
1)本方法采用车载激光雷达和摄像头，融合图像色彩纹理信息和三维点云几何信息进行非结构化道路分割，提升了地面感兴趣区域提取精度；2)本方法采用双向线性插值的方法，对三维点云进行数据增强，减小了因远处点云稀疏带来的模型估计误差；3)本方法采用基于贝叶斯优化的最小二乘分段线性拟合方法，可在无先验信息的情况下实现了地面点云的精确自适应分段；4)基于多传感器数据融合与自适应道路分块拟合方法，实现更加精准的非结构化道路状态参数估计。
附图说明
[0079]
图1为本发明实施例提供的非结构化道路状态参数估计系统结构示意图。
[0080]
图2为图1中道路参数估计单元的道路状态参数估计方法流程图。
[0081]
图3为本发明实施例提供的多传感器空间同步示意图。
[0082]
图4为本发明实施例提供的概率图融合建模示意图。
[0083]
图5为本发明实施例提供的双向线性插值方法示意图。
[0084]
图6为本发明实施例提供的道路自适应分段线性拟合示意图。
具体实施方式
[0085]
下面结合附图和实施例对本发明进行详细的描述。
[0086]
如图1所示，本实施例所提供的非结构化道路状态参数估计方法包括：
[0087]
步骤1，将采集到的点云数据与图像数据进行时间同步和空间同步。其中：点云数据由激光雷达采集得到，图像数据由相机采集得到。
[0088]
将采集到的点云数据与图像数据进行时间同步可以采用如下方法实现：
[0089]
通过车载gps自带的秒脉冲发生器发送pps脉冲信号，同时触发激光雷达和相机进
行测量，进而能够实现点云数据与图像数据的时间同步。
[0090]
将采集到的点云数据与图像数据进行空间可以采用如下一种方法实现，下面结合图3来说明，图中的lidar表示激光雷达，其采集到的点云数据也可以称作为三维点云，例如：点云数据中某个点l在激光雷达坐标系oxyz中的坐标描述为(x
l
，y
l
，z
l
)。图中的camera表示相机，其采集到的图像数据即为文中所称的图像，其图像中的某个像素c通常表示为像素坐标系ouv中的(uc，vc)：
[0091]
通过传感器标定获取相机的内参矩阵kc和外参矩阵[rx tc]及激光雷达的外参矩阵[r
l t
l
]，则可以得到该点云l到像素c的映射关系(1)，再利用映射关系(1)，实现点云数据与图像数据的空间同步：
[0092][0093]
式中，d
x
为像素c对应的深度值。
[0094]
步骤2，多传感器融合地面分割，结合图2和图4，其具体包括：
[0095]
步骤21，将步骤1输出的图像数据输入经训练好的实时语义分割网络中，输出图像地面分割结果中每个像素的概率值为p(c)。
[0096]
步骤22，将步骤1输出的点云数据输入经过训练好的点云语义分割网络中输出点云地面分割结果中每个点的概率值为p(l)。
[0097]
步骤23，将获取的和采用概率图模型进行建模。
[0098]
在一个实施例中，步骤23可以采用如下子步骤实现：
[0099]
步骤231，将中的每个点视为一个节点，这些节点在文中将被称为点云节点，而且点云节点值取该像素的概率值。例如：中的第i个点云节点可以被描述为li，其点云节点值被描述为p(li)。
[0100]
同样将中的每个像素视为一个节点，这些节点在文中将被称为像素节点，而且像素节点值取该像素的概率值。例如：中的第j个像素节点可以被描述为cj，其像素节点值被描述为p(cj)。
[0101]
显然一个节点的只对应一个节点概率值。
[0102]
步骤232，对于中的每个点云节点，以点云节点li为例，首先，使用k近邻方法，寻找与点云节点li最近邻的k个点云节点，这k个点云节点被描述为点云节点lk，其地面点云概率值用p(lk)描述。再将点云节点li与点云节点lk建立节点连接。最后计算点云节点li与点云节点lk与之间的距离dis(li，lk)，将dis(li，lk)的倒数作为点云节点li与点云节点lk的连接边的权重参数。
[0103]
步骤233，对于中的每个像素节点，以像素节点cj为例，首先，采用八领域搜索，确定像素节点cj附近的8个相邻的像素节点，这8个像素节点被描述为像素节点cm，其
地面像素概率值用p(cj)描述。然后，将连接像素节点ci与像素节点cj建立节点连接。再可以按如下情形的差异设置像素节点ci与像素节点cj之间的距离dis(cj，cm)，将dis(cj，cm)的倒数作为像素节点ci与像素节点cj的连接边的权重参数：
[0104]
情形一：当像素节点cj与像素节点cm属于直线相邻时，将dis(cj，cm)设置为第一预设值，例如：dis(cj，cm)＝1；
[0105]
情形二：当像素节点cj与像素节点cm属于对角相邻时，将dis(cj，cm)设置为第二预设值，例如：
[0106]
当然，还可以采用现有的其他方法设置像素节点ci与像素节点cj之间的距离dis(cj，cm)。
[0107]
步骤234，利用步骤1中给出的空间映射关系(1)将每个点云节点和像素节点建立一一对应的节点连接，并将这两种节点的连接边的权重参数设置为第三预设值，例如dis(li，cj)＝1。
[0108]
步骤24，根据步骤23的建立的概率图模型，重新估计概率图模型中点云节点和像素节点的属于地面的概率值和其计算公式如下：
[0109][0110][0111]
步骤25，根据步骤24估计的每个节点(包含点云节点和像素节点)的概率值按照式(7)获得的qi，判断每个节点i是否属于地面：
[0112][0113]
式中，阈值t需要根据真实数据进行调整，对于不同的非结构化场景给予不同的大小，1代表属于地面，0代表不属于地面。
[0114]
步骤26，根据qi的值更新点云地面分割结果和图像地面分割结果
[0115]
需要说明的是，步骤2还可以采用隐马尔科夫随机场、条件随机场和深度学习语义分割的方法，来进行多传感器融合地面分割。
[0116]
步骤3，将步骤2中获得的地面点云进行数据增强，以便于后续的地面状态参数计算。
[0117]
在一个实施例中，步骤3可以采用如下子步骤实现步骤3：
[0118]
步骤31，依据步骤1获得的映射关系(1)将步骤2获得的点云地面分割结果中的每个点云投影至图像地面分割结果的像素上，并搜索出不存在投影点云的像素，其在图像ouv坐标系中的坐标表示为(u
x
，v
x
)。
[0119]
步骤32，如图5所示，根据步骤31搜索获得的(u
x
，v
x
)及其邻域信息，采用式(6)、式
(7)和式(8)描述的双向线性插值方法进行像素(u
x
，v
x
)的深度补全：
[0120][0121][0122][0123]
式中，(u
t
，v
x
)表示沿着图像坐标系u轴负方向的距离像素(u
x
，v
x
)的最近的存在投影点云的像素，是该像素的深度值；(u
t+1
，v
x
)表示沿着图像坐标系u轴正方向的距离像素(u
x
，v
x
)的最近的存在投影点云的像素，是该像素的深度值；(u
x
，v
t
)表示沿着图像坐标系v轴负方向的距离像素(u
x
，v
x
)的最近的存在投影点云的像素，是该像素的深度值；(u
x
，v
t+1
)表示沿着图像坐标系轴负方向的距离像素(u
x
，v
x
)的最近的存在投影点云的像素，是该像素的深度值；表示u轴方向上的像素深度线性插值，表示v轴方向上的像素深度线性插值，d
x
为最后加权计算得到的像素(u
x
，v
x
)的深度值，ku和kv分别为横向和纵向深度估计值的权重：
[0124][0125]
需要说明的是，步骤32还可以采用基于德劳雷三角形的填充方法、扩展滤波的方法，对缺少投影点云的像素(u
x
，v
x
)进行深度补全。
[0126]
步骤33，将(u
x
，v
x
，d
x
)通过步骤1获得的映射关系(1)反投影至激光雷达坐标系oxyz中，生成新的地面点云，实现对地面点云数据的增强。
[0127]
步骤4，道路自适应分块拟合，其具体包括：
[0128]
步骤41，将步骤3获取的增强后的地面点云投影至道路侧向，并对投影至道路侧向的地面点云数据进行分块。
[0129]
步骤42，对分块后的地面点云分别进行平面拟合，获得依据道路实际坡度自适应分段拟合出来的多个道路平面。
[0130]
在一个实施例中，在非结构化道路场景，路面可能存在坡度起伏的情况，对不同坡度的路面应分别进行拟合计算，因此需要更加精确的路面划分方法来保证路面坡度估计的精确度，那么，在缺乏先验信息的情况下，本发明实施例采用基于贝叶斯优化的最小二乘分段线性拟合方法，对投影至道路侧向的地面点云数据进行精确自适应分块。如图6所示，具体包括：
[0131]
步骤411，为了减小噪声点对于模型拟合的影响，将步骤2获得的地面点云投影至激光雷达坐标系的xoz平面。
[0132]
步骤412，构建最佳分段数优化目标函数(10)：
[0133]
f(n)＝f(b)+k*n
ꢀꢀ
(10)
[0134]
式中，f(b)表示最佳分段位置优化目标函数，n表示分段数，k表示过拟合抑制参数，用于平衡残差平方和与分段数的权重。
[0135]
步骤413，进行一组初始采样，通过贝叶斯优化框架(11)，针对该非凸、非连续的目标函数f(n)进行全局优化，找到最佳分段数n：
[0136][0137]
式中，d表示初始采样集合，p( )表示概率分布函数。
[0138]
步骤414，采用残差平方和来定义最佳分段位置优化目标函数(12)：
[0139]
f(b)＝ssr(b)
ꢀꢀ
(12)
[0140]
式中，b＝[b0，b1，b2，...，bn]表示分段点位置，即最佳分段位置，ssr( )表示残差平方和函数。
[0141]
将上述最佳分段数优化目标函数(10)和最佳分段位置优化目标函数(12)通过差分进化算法进行优化，其优化步骤如下：
[0142]
a.初始化采样，定义b0为中在激光雷达坐标系的x轴上的最小值，bn为最大值；若分段数为n，则需要优化的分段点数目为n-1，采用式(13)提供的均匀采样的方式在解空间采样m组样本sm，获得原始样本：
[0143][0144]
式中，m∈{1，2，...，m},表示样本的索引值，n∈{1，2，...，n-1},表示每个样本的维度索引值，rand( )表示随机采样函数，和分别表示解空间中第n维的上下边界；
[0145]
b.用式(14)对所述步骤a获得的样本进行变异操作，获得变异样本：
[0146][0147]
式中，表示变异样本，分别代表每次迭代过程中随机选取的原始样本，g代表当前迭代次数，f表示缩放因子，其值一般在[0,2]之间选择，通常取0.5。
[0148]
c.利用式(15)将部分所述步骤a获取的所述原始样本与所述步骤b获取的所述变异样本的对应分量按照二项式交叉生成试验样本
[0149][0150]
式中，cr为交叉算子，rand( )表示随机采样函数；
[0151]
d.根据目标函数来计算原始样本和试验样本的适应度，按照式(16)选取结果较好的样本进行保留，进入下一次迭代：
[0152][0153]
式中，f( )表示最佳分段数优化目标函数；
[0154]
最后，当迭代次数达到设定阈值时，选取最佳适应度的样本作为输出，获得最佳分段位置b
best
。
[0155]
步骤416，根据获取的最佳分段位置b
best
，将步骤3输出的增强后的地面点云进行分块操作。
[0156]
在一个实施例中，步骤41是经过分段拟合后的多个道路平面，当然也可以采用现有的其他方法实现，但这无法自适应的需求。
[0157]
步骤42，经过将投影后的点云进行自适应分段之后，将每一段的索引返回至原始点云，实现地面点云分块；再针对每一块点云分别进行模型拟合，根据地面点云的平面假设，对于存在相同纵向坡度的每一块点云可以近似看作为一个平面。对于路况复杂的非结构化道路，为了有效的减少地面局部凸起和凹陷对路面模型的影响，本文采用随机采样一致性算法(ransac)对分块地面进行拟合，即以平面为初始模型，通过随机采样一致性算法进行迭代，当误差小于设定阈值d时，可得到每块点云的最佳平面方程：
[0158]ai
x+biy+ciz+di＝0
ꢀꢀ
(17)
[0159]
式中，i∈[1，2，...，n]，在此处是每个平面的索引值。此公式中的参数a，b，c，d就是用ransac平面拟合算法得到的拟合平面的四个参数。
[0160]
步骤5，道路参数估计：基于路面平面模型，通过计算相邻两平面的法向量夹角即可求得相邻平面的相对纵向/横向坡度；通过计算地面点云到对应拟合平面之间的距离，并采用距离的均方根作为路面粗糙度的评价指标；通过拟合每个分块平面的道路边界，进行道路曲率估计。
[0161]
所述步骤5中，由于车辆有可能在任何地形启动，因此难以获取绝对的纵向/横向坡度，而在车辆行驶过程中相邻两平面的相对纵向/横向坡度已经完全足够用于车辆的规划控制。根据步骤3中获取的平面模型，容易求得每个平面i的法向量为ni＝(ai，bi，ci)，则相邻两平面的相对纵向坡度和相对横向坡度可由相邻平面的法向量夹角计算得到，其计算公式如下：
[0162][0163][0164]
式中，i∈[2，3，...，n]。
[0165]
路面粗糙度用于表征局部路面的坑洼程度，本发明实施例先计算地面点云中的每一个点pj(xj，yj，zj)到其对应的分块拟合平面的距离d
ij
，再通过计算距离的均方根误差作为路面粗糙度的评价指标μ，为每一个分块平面进行路面粗糙度估计：
[0166][0167][0168]
道路曲率用于表征道路的弯曲程度，本发明实施例根据图像地面分割结果，检测并标记道路边界点，为了消除道路坡度对曲率估计的影响，将道路边界点反投影至分块平面中，利用分块平面坡度信息计算与车辆坐标系z轴的旋转矩阵，用来消除每个分块平面相对于车辆自身的坡度；在每个分块平面上利用三阶bezier曲线进行道路边界拟合，并计算
拟合后的道路边界线的曲率r作为道路曲率。
[0169]
本发明专利提出一种非结构化道路状态参数估计方法。其步骤：通过gps同步授时以及传感器联合标定实现激光雷达与相机的时空同步；通过激光雷达和相机获取同步之后的三维点云和rgb图像数据，进行概率图建模，融合点云和图像的分割结果，通过概率打分实现精准的地面分割；并采用双向线性插值的方法对于无点云投影像素进行深度补全，通过逆投影变换实现点云的数据增强；通过对增强之后的地面点云采用自适应分块拟合，实现精准的路面建模；最后基于路面平面模型对道路横/纵向坡度、路面粗糙度和道路曲率进行参数估计，从而提前获取前方道路的状态参数信息，为后续路径规划和运动控制提供保障。
[0170]
此外，本发明还提供一种采用如上述各实施例所述非结构化道路状态参数估计方法的系统，其包括时间同步单元、信息采集单元、道路参数估计单元。其中时间同步单元由车载gps和触发盒构成，主要用来给传感器提供触发信号，实现传感器的时间同步功能；信息采集单元由激光雷达和相机组成，主要用于采集车辆前方道路的三维点云信息和图像信息，实现前方道路多传感器数据采集功能；道路参数估计单元由计算单元构成，用于处理采集的点云和图像数据并计算出前方道路的状态参数，实现道路参数估计功能。
[0171]
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。