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

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

Info

Publication number
CN114565616B
CN114565616B CN202210202063.XA CN202210202063A CN114565616B CN 114565616 B CN114565616 B CN 114565616B CN 202210202063 A CN202210202063 A CN 202210202063A CN 114565616 B CN114565616 B CN 114565616B
Authority
CN
China
Prior art keywords
point cloud
pixel
node
ground
road
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210202063.XA
Other languages
English (en)
Other versions
CN114565616A (zh
Inventor
谢国涛
陈涵
周岩
秦兆博
秦晓辉
王晓伟
秦洪懋
胡满江
徐彪
边有钢
杨泽宇
丁荣军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuxi Institute Of Intelligent Control Hunan University
Original Assignee
Wuxi Institute Of Intelligent Control Hunan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuxi Institute Of Intelligent Control Hunan University filed Critical Wuxi Institute Of Intelligent Control Hunan University
Priority to CN202210202063.XA priority Critical patent/CN114565616B/zh
Publication of CN114565616A publication Critical patent/CN114565616A/zh
Application granted granted Critical
Publication of CN114565616B publication Critical patent/CN114565616B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/50—Image enhancement or restoration using two or more images, e.g. averaging or subtraction
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30236—Traffic on road, railway or crossing

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Processing (AREA)

Abstract

本发明公开了一种非结构化道路状态参数估计方法及系统，其包括：步骤1，将采集到的点云数据与图像数据进行时间同步和空间同步，并获得激光雷达坐标系中的点云数据中的点云到像素坐标系中的像素的映射关系；步骤2，多传感器融合地面分割，输出点云地面分割结果和图像地面分割结果；步骤3，地面点云数据增强；步骤4，道路自适应分块拟合，实现路面建模；步骤5，道路参数估计：基于路面平面模型，通过计算相邻两平面的法向量夹角即可求得相邻平面的相对纵向/横向坡度；通过计算地面点云到对应拟合平面之间的距离，并采用距离的均方根作为路面粗糙度的评价指标；通过拟合每个分块平面的道路边界，进行道路曲率估计。

Description

一种非结构化道路状态参数估计方法及系统
技术领域
本发明涉及智能车辆环境感知技术领域，特别是关于一种非结构化道路状态参数估计方法及系统。
背景技术
汽车智能化是汽车行业发展的重要趋势，环境感知技术是智能车辆技术的核心技术之一。环境感知技术通过获取和分析传感器数据，为决策、控制等技术提供环境信息。然而在矿区、越野场景这类非结构化道路上行驶时，智能车辆需要面对路面类型多变、坡度起伏明显等问题，这无疑对智能车辆的运动控制、安全行驶带来极大的挑战。因此提前预知前方道路地面状态参数对保障智能车辆正常安全行驶具有重要意义。利用车载传感器实时估计车辆前方的道路状态，车辆便能够根据相关状态参数及时调整行驶姿态，实现最优的路径规划和运动控制。
目前的各种车载传感器的地面状态的估计方法主要存在以下几点问题：
(1)基于深度相机的方法对光照条件要求严格，且受相机深度估计值影响较大，在复杂环境条件下鲁棒性差；
(2)基于三维点云的方法仅仅依赖空间几何信息，无法做到精确的地面分割，且对于远处点云稀疏区域，参数估计误差较大；
(3)目前存在的道路状态参数估计方法中所使用的平面分块策略均未考虑实际道路情况，鲁棒性差，大大降低了后续参数估计的准确度；
(4)基于车辆行驶参数的方法属于接触式估计方法，只有当车辆行驶到当前路面时才能对其地面状态进行精准的估计，无法提前预测车辆前方行驶区域的地面状态参数，达不到提前规划的目的。
发明内容
本发明的目的在于提供一种非结构化道路状态参数估计方法及系统来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的，本发明提供一种非结构化道路状态参数估计方法，其包括：
步骤1，将采集到的点云数据与图像数据进行时间同步和空间同步，并获得激光雷达坐标系OXYZ中的点云数据中的某个点云l(xl，yl，zl)到像素坐标系OUV中图像数据的某个像素c(uc，vc)的映射关系；
步骤2，多传感器融合地面分割，输出点云地面分割结果
Figure GDA0003896610690000021
和图像地面分割结果
Figure GDA0003896610690000022
步骤3，地面点云数据增强，其具体包括：
步骤31，依据步骤1获得的映射关系将步骤2获得的点云地面分割结果
Figure GDA0003896610690000023
中的每个点云投影至图像地面分割结果
Figure GDA0003896610690000024
的像素上，并搜索出不存在投影点云的像素，其在图像OUV坐标系中的坐标表示为(ux，vx)；
步骤32，根据步骤31搜索获得的(ux，vx)及其邻域信息，对所述像素(ux，vx)的深度补全；
步骤33，将(ux，vx，dx)通过步骤1获得的映射关系(1)反投影至激光雷达坐标系OXYZ中，生成新的地面点云，实现对地面点云数据的增强。
步骤4，道路自适应分块拟合，实现路面建模；
步骤5，道路参数估计：基于路面平面模型，通过计算相邻两平面的法向量夹角即可求得相邻平面的相对纵向/横向坡度；通过计算地面点云到对应拟合平面之间的距离，并采用距离的均方根作为路面粗糙度的评价指标；通过拟合每个分块平面的道路边界，进行道路曲率估计。
进一步地，所述步骤32采用式(6)、式(7)和式(8)描述的双向线性插值的方法对所述像素(ux，vx)的深度补全：
Figure GDA0003896610690000025
Figure GDA0003896610690000026
Figure GDA0003896610690000027
式中，(ut，vx)表示沿着图像坐标系U轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure GDA0003896610690000028
是该像素的深度值；(ut+1，vx)表示沿着图像坐标系U轴正方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure GDA0003896610690000029
是该像素的深度值；(ux，vt)表示沿着图像坐标系V轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure GDA00038966106900000210
是该像素的深度值；(ux，vt+1)表示沿着图像坐标系V轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure GDA0003896610690000031
是该像素的深度值；
Figure GDA0003896610690000032
表示U轴方向上的像素深度线性插值，
Figure GDA0003896610690000033
表示v轴方向上的像素深度线性插值，dx为最后加权计算得到的像素(ux，vx)的深度值，Ku和Kv分别为横向和纵向深度估计值的权重。
进一步地，所述步骤33中的Ku和Kv依据式(9)获得：
Figure GDA0003896610690000034
进一步地，所述步骤4具体包括：
步骤41，将步骤3获取的增强后的地面点云投影至道路侧向，并对投影至道路侧向的地面点云数据进行分块。
步骤42，对分块后的地面点云分别进行平面拟合，获得依据道路实际坡度自适应分段拟合出来的多个道路平面。
进一步地，所述步骤41具体包括：
步骤411，将步骤2获得的地面点云投影至激光雷达坐标系的XOZ平面；
步骤412，构建最佳分段数优化目标函数(10)：
f(N)＝f(b)+k*N (10)
式中，f(b)表示最佳分段位置优化目标函数，N表示分段数，k表示过拟合抑制参数；
步骤413，初始采样，通过贝叶斯优化框架(11)，针对该非凸、非连续的目标函数f(N)进行全局优化，找到最佳分段数N：
Figure GDA0003896610690000035
式中，D表示初始采样集合，p表示概率分布函数；
步骤414，采用残差平方和来定义最佳分段位置优化目标函数(12)：
f(b)＝SSR(b) (12)
式中，b＝[b0，b1，b2，...，bN]表示最佳分段位置，SSR表示残差平方和函数；
步骤415，将上述最佳分段数优化目标函数(10)和最佳分段位置优化目标函数(12)通过差分进化算法进行优化，其优化步骤如下：
a.初始化采样，定义b0为
Figure GDA0003896610690000036
中在激光雷达坐标系的X轴上的最小值，bN为最大值；若分段数为N，则需要优化的分段点数目为N-1，采用式(13)提供的均匀采样的方式在解空间采样M组样本Sm，一般设置M＝(5～10)N，获得原始样本：
Figure GDA0003896610690000041
式中，m∈{1，2，...，M}，表示样本的索引值，n∈{1，2，...，N-1}，表示每个样本的维度索引值，rand表示随机采样函数，
Figure GDA0003896610690000042
和
Figure GDA0003896610690000043
分别表示解空间中第n维的上下边界；
b.用式(14)对所述步骤a获得的样本进行变异操作，获得变异样本：
Figure GDA0003896610690000044
式中，
Figure GDA0003896610690000045
表示变异样本，
Figure GDA0003896610690000046
分别代表每次迭代过程中随机选取的原始样本，G表示当前迭代次数，F表示缩放因子；
c.利用式(15)将部分所述步骤a获取的所述原始样本与所述步骤b获取的所述变异样本的对应分量按照二项式交叉生成试验样本
Figure GDA0003896610690000047
Figure GDA0003896610690000048
其中，CR为交叉算子，rand表示随机采样函数；
d.根据目标函数来计算原始样本和试验样本的适应度，按照式(16)选取结果较好的样本进行保留，进入下一次迭代：
Figure GDA0003896610690000049
式中，f表示最佳分段位置优化目标函数；
最后，当迭代次数达到设定阈值时，选取最佳适应度的样本作为输出，获得最佳分段位置bbest。
步骤416，根据获取的最佳分段位置bbest，将步骤3输出的增强后的地面点云进行分块操作。
进一步地，所述步骤42采用随机采样一致性算法对步骤41提供的分块点云进行拟合，获得每块点云的最佳平面方程：
Aix+Biy+Ciz+Di＝0 (17)
式中，i∈[1，2，...，N]，Ai、Bi、Ci、Di为随机采样一致性算法计算得到的拟合平面的四个参数。
进一步地，所述步骤2具体包括：
步骤21，将步骤1输出的图像数据输入经训练好的实时语义分割网络中，输出图像地面分割结果
Figure GDA0003896610690000051
Figure GDA0003896610690000052
中每个像素的概率值为p(c)；
步骤22，步骤1输出的点云数据输入经过训练好的点云语义分割网络中输出点云地面分割结果
Figure GDA0003896610690000053
Figure GDA0003896610690000054
中每个点的概率值为p(l)；
步骤23，将获取的
Figure GDA0003896610690000055
和
Figure GDA0003896610690000056
采用概率图模型进行建模；
步骤24，根据步骤23的建立的概率图模型，重新估计概率图模型中点云节点和像素节点的属于地面的概率值
Figure GDA0003896610690000057
和
Figure GDA0003896610690000058
其计算公式如下：
Figure GDA0003896610690000059
Figure GDA00038966106900000510
步骤25，根据步骤24估计的每个节点(包含点云节点和像素节点)的概率值
Figure GDA00038966106900000511
按照式(7)获得的Qi，判断每个节点i是否属于地面：
Figure GDA00038966106900000512
式中，阈值T需要根据真实数据进行调整，对于不同的非结构化场景给予不同的大小，1代表属于地面，0代表不属于地面；
步骤26，根据Qi的值更新点云地面分割结果
Figure GDA00038966106900000513
和图像地面分割结果
Figure GDA00038966106900000514
进一步地，所述步骤23具体包括：
步骤231，将所述点云地面分割结果
Figure GDA00038966106900000515
中的第i个点云节点描述为li，其点云节点值被描述为p(li)；将所述图像地面分割结果
Figure GDA00038966106900000516
中的第j个像素节点描述为cj，其像素节点值被描述为p(cj)；
步骤232，先使用K近邻方法，寻找与所述点云地面分割结果
Figure GDA00038966106900000517
中的点云节点li最近邻的k个点云节点lk，其地面点云概率值用p(lk)描述，再将点云节点li与点云节点lk建立节点连接，最后计算点云节点li与点云节点lk与之间的距离dis(li，lk)，将dis(li，lk)的倒数作为点云节点li与点云节点lk的连接边的权重参数；
步骤233，先采用八领域搜索，确定点云地面分割结果
Figure GDA00038966106900000518
中的像素节点cj附近的8个相邻的像素节点cm，其地面像素概率值用p(cj)描述，然后，将连接像素节点ci与像素节点cj建立节点连接，再设置像素节点ci与像素节点cj之间的距离dis(cj，cm)，将dis(cj，cm)的倒数作为像素节点ci与像素节点cj的连接边的权重参数：
步骤234，利用步骤1中给出的空间映射关系将每个点云节点和像素节点建立一一对应的节点连接，并将这两种节点的连接边的权重参数设置为第三预设值。
进一步地，所述步骤233按如下情形的差异设置像素节点ci与像素节点cj之间的距离dis(cj，cm)：
情形一：当像素节点cj与像素节点cm属于直线相邻时，将dis(cj，cm)设置为第一预设值；
情形二：当像素节点cj与像素节点cm属于对角相邻时，将dis(cj，cm)设置为第二预设值。
本发明还提供一种采用如上所述非结构化道路状态参数估计方法的系统。
本发明由于采取以上技术方案，其具有以下优点：
1)本方法采用车载激光雷达和摄像头，融合图像色彩纹理信息和三维点云几何信息进行非结构化道路分割，提升了地面感兴趣区域提取精度；2)本方法采用双向线性插值的方法，对三维点云进行数据增强，减小了因远处点云稀疏带来的模型估计误差；3)本方法采用基于贝叶斯优化的最小二乘分段线性拟合方法，可在无先验信息的情况下实现了地面点云的精确自适应分段；4)基于多传感器数据融合与自适应道路分块拟合方法，实现更加精准的非结构化道路状态参数估计。
附图说明
图1为本发明实施例提供的非结构化道路状态参数估计系统结构示意图。
图2为图1中道路参数估计单元的道路状态参数估计方法流程图。
图3为本发明实施例提供的多传感器空间同步示意图。
图4为本发明实施例提供的概率图融合建模示意图。
图5为本发明实施例提供的双向线性插值方法示意图。
图6为本发明实施例提供的道路自适应分段线性拟合示意图。
具体实施方式
下面结合附图和实施例对本发明进行详细的描述。
如图1所示，本实施例所提供的非结构化道路状态参数估计方法包括：
步骤1，将采集到的点云数据与图像数据进行时间同步和空间同步。其中：点云数据由激光雷达采集得到，图像数据由相机采集得到。
将采集到的点云数据与图像数据进行时间同步可以采用如下方法实现：
通过车载GPS自带的秒脉冲发生器发送PPS脉冲信号，同时触发激光雷达和相机进行测量，进而能够实现点云数据与图像数据的时间同步。
将采集到的点云数据与图像数据进行空间可以采用如下一种方法实现，下面结合图3来说明，图中的Lidar表示激光雷达，其采集到的点云数据也可以称作为三维点云，例如：点云数据中某个点l在激光雷达坐标系OXYZ中的坐标描述为(xl，yl，zl)。图中的Camera表示相机，其采集到的图像数据即为文中所称的图像，其图像中的某个像素c通常表示为像素坐标系OUV中的(uc，vc)：
通过传感器标定获取相机的内参矩阵KC和外参矩阵[RC TC]及激光雷达的外参矩阵[RL TL]，则可以得到该点云l到像素c的映射关系(1)，再利用映射关系(1)，实现点云数据与图像数据的空间同步：
Figure GDA0003896610690000071
式中，dx为像素c对应的深度值。
步骤2，多传感器融合地面分割，结合图2和图4，其具体包括：
步骤21，将步骤1输出的图像数据输入经训练好的实时语义分割网络中，输出图像地面分割结果
Figure GDA0003896610690000072
Figure GDA0003896610690000073
中每个像素的概率值为p(c)。
步骤22，将步骤1输出的点云数据输入经过训练好的点云语义分割网络中输出点云地面分割结果
Figure GDA0003896610690000074
Figure GDA0003896610690000075
中每个点的概率值为p(l)。
步骤23，将获取的
Figure GDA0003896610690000076
和
Figure GDA0003896610690000077
采用概率图模型进行建模。
在一个实施例中，步骤23可以采用如下子步骤实现：
步骤231，将
Figure GDA0003896610690000078
中的每个点视为一个节点，这些节点在文中将被称为点云节点，而且点云节点值取该像素的概率值。例如：
Figure GDA0003896610690000079
中的第i个点云节点可以被描述为li，其点云节点值被描述为p(li)。
同样将
Figure GDA00038966106900000710
中的每个像素视为一个节点，这些节点在文中将被称为像素节点，而且像素节点值取该像素的概率值。例如：
Figure GDA00038966106900000711
中的第j个像素节点可以被描述为cj，其像素节点值被描述为p(cj)。
显然一个节点的只对应一个节点概率值。
步骤232，对于
Figure GDA0003896610690000081
中的每个点云节点，以点云节点li为例，首先，使用K近邻方法，寻找与点云节点li最近邻的k个点云节点，这k个点云节点被描述为点云节点lk，其地面点云概率值用p(lk)描述。再将点云节点li与点云节点lk建立节点连接。最后计算点云节点li与点云节点lk与之间的距离dis(li，lk)，将dis(li，lk)的倒数作为点云节点li与点云节点lk的连接边的权重参数。
步骤233，对于
Figure GDA0003896610690000082
中的每个像素节点，以像素节点cj为例，首先，采用八领域搜索，确定像素节点cj附近的8个相邻的像素节点，这8个像素节点被描述为像素节点cm，其地面像素概率值用p(cj)描述。然后，将连接像素节点ci与像素节点cj建立节点连接。再可以按如下情形的差异设置像素节点ci与像素节点cj之间的距离dis(cj，cm)，将dis(cj，cm)的倒数作为像素节点ci与像素节点cj的连接边的权重参数：
情形一：当像素节点cj与像素节点cm属于直线相邻时，将dis(cj，cm)设置为第一预设值，例如：dis(cj，cm)＝1；
情形二：当像素节点cj与像素节点cm属于对角相邻时，将dis(cj，cm)设置为第二预设值，例如：
Figure GDA0003896610690000083
当然，还可以采用现有的其他方法设置像素节点ci与像素节点cj之间的距离dis(cj，cm)。
步骤234，利用步骤1中给出的空间映射关系(1)将每个点云节点和像素节点建立一一对应的节点连接，并将这两种节点的连接边的权重参数设置为第三预设值，例如dis(li，cj)＝1。
步骤24，根据步骤23的建立的概率图模型，重新估计概率图模型中点云节点和像素节点的属于地面的概率值
Figure GDA0003896610690000084
和
Figure GDA0003896610690000085
其计算公式如下：
Figure GDA0003896610690000086
Figure GDA0003896610690000087
步骤25，根据步骤24估计的每个节点(包含点云节点和像素节点)的概率值
Figure GDA0003896610690000088
按照式(7)获得的Qi，判断每个节点i是否属于地面：
Figure GDA0003896610690000091
式中，阈值T需要根据真实数据进行调整，对于不同的非结构化场景给予不同的大小，1代表属于地面，0代表不属于地面。
步骤26，根据Qi的值更新点云地面分割结果
Figure GDA0003896610690000092
和图像地面分割结果
Figure GDA0003896610690000093
需要说明的是，步骤2还可以采用隐马尔科夫随机场、条件随机场和深度学习语义分割的方法，来进行多传感器融合地面分割。
步骤3，将步骤2中获得的地面点云进行数据增强，以便于后续的地面状态参数计算。
在一个实施例中，步骤3可以采用如下子步骤实现步骤3：
步骤31，依据步骤1获得的映射关系(1)将步骤2获得的点云地面分割结果
Figure GDA0003896610690000094
中的每个点云投影至图像地面分割结果
Figure GDA0003896610690000095
的像素上，并搜索出不存在投影点云的像素，其在图像OUV坐标系中的坐标表示为(ux，vx)。
步骤32，如图5所示，根据步骤31搜索获得的(ux，vx)及其邻域信息，采用式(6)、式(7)和式(8)描述的双向线性插值方法进行像素(ux，vx)的深度补全：
Figure GDA0003896610690000096
Figure GDA0003896610690000097
Figure GDA0003896610690000098
式中，(ut，vx)表示沿着图像坐标系U轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure GDA0003896610690000099
是该像素的深度值；(ut+1，vx)表示沿着图像坐标系U轴正方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure GDA00038966106900000910
是该像素的深度值；(ux，vt)表示沿着图像坐标系V轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure GDA00038966106900000911
是该像素的深度值；(ux，vt+1)表示沿着图像坐标系V轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure GDA00038966106900000912
是该像素的深度值；
Figure GDA00038966106900000913
表示U轴方向上的像素深度线性插值，
Figure GDA00038966106900000914
表示v轴方向上的像素深度线性插值，dx为最后加权计算得到的像素(ux，vx)的深度值，Ku和Kv分别为横向和纵向深度估计值的权重：
Figure GDA0003896610690000101
需要说明的是，步骤32还可以采用基于德劳雷三角形的填充方法、扩展滤波的方法，对缺少投影点云的像素(ux，vx)进行深度补全。
步骤33，将(ux，vx，dx)通过步骤1获得的映射关系(1)反投影至激光雷达坐标系OXYZ中，生成新的地面点云，实现对地面点云数据的增强。
步骤4，道路自适应分块拟合，其具体包括：
步骤41，将步骤3获取的增强后的地面点云投影至道路侧向，并对投影至道路侧向的地面点云数据进行分块。
步骤42，对分块后的地面点云分别进行平面拟合，获得依据道路实际坡度自适应分段拟合出来的多个道路平面。
在一个实施例中，在非结构化道路场景，路面可能存在坡度起伏的情况，对不同坡度的路面应分别进行拟合计算，因此需要更加精确的路面划分方法来保证路面坡度估计的精确度，那么，在缺乏先验信息的情况下，本发明实施例采用基于贝叶斯优化的最小二乘分段线性拟合方法，对投影至道路侧向的地面点云数据进行精确自适应分块。如图6所示，具体包括：
步骤411，为了减小噪声点对于模型拟合的影响，将步骤2获得的地面点云投影至激光雷达坐标系的XOZ平面。
步骤412，构建最佳分段数优化目标函数(10)：
f(N)＝f(b)+k*N (10)
式中，f(b)表示最佳分段位置优化目标函数，N表示分段数，k表示过拟合抑制参数，用于平衡残差平方和与分段数的权重。
步骤413，进行一组初始采样，通过贝叶斯优化框架(11)，针对该非凸、非连续的目标函数f(N)进行全局优化，找到最佳分段数N：
Figure GDA0003896610690000102
式中，D表示初始采样集合，p表示概率分布函数。
步骤414，采用残差平方和来定义最佳分段位置优化目标函数(12)：
f(b)＝SSR(b) (12)
式中，b＝[b0，b1，b2，...，bN]表示分段点位置，即最佳分段位置，SSR表示残差平方和函数。
将上述最佳分段数优化目标函数(10)和最佳分段位置优化目标函数(12)通过差分进化算法进行优化，其优化步骤如下：
a.初始化采样，定义b0为
Figure GDA0003896610690000111
中在激光雷达坐标系的X轴上的最小值，bN为最大值；若分段数为N，则需要优化的分段点数目为N-1，采用式(13)提供的均匀采样的方式在解空间采样M组样本Sm，获得原始样本：
Figure GDA0003896610690000112
式中，m∈{1，2，...，M}，表示样本的索引值，n∈{1，2，...，N-1}，表示每个样本的维度索引值，rand表示随机采样函数，
Figure GDA0003896610690000113
和
Figure GDA0003896610690000114
分别表示解空间中第n维的上下边界；
b.用式(14)对所述步骤a获得的样本进行变异操作，获得变异样本：
Figure GDA0003896610690000115
式中，
Figure GDA0003896610690000116
表示变异样本，
Figure GDA0003896610690000117
分别代表每次迭代过程中随机选取的原始样本，G代表当前迭代次数，F表示缩放因子，其值一般在[0，2]之间选择，通常取0.5。
c.利用式(15)将部分所述步骤a获取的所述原始样本与所述步骤b获取的所述变异样本的对应分量按照二项式交叉生成试验样本
Figure GDA0003896610690000118
Figure GDA0003896610690000119
式中，CR为交叉算子，rand表示随机采样函数；
d.根据目标函数来计算原始样本和试验样本的适应度，按照式(16)选取结果较好的样本进行保留，进入下一次迭代：
Figure GDA00038966106900001110
式中，f表示最佳分段数优化目标函数；
最后，当迭代次数达到设定阈值时，选取最佳适应度的样本作为输出，获得最佳分段位置bbest。
步骤416，根据获取的最佳分段位置bbest，将步骤3输出的增强后的地面点云进行分块操作。
在一个实施例中，步骤41是经过分段拟合后的多个道路平面，当然也可以采用现有的其他方法实现，但这无法自适应的需求。
步骤42，经过将投影后的点云进行自适应分段之后，将每一段的索引返回至原始点云，实现地面点云分块；再针对每一块点云分别进行模型拟合，根据地面点云的平面假设，对于存在相同纵向坡度的每一块点云可以近似看作为一个平面。对于路况复杂的非结构化道路，为了有效的减少地面局部凸起和凹陷对路面模型的影响，本文采用随机采样一致性算法(RANSAC)对分块地面进行拟合，即以平面为初始模型，通过随机采样一致性算法进行迭代，当误差小于设定阈值d时，可得到每块点云的最佳平面方程：
Aix+Biy+Ciz+Di＝0 (17)
式中，i∈[1，2，...，N]，在此处是每个平面的索引值。此公式中的参数A，B，C，D就是用RANSAC平面拟合算法得到的拟合平面的四个参数。
步骤5，道路参数估计：基于路面平面模型，通过计算相邻两平面的法向量夹角即可求得相邻平面的相对纵向/横向坡度；通过计算地面点云到对应拟合平面之间的距离，并采用距离的均方根作为路面粗糙度的评价指标；通过拟合每个分块平面的道路边界，进行道路曲率估计。
所述步骤5中，由于车辆有可能在任何地形启动，因此难以获取绝对的纵向/横向坡度，而在车辆行驶过程中相邻两平面的相对纵向/横向坡度已经完全足够用于车辆的规划控制。根据步骤3中获取的平面模型，容易求得每个平面i的法向量为ni＝(Ai，Bi，Ci)，则相邻两平面的相对纵向坡度
Figure GDA0003896610690000121
和相对横向坡度
Figure GDA0003896610690000122
可由相邻平面的法向量夹角计算得到，其计算公式如下：
Figure GDA0003896610690000123
Figure GDA0003896610690000124
式中，i∈[2，3，...，N]。
路面粗糙度用于表征局部路面的坑洼程度，本发明实施例先计算地面点云中的每一个点Pj(xj，yj，zj)到其对应的分块拟合平面的距离dij，再通过计算距离的均方根误差作为路面粗糙度的评价指标μ，为每一个分块平面进行路面粗糙度估计：
Figure GDA0003896610690000125
Figure GDA0003896610690000131
道路曲率用于表征道路的弯曲程度，本发明实施例根据图像地面分割结果，检测并标记道路边界点，为了消除道路坡度对曲率估计的影响，将道路边界点反投影至分块平面中，利用分块平面坡度信息计算与车辆坐标系Z轴的旋转矩阵，用来消除每个分块平面相对于车辆自身的坡度；在每个分块平面上利用三阶Bezier曲线进行道路边界拟合，并计算拟合后的道路边界线的曲率r作为道路曲率。
本发明专利提出一种非结构化道路状态参数估计方法。其步骤：通过GPS同步授时以及传感器联合标定实现激光雷达与相机的时空同步；通过激光雷达和相机获取同步之后的三维点云和RGB图像数据，进行概率图建模，融合点云和图像的分割结果，通过概率打分实现精准的地面分割；并采用双向线性插值的方法对于无点云投影像素进行深度补全，通过逆投影变换实现点云的数据增强；通过对增强之后的地面点云采用自适应分块拟合，实现精准的路面建模；最后基于路面平面模型对道路横/纵向坡度、路面粗糙度和道路曲率进行参数估计，从而提前获取前方道路的状态参数信息，为后续路径规划和运动控制提供保障。
此外，本发明还提供一种采用如上述各实施例所述非结构化道路状态参数估计方法的系统，其包括时间同步单元、信息采集单元、道路参数估计单元。其中时间同步单元由车载GPS和触发盒构成，主要用来给传感器提供触发信号，实现传感器的时间同步功能；信息采集单元由激光雷达和相机组成，主要用于采集车辆前方道路的三维点云信息和图像信息，实现前方道路多传感器数据采集功能；道路参数估计单元由计算单元构成，用于处理采集的点云和图像数据并计算出前方道路的状态参数，实现道路参数估计功能。
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

