一种用于车路协同的空间对齐方法及系统

1.本发明涉及智能交通技术领域，尤其是涉及一种用于车路协同的空间对齐方法及系统。


背景技术：

2.车路协同技术是道路行车安全方面的重要支撑技术，也是实现智能交通的基础研究问题。当前，很多汽车厂商都在致力于智能交通系统its(intelligent transportation systems)的发展，其中智能车路协同关键技术是its研究的热点和前沿技术。目前，世界各国正在积极进行车路协同系统方面的研究与实验，并将其作为改善道路交通安全和效率的重要手段。在国外，欧洲汽车公司早已将车联网技术应用于车队的智能化管理。同时，欧洲的客运公司也在积极推广应用车联网技术。美国的ivhs，日本的vics等系统也都通过车辆与道路之间建立有效的信息通信，从而实现智能交通的管理和信息服务。这些信息服务虽然侧重点各有不同，但都是以车路协同技术为基础。
3.车路协同系统由“先进的车”和“智慧的路”来支持，通过通信平台实现车路云一体化建设，满足车路协同的各种应用需求。“先进的车”是指安装有传感设备的智能汽车，但是单车智能的感知精度和感知范围有限，比如受公交车遮挡的行人；而“智慧的路”是通过安装在路侧的各类先进传感器，通过收集和共享道路和交通参与者的实时信息，由于路侧传感器安装位置较高，可以近似提供宽阔的视野，从而弥补车端传感设备的盲区；此外，路侧还可以部署边缘云来降低车载端的感知成本。通过车路协同感知，可以大大增加车辆的感知范围和感知能力，进而有利于减少交通事故，保障公路中交通参与者的人身安全和财产安全，并且避免道路拥塞，改善交通运输环境。在开展车路协同感知融合之前，需要先进行车路空间对齐，把车载和路侧感知到的信息放在统一的坐标系下进行融合。如果没有进行空间对齐，将车、路感知到的目标或轨迹进行关联时容易产生误差，影响融合效果。


技术实现要素：

4.本发明的目的就是为了克服上述现有技术存在的缺陷而提供了一种用于车路协同的空间对齐方法及系统，保证了车路协同系统在信息融合过程中空间对齐，为车路协同感知构建统一坐标系，提高了感知目标物的定位和跟踪精度。
5.本发明的目的可以通过以下技术方案来实现：
6.根据本发明的第一方面，提供了一种用于车路协同的空间对齐方法，该方法包括以下步骤：
7.步骤s1、分别在路侧、车端部署智能网联路侧子系统、智能网联车载子系统，其中，子系统间通过v2x进行车路协同；
8.步骤s2、智能网联路侧子系统和智能网联车载子系统分别通过构建大地wgs84坐标系进行子系统间的空间对齐；
9.步骤s3、智能网联路侧子系统构建东北天坐标系进行路侧子系统内部的空间对
齐，然后将东北天坐标系转换到大地wgs84坐标系；
10.步骤s4、智能网联车载子系统构建东北天坐标系进行车载子系统内部的空间对齐，然后基于车载gps/北斗将东北天坐标系转换到大地wgs84坐标系。
11.优选地，所述智能网联路侧子系统包括集成在一起的路侧感知设备、路侧计算设备和路侧通信设备；所述路侧感知设备包括摄像头、激光雷达和毫米波雷达。
12.优选地，所述智能网联车载子系统包括集成在一起的gps/北斗模块、车载感知设备、车载计算设备和车载通信设备；所述车载感知设备包括摄像头、激光雷达和毫米波雷达。
13.优选地，所述毫米波雷达、超声波雷达、激光雷达和摄像头对应的空间对齐过程分别为：
14.毫米波雷达：从毫米波雷达坐标系转换到东北天坐标系，具体为：将毫米波雷达固定安装在路侧，通过毫米波雷达外参标定将毫米波雷达坐标系转换到东北天坐标系；
15.激光雷达：从激光雷达坐标系转换到东北天坐标系，具体为：将激光雷达固定安装在路侧，使指南针方向朝向激光雷达坐标系中x轴方向并记录坐标系夹角θ，并采用最小二乘法求取激光雷达三维点云坐标系到东北天坐标系的转换矩阵；
16.摄像头：先对相机进行内参标定，然后通过联合标定由像素坐标系转换到激光雷达坐标系，再从激光雷达坐标系转换到东北天坐标系。
17.优选地，所述对相机进行内参标定，具体为：
18.1)构建相机成像模型，数学表达式为：
[0019][0020]
式中，(u，v，w)为在世界坐标系下某点的物理坐标，(u，v)为(u，v，w)对应的在像素坐标系下的像素坐标，z为尺度因子；为相机的内参矩阵，其中，f为像距，dx、dy分别为x、y方向上的一个像素在相机感光板上的物理长度，uo、vo分别为相机感光板中心在像素坐标系下的坐标，θ表示感光板的横边和纵边之间的角度；
[0021]
2)采集待标定相机的棋盘格标定板的图片，利用相机内参标定原理计算出相机内参矩阵和畸变系数。
[0022]
优选地，所述通过联合标定由像素坐标系转换到激光雷达坐标系，包括以下子步骤：
[0023]
1)在相机和激光雷达共视区内放置标记物，采集标定数据：
[0024]
2)分别获取标记物在相机中的像素坐标、在激光雷达点云中的坐标；
[0025]
3)采用位姿算法求解激光雷达坐标系到相机坐标系的转换矩阵；
[0026]
4)提取点云路面数，并对路面点云进行分区，然后对每个点云分区进行拟合，对路面点云进行上采样，生成点云-像素坐标点对；
[0027]
5)采用生成的点云-像素坐标点对做数据集，采用人工智能算法进行三维点云坐
标的多元回归预测，将像素坐标转换到点云坐标；
[0028]
6)基于激光雷达坐标系到东北天坐标系的转换，将像素坐标到东北天坐标的转换。
[0029]
优选地，所述步骤3)具体为：采用epnp算法求解激光雷达坐标系到相机坐标系的转换矩阵。
[0030]
优选地，所述步骤5)中的人工智能方法包括决策树和随机森林。
[0031]
优选地，所述步骤s3或s4中将东北天坐标系转换到大地wgs84坐标系，转换表达式为：
[0032][0033][0034]
式中，(x,y)为东北天坐标系中任意点坐标，(lat1,lon1)为(x,y)在wgs84坐标系下的经纬度，(lat0,lon0)为东北天坐标系原点的经纬度，r为地球半径。
[0035]
根据本发明的第二方面，提供了一种用于车路协同的空间对齐系统，该系统包括：
[0036]
设置于路侧的智能网联路侧子系统、设置于车辆上的智能网联车载子系统，且智能网联车载子系统和智能网联路侧子系统通过v2x进行车路协同；
[0037]
其中，车路协同过程中采用任一项所述的方法进行空间对齐。
[0038]
与现有技术相比，本发明具有以下优点：
[0039]
1)针对车路协同对时空同步的需要所设计的，通过空间对齐机制，即使针对不同的子系统，如车载子系统或路侧子系统，也能够获取到各个目标物在统一坐标系下的位置状态，为基于位置点的车路协同融合创造了有利条件，保证了车路协同系统在信息融合过程中空间上的对齐，可以为车路协同感知构建统一的坐标系，从而有助于提高感知目标物的定位和跟踪精度；
[0040]
2)本发明针对不同感知设备分别建立东北天坐标系，并通过联合标定法，提高了位置坐标转换的准确性；
[0041]
3)本发明采用人工智能算法实现三维点云到图像像素的映射，使得空间对齐的准确性更高；
[0042]
4)本发明还可进一步扩展到车、路、云一体化系统，可扩展性强、实用性高。
附图说明
[0043]
图1为根据本发明的用于车路协同的空间对齐方法及系统的架构图；
[0044]
图2为根据本发明的用于车路协同的空间对齐方法及系统的智能网联路侧子系统的架构图；
[0045]
图3为根据本发明的用于车路协同的空间对齐方法及系统的激光雷达点云上采样过程；
[0046]
图4为根据本发明的用于车路协同的空间对齐方法及系统的雷达坐标系到东北天坐标系转换的示意图；
[0047]
图5为根据本发明的用于车路协同的空间对齐方法及系统的棋盘格采集示意图；其中，图5a和5b分别不同角度采集示意图。
具体实施方式
[0048]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
[0049]
实施例
[0050]
参照图1-5，本实施例给出了一种用于车路协同的空间对齐系统，该系统包括：设置于路侧的智能网联路侧子系统和设置于车辆上的智能网联车载子系统，且智能网联路侧子系统和智能网联车载子系统通过v2x集成起来的车路协同系统。作为优选的技术方案，该车路协同系统可进一步扩展到车、路、云一体化系统。
[0051]
智能网联路侧子系统包括有路侧感知设备、路侧计算设备和路侧通信设备，其中：路侧计算设备包括mec服务器、工控机和电脑，路侧感知设备包括摄像头、激光雷达、毫米波雷达或其它传感器如微波雷达，路侧通信设备包括5g/4g基站、v2x rsu；
[0052]
路侧感知设备、路侧计算设备和路侧通信设备之间通过路由器、交换机或点到点网络集成起来，其空间对齐过程为如图2所示，针对毫米波雷达，从毫米波雷达坐标系转换到东北天坐标系，微波雷达同毫米波雷达一样；针对激光雷达，从激光雷达坐标系转换到东北天坐标系；针对摄像头，先对相机进行内参标定，然后通过联合标定由像素坐标系转换到激光雷达坐标系，再从激光雷达坐标系转换到东北天坐标系。
[0053]
智能网联车载子系统包括含有兼容北斗/gps的gnss模块，以及集成rtk和imu的组合导航定位系统，车载感知设备、车载计算设备及车载通信设备；其中gps/北斗含有rtk高精度定位。
[0054]
车载计算设备包括嵌入式控制器与工控机，车载通信设备包括5g/4g cpe、v2x obu；
[0055]
车载感知设备包括摄像头、激光雷达、毫米波雷达或其它传感器如超声波雷达；其空间对齐过程为：针对毫米波雷达，从毫米波雷达坐标系转换到东北天坐标系；针对超声波雷达，从超声波雷达坐标系转换到东北天坐标系；针对激光雷达，从激光雷达坐标系转换到东北天坐标系；针对摄像头，先对相机进行内参标定，然后通过联合标定由像素坐标系转换到激光雷达坐标系，再从激光雷达坐标系转换到东北天坐标系。
[0056]
从激光雷达坐标系转换到东北天坐标系，包括以下步骤：
[0057]
激光雷达坐标系和东北天坐标系之间的转换是三维直角坐标系的刚性变换，转换公式如下：
[0058][0059]
其中，x
enu
、y
enu
、z
enu
是东北天坐标，x
lidar
、y
lidar
、z
lidar
是激光雷达坐标。通过对激光
雷达进行标定，在得知若干点在两个坐标系中的对应坐标时，即可利用最小二乘法求得该转换矩阵。
[0060]
对相机进行内参标定，包括以下步骤：
[0061]
相机成像模型可由如下公式描述：
[0062][0063]
其中，(u，v，w)为在世界坐标系下某一点的物理坐标，(u，v)为该点对应的在像素坐标系下的像素坐标，z为尺度因子。
[0064]
矩阵：
[0065][0066]
称为相机的内参矩阵，其中，f为像距，dx，dy分别为x，y方向上的一个像素在相机感光板上的物理长度(即一个像素在感光板上是多少毫米)，uo，vo分别为相机感光板中心在像素坐标系下的坐标，θ表示感光板的横边和纵边之间的角度(90
°
表示无误差)。
[0067]
首先制作棋盘格标定板，测量棋盘格中一个格子的尺寸大小；
[0068]
将棋盘格置于相机视野中，不断移动棋盘格，并且保证棋盘格在相机视野的上下左右前后都有出现，且具有不同方向的倾角，拍摄大约30张棋盘格图像，如图5所示；
[0069]
使用python语言利用张正友标定法对采集的棋盘格图像处理，计算出相机的内参矩阵和畸变系数，并利用求得的畸变系数对图像进行去畸变处理，对比图像去畸变前后的变化，显示图像去畸变后的效果。
[0070]
通过联合标定由像素坐标系转换到激光雷达坐标系，包括以下步骤：
[0071]
(1)标定数据采集。在相机和激光雷达共视区内放置标记物，为了能够在点云中清晰分辨出标记物的位置，可以在标记物上贴上高反射率反光条，如贴上高反射率反光条的手持gps标定杆。用ros同时记录激光雷达点云数据和图像数据的，在使用epnp算法求解联合标定参数时，理论上只需要4个特征点即可求解出标定参数，为了减小在采集数据过程中人为和设备原因引入的误差，可采集多于4个特征点用于求解激光雷达和相机的标定参数；
[0072]
(2)获取标记物在相机中的像素坐标。由于录制的相机数据为ros格式，首先采用python编程语言实现从ros格式的相机数据中提取出图片，以便选择图片中的像素点。获取到图片后，使用c++编程语言实现从图片中提取出像素坐标，使用时，注意准确选择标记物上所贴的反光条的位置，保证提取出的像素点代表真实的标记物所在的位置；
[0073]
(3)获取标记物在激光雷达点云中的坐标。用ros工具回放采集的激光雷达标定数据，将激光雷达点云数据在rviz中可视化，由于标记物上的反光条反射率较高，在rviz中其颜色会与周围颜色明显不同，使用publish point工具在点云中选取标记物的位置，提取出标记物在激光雷达点云中的坐标；
[0074]
(4)获取到多个特征点在激光雷达坐标系和相机坐标系中的坐标后，使用c++编程语言，利用epnp算法迭代优化求解激光雷达坐标系到相机坐标系的转换矩阵，计算像素重投影误差，验证标定结果的精确度。
[0075]
(5)提取点云路面数据。将录制的激光雷达点云标定数据在rviz中可视化，使用publish point工具将路面的点云选中，将点云中路面上的坐标记录下来，获取到路面点云数据；
[0076]
(6)对路面点云进行分区。将路面点云数据在rviz中可视化，调整点云视角，根据路面点云的横截面和路面点云的坐标对点云进行分区，将近似在一个平面的点云区域划为一个点云分区，将整个路面点云划成多个点云分区，并且用publish point工具将每个点云分区的角点保存在文件中；
[0077]
(7)对每个点云分区进行拟合，对路面点云进行上采样。采用python编程语言实现最小二乘法拟合平面算法，用平面拟合每个点云分区，得到每个点云分区的平面方程后，在x轴和y轴方向上间隔1cm生成一个点，增加点云密度，实现对点云的上采样；
[0078]
(8)生成点云-像素坐标点对。用c++编程语言实现点云坐标到像素坐标的转换，根据激光雷达-相机联合标定的结果，将点云坐标映射到像素坐标，生成点云-像素坐标点对；
[0079]
(9)像素坐标转换到点云坐标。用python编程语言实现决策树回归或随机森林回归算法，用生成的点云-像素坐标点对做数据集，实现预测三维点云坐标的多元回归，实现像素坐标到点云坐标的转换；
[0080]
(10)利用前面的激光雷达坐标系到东北天坐标系的转换，可进一步实现像素坐标到东北天坐标的转换。
[0081]
注意，本发明的实施方案不限于基于epnp的联合标定，也可以采用三对点估计位姿的p3p、直接线性变换(dlt)、ba(bundle adjustment)等方法实现联合标定。
[0082]
智能网联路侧子系统与智能网联车载子系统之间的空间对齐，是通过分别将智能网联路侧子系统、智能网联车载子系统从东北天坐标系转换到wgs84坐标系，包括以下步骤：
[0083]
如图4所示，设东北天坐标系原点o的经纬度为lato、lono，东北天坐标系中某点的坐标为(x
′
，y
′
)，则此点的wgs84坐标为
[0084][0085][0086]
其中，r为地球半径，lat1、lon1为wgs84坐标。
[0087]
通过上述公式即可得出激光雷达坐标系中此点的wgs84坐标。
[0088]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