1.一种非结构化道路状态参数估计方法，其特征在于，包括：
步骤1，将采集到的点云数据与图像数据进行时间同步和空间同步，并获得激光雷达坐标系OXYZ中的点云数据中的某个点云l(xl,yl,zl)到像素坐标系OUV中的像素c(uc,vc)的映射关系；
步骤2，多传感器融合地面分割，输出点云地面分割结果
Figure FDA0003905613930000011
和图像地面分割结果
Figure FDA0003905613930000012
步骤3，地面点云数据增强，其具体包括：
步骤31，依据步骤1获得的映射关系将步骤2获得的点云地面分割结果
Figure FDA0003905613930000013
中的每个点云投影至图像地面分割结果
Figure FDA0003905613930000014
的像素上，并搜索出不存在投影点云的像素，其在图像OUV坐标系中的坐标表示为(ux，vx)；
步骤32，根据步骤31搜索获得的(ux，vx)及其邻域信息，对所述像素(ux，vx)的深度补全；
步骤33，将(ux,vx,dx)通过步骤1获得的映射关系反投影至激光雷达坐标系OXYZ中，生成新的地面点云，实现对地面点云数据的增强；
步骤4，道路自适应分块拟合，实现路面建模；
步骤5，道路参数估计：基于路面平面模型，通过计算相邻两平面的法向量夹角即可求得相邻平面的相对纵向/横向坡度；通过计算地面点云到对应拟合平面之间的距离，并采用距离的均方根作为路面粗糙度的评价指标；通过拟合每个分块平面的道路边界，进行道路曲率估计；
所述步骤4具体包括：
步骤41，将步骤3获取的增强后的地面点云投影至道路侧向，并对投影至道路侧向的地面点云数据进行分块；
步骤42，对分块后的地面点云分别进行平面拟合，获得依据道路实际坡度自适应分段拟合出来的多个道路平面；
所述步骤41具体包括：
步骤411，将步骤2获得的地面点云投影至激光雷达坐标系的XOZ平面；
步骤412，构建最佳分段数优化目标函数(10)：
f(N)＝f(b)+k*N (10)
式中，f(b)表示最佳分段位置优化目标函数，N表示分段数，k表示过拟合抑制参数；
步骤413，初始采样，通过贝叶斯优化框架(11)，针对该非凸、非连续的目标函数f(N)进行全局优化，找到最佳分段数N：
Figure FDA0003905613930000021
式中，D表示初始采样集合，p表示概率分布函数；
步骤414，采用残差平方和来定义最佳分段位置优化目标函数(12)：
f(b)＝SSR(b) (12)式中，b＝[b0,b1,b2,…,bN]表示最佳分段位置，SSR表示残差平方和函数；
步骤415，将上述最佳分段数优化目标函数(10)和最佳分段位置优化目标函数(12)通过差分进化算法进行优化，其优化步骤如下：
a.初始化采样，定义b0为
Figure FDA0003905613930000022
中在激光雷达坐标系的X轴上的最小值，bN为最大值；若分段数为N，则需要优化的分段点数目为N-1，采用式(13)提供的均匀采样的方式在解空间采样M组样本Sm，获得原始样本：
Figure FDA0003905613930000023
式中，m∈{1,2,…,M},表示样本的索引值，n∈{1,2,…,N-1},表示每个样本的维度索引值，rand表示随机采样函数，
Figure FDA0003905613930000024
和
Figure FDA0003905613930000025
分别表示解空间中第n维的上下边界；
b.用式(14)对所述步骤a获得的样本进行变异操作，获得变异样本：
Figure FDA0003905613930000026
式中，
Figure FDA0003905613930000027
表示变异样本，
Figure FDA0003905613930000028
分别代表每次迭代过程中随机选取的原始样本，G表示当前迭代次数，F表示缩放因子；
c.利用式(15)将部分所述步骤a获取的所述原始样本与所述步骤b获取的所述变异样本的对应分量按照二项式交叉生成试验样本
Figure FDA0003905613930000029
Figure FDA00039056139300000210
其中，CR为交叉算子；
d.根据目标函数来计算原始样本和试验样本的适应度，按照式(16)选取结果较好的样本进行保留，进入下一次迭代：
Figure FDA0003905613930000031
式中，f表示最佳分段位置优化目标函数；
最后，当迭代次数达到设定阈值时，选取最佳适应度的样本作为输出，获得最佳分段位置bbest；
步骤416，根据获取的最佳分段位置bbest，将步骤3输出的增强后的地面点云进行分块操作。
2.如权利要求1所述的非结构化道路状态参数估计方法，其特征在于，所述步骤32采用式(6)、式(7)和式(8)描述的双向线性插值的方法对所述像素(ux，vx)的深度补全：
Figure FDA0003905613930000032
Figure FDA0003905613930000033
Figure FDA0003905613930000034
式中，(ut，vx)表示沿着图像坐标系U轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure FDA0003905613930000035
是该像素的深度值；(ut+1，vx)表示沿着图像坐标系U轴正方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure FDA0003905613930000036
是该像素的深度值；(ux，vt)表示沿着图像坐标系V轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure FDA0003905613930000037
是该像素的深度值；(ux，vt+1)表示沿着图像坐标系V轴负方向的距离像素(ux，vx)的最近的存在投影点云的像素，
Figure FDA0003905613930000038
是该像素的深度值；
Figure FDA0003905613930000039
表示U轴方向上的像素深度线性插值，
Figure FDA00039056139300000310
表示v轴方向上的像素深度线性插值，dx为最后加权计算得到的像素(ux，vx)的深度值，Ku和Kv分别为横向和纵向深度估计值的权重。
3.如权利要求2所述的非结构化道路状态参数估计方法，其特征在于，所述步骤33中的Ku和Kv依据式(9)获得：
Figure FDA00039056139300000311
4.如权利要求1所述的非结构化道路状态参数估计方法，其特征在于，所述步骤42采用随机采样一致性算法对步骤41提供的分块点云进行拟合，获得每块点云的最佳平面方程：
Aix+Biy+Ciz+Di＝0 (17)
式中，i∈[1,2,…,N]，Ai、Bi、Ci、Di为随机采样一致性算法计算得到的拟合平面的四个参数。
5.如权利要求2所述的非结构化道路状态参数估计方法，其特征在于，所述步骤2具体包括：
步骤21，将步骤1输出的图像数据输入经训练好的实时语义分割网络中，输出图像地面分割结果
Figure FDA0003905613930000041
Figure FDA0003905613930000042
中每个像素的概率值为p(c)；
步骤22，步骤1输出的点云数据输入经过训练好的点云语义分割网络中输出点云地面分割结果
Figure FDA0003905613930000043
Figure FDA0003905613930000044
中每个点的概率值为p(l)；
步骤23，将获取的
Figure FDA0003905613930000045
和
Figure FDA0003905613930000046
采用概率图模型进行建模；所述步骤23具体包括：
步骤231，将所述点云地面分割结果
Figure FDA0003905613930000047
中的第i个点云节点描述为li，其点云节点值被描述为p(li)；将所述图像地面分割结果
Figure FDA0003905613930000048
中的第j个像素节点描述为cj，其像素节点值被描述为p(cj)；
步骤232，先使用K近邻方法，寻找与所述点云地面分割结果
Figure FDA0003905613930000049
中的点云节点li最近邻的k个点云节点lk，其地面点云概率值用p(lk)描述，再将点云节点li与点云节点lk建立节点连接，最后计算点云节点li与点云节点lk与之间的距离dis(li,lk)，将dis(li,lk)的倒数作为点云节点li与点云节点lk的连接边的权重参数；
步骤233，先采用八领域搜索，确定点云地面分割结果
Figure FDA00039056139300000410
中的像素节点cj附近的8个相邻的像素节点cm，其地面像素概率值用p(cj)描述，然后，将连接像素节点ci与像素节点cj建立节点连接，再设置像素节点ci与像素节点cj之间的距离dis(cj,cm)，将dis(cj,cm)的倒数作为像素节点ci与像素节点cj的连接边的权重参数：
步骤234，利用步骤1中给出的空间映射关系将每个点云节点和像素节点建立一一对应的节点连接，并将这两种节点的连接边的权重参数设置为第三预设值；
步骤24，根据步骤23的建立的概率图模型，重新估计概率图模型中点云节点和像素节点的属于地面的概率值
Figure FDA0003905613930000051
和
Figure FDA0003905613930000052
其计算公式如下：
Figure FDA0003905613930000053
Figure FDA0003905613930000054
步骤25，根据步骤24估计的每个节点的概率值
Figure FDA0003905613930000055
按照式(7)获得的Qi，判断每个节点i是否属于地面，节点包含点云节点和像素节点：
Figure FDA0003905613930000056
式中，阈值T需要根据真实数据进行调整，对于不同的非结构化场景给予不同的大小，1代表属于地面，0代表不属于地面；
步骤26，根据Qi的值更新点云地面分割结果
Figure FDA0003905613930000057
和图像地面分割结果
Figure FDA0003905613930000058
6.如权利要求5所述的非结构化道路状态参数估计方法，其特征在于，所述步骤233按如下情形的差异设置像素节点ci与像素节点cj之间的距离dis(cj,cm)：
情形一：当像素节点cj与像素节点cm属于直线相邻时，将dis(cj,cm)设置为第一预设值；
情形二：当像素节点cj与像素节点cm属于对角相邻时，将dis(cj,cm)设置为第二预设值。
7.一种采用如权利要求1-6中任一项所述非结构化道路状态参数估计方法的系统。
CN202210202063.XA 2022-03-03 2022-03-03 一种非结构化道路状态参数估计方法及系统 Active CN114565616B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210202063.XA CN114565616B (zh) 2022-03-03 2022-03-03 一种非结构化道路状态参数估计方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210202063.XA CN114565616B (zh) 2022-03-03 2022-03-03 一种非结构化道路状态参数估计方法及系统