技术特征：
1.一种用于车路协同的空间对齐方法，其特征在于，该方法包括以下步骤：步骤s1、分别在路侧、车端部署智能网联路侧子系统、智能网联车载子系统，其中，子系统间通过v2x进行车路协同；步骤s2、智能网联路侧子系统和智能网联车载子系统分别通过构建大地wgs84坐标系进行子系统间的空间对齐；步骤s3、智能网联路侧子系统构建东北天坐标系进行路侧子系统内部的空间对齐，然后将东北天坐标系转换到大地wgs84坐标系；步骤s4、智能网联车载子系统构建东北天坐标系进行车载子系统内部的空间对齐，然后基于车载gps/北斗将东北天坐标系转换到大地wgs84坐标系。2.根据权利要求1所述的一种用于车路协同的空间对齐方法，其特征在于，所述智能网联路侧子系统包括集成在一起的路侧感知设备、路侧计算设备和路侧通信设备；所述路侧感知设备包括摄像头、激光雷达和毫米波雷达。3.根据权利要求1所述的一种用于车路协同的空间对齐方法，其特征在于，所述智能网联车载子系统包括集成在一起的gps/北斗模块、车载感知设备、车载计算设备和车载通信设备；所述车载感知设备包括摄像头、激光雷达和毫米波雷达。4.根据权利要求2或3所述的一种用于车路协同的空间对齐方法，其特征在于，所述毫米波雷达、激光雷达和摄像头对应的空间对齐过程分别为：毫米波雷达：从毫米波雷达坐标系转换到东北天坐标系，具体为：将毫米波雷达固定安装在路侧，通过毫米波雷达外参标定将毫米波雷达坐标系转换到东北天坐标系；激光雷达：从激光雷达坐标系转换到东北天坐标系，具体为：固定安装激光雷达，使指南针方向朝向激光雷达坐标系中x轴方向并记录坐标系夹角θ，并采用最小二乘法求取激光雷达三维点云坐标系到东北天坐标系的转换矩阵；摄像头：先对相机进行内参标定，然后通过联合标定由像素坐标系转换到激光雷达坐标系，再从激光雷达坐标系转换到东北天坐标系。5.根据权利要求4所述的一种用于车路协同的空间对齐方法，其特征在于，所述对相机进行内参标定，具体为：1)构建相机成像模型，数学表达式为：式中，(u，v，w)为在世界坐标系下某点的物理坐标，(u，v)为(u，v，w)点对应的在像素坐标系下的像素坐标，z为尺度因子；为相机的内参矩阵，其中，f为像距，dx、dy分别为x、y方向上的一个像素在相机感光板上的物理长度，u
o
、v
o
分别为相机感光板中心在像素坐标系下的坐标，θ表示感光板的横边和纵边之间的角度；2)采集待标定相机的棋盘格标定板的图片，利用相机内参标定原理计算出相机内参矩阵和畸变系数。
6.根据权利要求5所述的一种用于车路协同的空间对齐方法，其特征在于，所述通过联合标定由像素坐标系转换到激光雷达坐标系，包括以下子步骤：1)在相机和激光雷达共视区内放置标记物，采集标定数据：2)分别获取标记物在相机中的像素坐标、在激光雷达点云中的坐标；3)采用位姿算法求解激光雷达坐标系到相机坐标系的转换矩阵；4)提取点云路面数，并对路面点云进行分区，然后对每个点云分区进行拟合，对路面点云进行上采样，生成点云-像素坐标点对；5)采用生成的点云-像素坐标点对做数据集，采用人工智能算法进行三维点云坐标的多元回归预测，将像素坐标转换到点云坐标；6)基于激光雷达坐标系到东北天坐标系的转换，将像素坐标到东北天坐标的转换。7.根据权利要求6所述的一种用于车路协同的空间对齐方法，其特征在于，所述步骤3)具体为：采用epnp算法求解激光雷达坐标系到相机坐标系的转换矩阵。8.根据权利要求6所述的一种用于车路协同的空间对齐方法，其特征在于，所述步骤5)中的人工智能方法包括决策树和随机森林。9.根据权利要求1所述的一种用于车路协同的空间对齐方法，其特征在于，所述步骤s3或s4中将东北天坐标系转换到大地wgs84坐标系，转换表达式为：或s4中将东北天坐标系转换到大地wgs84坐标系，转换表达式为：式中，(x,y)为东北天坐标系中任意点坐标，(lat1,lon1)为(x,y)在wgs84坐标系下的经纬度，(lat0,lon0)为东北天坐标系原点的经纬度，r为地球半径。10.一种用于车路协同的空间对齐系统，其特征在于，该系统包括：设置于路侧的智能网联路侧子系统、设置于车辆上的智能网联车载子系统，且智能网联车载子系统和智能网联路侧子系统通过v2x进行车路协同；其中，车路协同过程中采用权利要求1～9任一项所述的方法进行空间对齐。

技术总结
本发明涉及一种用于车路协同的空间对齐方法及系统，该方法包括：S1、分别在路侧、车端部署智能网联路侧子系统、智能网联车载子系统，子系统间通过V2X进行车路协同；S2、智能网联路侧子系统和智能网联车载子系统分别通过构建大地WGS84坐标系进行子系统间的空间对齐；S3、智能网联路侧子系统构建东北天坐标系进行子系统内部的空间对齐，然后将东北天坐标系转换到大地WGS84坐标系；S4、智能网联车载子系统构建东北天坐标系进行车载子系统内部的空间对齐，然后基于车载GPS/北斗将东北天坐标系转换到大地WGS84坐标系。与现有技术相比，本发明保证了车路协同系统在信息融合过程中空间对齐，为车路协同感知构建统一坐标系，提高了感知目标物的定位和跟踪精度。了感知目标物的定位和跟踪精度。了感知目标物的定位和跟踪精度。


技术研发人员：王平 徐筠益 王超 王新红
受保护的技术使用者：同济大学
技术研发日：2023.02.09
技术公布日：2023/6/6