Publications (2)

Publication Number Publication Date
CN114565616A CN114565616A (zh) 2022-05-31
CN114565616B true CN114565616B (zh) 2022-12-09

Family

ID=81718524

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210202063.XA Active CN114565616B (zh) 2022-03-03 2022-03-03 一种非结构化道路状态参数估计方法及系统

Country Status (1)

Country Link
CN (1) CN114565616B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114993461B (zh) * 2022-08-08 2022-11-04 成都久和建设设备有限责任公司 一种塔机机构电机振动的检测系统和方法
CN115797338B (zh) * 2023-02-02 2023-05-02 河北工业大学 基于双目视觉的全景路面多性能指标计算方法及系统
CN116184369B (zh) * 2023-04-26 2023-09-12 长沙智能驾驶研究院有限公司 激光雷达的外参标定方法、装置、设备、介质及产品
CN116977331B (zh) * 2023-09-22 2023-12-12 武汉展巨华科技发展有限公司 基于机器视觉的3d模型表面检测方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106157307A (zh) * 2016-06-27 2016-11-23 浙江工商大学 一种基于多尺度cnn和连续crf的单目图像深度估计方法
CN110059768A (zh) * 2019-04-30 2019-07-26 福州大学 用于街景理解的融合点与区域特征的语义分割方法及系统
CN110349221A (zh) * 2019-07-16 2019-10-18 北京航空航天大学 一种三维激光雷达与双目可见光传感器的融合标定方法
CN110781891A (zh) * 2019-11-28 2020-02-11 吉林大学 一种基于激光雷达传感器的识别车辆可行驶区域的方法
CN111239757A (zh) * 2020-03-12 2020-06-05 湖南大学 一种路面特征参数自动提取方法及系统
CN112184589A (zh) * 2020-09-30 2021-01-05 清华大学 一种基于语义分割的点云强度补全方法及系统
CN112396650A (zh) * 2020-03-30 2021-02-23 青岛慧拓智能机器有限公司 一种基于图像和激光雷达融合的目标测距系统及方法
CN112907573A (zh) * 2021-03-25 2021-06-04 东南大学 一种基于3d卷积的深度补全方法
CN113345008A (zh) * 2021-05-31 2021-09-03 湖南大学 一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法
CN114120283A (zh) * 2021-11-29 2022-03-01 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种道路场景三维语义分割中未知障碍物的判别方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106570915B (zh) * 2016-10-31 2020-05-19 信阳师范学院 一种基于点云数据的全凸闭合连续曲线的树干直径提取方法
CN106931883B (zh) * 2017-03-30 2019-01-18 信阳师范学院 一种基于激光点云数据的树干材积获取方法
CN110689576B (zh) * 2019-09-29 2023-04-07 桂林电子科技大学 一种基于Autoware的动态3D点云正态分布AGV定位方法
CN112258618B (zh) * 2020-11-04 2021-05-14 中国科学院空天信息创新研究院 基于先验激光点云与深度图融合的语义建图与定位方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106157307A (zh) * 2016-06-27 2016-11-23 浙江工商大学 一种基于多尺度cnn和连续crf的单目图像深度估计方法
CN110059768A (zh) * 2019-04-30 2019-07-26 福州大学 用于街景理解的融合点与区域特征的语义分割方法及系统
CN110349221A (zh) * 2019-07-16 2019-10-18 北京航空航天大学 一种三维激光雷达与双目可见光传感器的融合标定方法
CN110781891A (zh) * 2019-11-28 2020-02-11 吉林大学 一种基于激光雷达传感器的识别车辆可行驶区域的方法
CN111239757A (zh) * 2020-03-12 2020-06-05 湖南大学 一种路面特征参数自动提取方法及系统
CN112396650A (zh) * 2020-03-30 2021-02-23 青岛慧拓智能机器有限公司 一种基于图像和激光雷达融合的目标测距系统及方法
CN112184589A (zh) * 2020-09-30 2021-01-05 清华大学 一种基于语义分割的点云强度补全方法及系统
CN112907573A (zh) * 2021-03-25 2021-06-04 东南大学 一种基于3d卷积的深度补全方法
CN113345008A (zh) * 2021-05-31 2021-09-03 湖南大学 一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法
CN114120283A (zh) * 2021-11-29 2022-03-01 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种道路场景三维语义分割中未知障碍物的判别方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Doppler Spectrum Classification with CNNs via Heatmap Location Encoding and a Multi-head Output Layer;Andrew Gilbert;《arxiv》;20191108;论文全文 *
露天矿山运输无人驾驶系统综述;李东林;《机车电传动》;20190310;论文全文 *
面向无人机小样本目标识别的元学习方法研究;李宏男;《无人系统技术》;20191130;论文全文 *

Also Published As

Publication number Publication date
CN114565616A (zh) 2022-05-31

Similar Documents

Publication Publication Date Title
CN114565616B (zh) 一种非结构化道路状态参数估计方法及系统
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN111798475B (zh) 一种基于点云深度学习的室内环境3d语义地图构建方法
CN109752701A (zh) 一种基于激光点云的道路边沿检测方法
CN111126269A (zh) 三维目标检测方法、装置以及存储介质
CN111880191B (zh) 基于多智能体激光雷达和视觉信息融合的地图生成方法
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
Konrad et al. Localization in digital maps for road course estimation using grid maps
JP2014523572A (ja) 地図データの生成
CN111340875A (zh) 一种基于三维激光雷达的空间移动目标检测方法
Jang et al. Road lane semantic segmentation for high definition map
CN111457930B (zh) 一种利用车载Lidar与无人机联合的高精度测图定位方法
EP3293700A1 (en) 3d reconstruction for vehicle
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
CN114659514A (zh) 一种基于体素化精配准的LiDAR-IMU-GNSS融合定位方法
CN106570499A (zh) 一种基于概率图模型的目标跟踪方法
CN116879870A (zh) 一种适用于低线束3d激光雷达的动态障碍物去除方法
CN113111707A (zh) 一种基于卷积神经网络的前车检测与测距方法
CN117516560A (zh) 一种基于语义信息的非结构化环境地图构建方法及系统
Hirata et al. Real-time dense depth estimation using semantically-guided LIDAR data propagation and motion stereo
Huang et al. Probabilistic lane estimation for autonomous driving using basis curves
Meng et al. Berm detection for autonomous truck in surface mine dump area
CN116385997A (zh) 一种车载障碍物精确感知方法、系统及存储介质
CN115861966A (zh) 一种基于高精度地图的障碍物检测方法
CN112241002B (zh) 一种基于Karto SLAM的鲁棒闭环检测新方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant