一种基于三维相机组网的S型公路模拟障碍物预警方法

一种基于三维相机组网的s型公路模拟障碍物预警方法
技术领域
1.本发明属于智能汽车障碍物预警领域，具体涉及一种基于三维相机组网的s型公路模拟障碍物预警方法。


背景技术：

2.随着人工智能技术的兴起，以自动驾驶车辆为研究对象的运动路径规划问题越来越受到重视，而预警避障规划是自动驾驶车辆的关键部分，对自动驾驶车辆的研究具有重大意义。国内现有的公路模拟障碍物预警技术存在受无光源及可见度低下的环境干扰，不能准确向汽车发送及时预警的缺点；复杂场景下无法获取二维图像进行道路预警，不能够保证自动驾驶车辆成功预警避障的缺点。


技术实现要素：

3.发明目的：本发明针对现有技术中的不足，提供一种基于三维相机组网的s型公路模拟障碍物预警方法。
4.技术方案：为实现上述目的，本发明采用以下技术方案一种基于三维相机组网的s型公路模拟障碍物预警方法，其特征在于，包括如下步骤：
5.步骤一、在s型公路的每个弯道上方均设置三维相机，三维相机的镜头向下设置；
6.步骤二、建立以每个三维相机为原点的三维坐标系，然后利用三维相机拍摄s型公路场景内的模拟障碍物和模拟汽车，得到视场点云数据；
7.步骤三、对视场点云数据进行随机抽样一致估计算法得到去除参考面的视场点云数据，对去除参考面的视场点云数据进行直通滤波和体素滤波，得到滤波视场点云数据；
8.步骤四、对滤波视场点云数据进行欧式聚类，得到模拟汽车点云数据、独立模拟障碍物点云数据和独立模拟障碍物数量i；
9.步骤五、对独立模拟障碍物点云数据和模拟汽车点云数据分别进行边界框估计，得到独立模拟障碍物点云数据的质心坐标pi(xi，yi，zi)、模拟汽车点云数据的质心坐标r(x0，y0，z0)和独立模拟障碍物的形状，其中，xi、yi和zi分别表示独立模拟障碍物点云数据的质心坐标的x轴坐标、y轴坐标和z轴坐标，x0、y0和z0分别表示模拟汽车点云数据的质心坐标的x轴坐标、y轴坐标和z轴坐标；
10.步骤六、利用每个独立模拟障碍物点云数据的质心坐标和模拟汽车点云数据的质心坐标计算每个独立模拟障碍物点云数据和模拟汽车点云数据的欧氏距离di；
11.步骤七、按照由小到大的顺序对di进行排序操作，得到每个独立模拟障碍物点云数据和模拟汽车点云数据的最小欧氏距离d
min
和最大欧氏距离d
max
；
12.步骤八、对每个独立模拟障碍物点云数据的质心坐标和模拟汽车点云数据的质心坐标去除z轴坐标值，得到只包含有x轴坐标值和y轴坐标值的每个独立模拟障碍物点云数据的质心坐标bi(xi，yi)和模拟汽车点云数据的质心坐标a(x0，y0)；
13.步骤九、利用只包含有x轴坐标值和y轴坐标值的每个独立模拟障碍物点云数据的
质心坐标和模拟汽车点云数据的质心坐标，计算得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据在xoy平面的欧氏距离di；
14.步骤十、利用只包含有x轴坐标值和y轴坐标值的每个独立模拟障碍物点云数据的质心坐标和模拟汽车点云数据的质心坐标和反正切函数，计算得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据的方位角；
15.步骤十一、通过模拟汽车的汽车控制系统读取模拟汽车行驶速度v，并根据汽车行驶速度v计算模拟汽车点云数据到每个独立模拟障碍物点云数据的行驶时间ti；
16.步骤十二、将独立模拟障碍物数量i、独立模拟障碍物的形状、每个独立障碍物点云数据和模拟汽车点云数据的最小欧氏距离d
min
和最大欧氏距离d
max
、每个独立模拟障碍物点云数据相对于模拟汽车点云数据在xoy平面的欧氏距离di、每个独立模拟障碍物点云数据相对于模拟汽车点云数据的方位角和模拟汽车点云数据到每个独立模拟障碍物点云数据的行驶时间ti传输给模拟汽车控制系统，对汽车在s型公路行驶时提前进行预警。
17.对上述技术方案的进一步设计为：所述的步骤三具体包括如下步骤：
18.301)选取估计模型为参考面模型；
19.302)随机选取视场点云数据中部分点云为初始值，然后用此部分点云拟合估计模型，并利用部分点云与参考面模型的误差判断部分点云是否是参考面模型内部点云；
20.303)当部分点云是参考面模型内部点云时，则用参考面模型去测试所有其它的视场点云数据，如果某个点云适用于参考面模型，则将该点云扩充到初始值的部分点云中；当部分点云不是参考面模型内部点云时，则重新随机选取视场点云数据中部分点云为初始值；
21.304)当有至少85％的点云被归类为参考面模型内部点云时，那么估计模型为合理估计模型；
22.305)保存最终合理估计模型为只包含参考面的视场点云数据；
23.306)用视场点云数据减去只包含参考面的视场点云数据可以得到去除参考面的视场点云数据；
24.307)输入直通滤波笛卡尔坐标系内三个坐标轴方向的限定范围，然后对去除参考面的视场点云数据进行直通滤波得到直通滤波后的视场点云数据；
25.308)对直通滤波后的视场点云数据创建一个三维体素栅格，然后将每个体素栅格内所有的点都用该体素栅格内点集的重心来替换，得到滤波视场点云数据。
26.所述的步骤四具体包括如下步骤：
27.401)选取滤波视场点云数据的某个点作为当前点；
28.402)通过高维树近邻搜索算法找到k个离当前点最近的点；
29.403)将当前点最近的点中满足设定阈值数量范围的点聚类到集合q中，并扩充集合q，直到集合q中点的数量不再增加，得到模拟汽车点云数据或独立模拟障碍物点云数据；
30.404)将点的个数小于二十万的集合视为模拟汽车点云数据，将点的个数大于等于二十万的集合视为独立模拟障碍物点云数据，并统计得到独立模拟障碍物的数量。
31.所述的步骤五具体包括如下步骤：
32.501)对模拟汽车点云数据和独立模拟障碍物点云数据分别计算惯性矩和偏心率；
33.502)再对模拟汽车点云数据和独立模拟障碍物点云数据分别计算特征值以及对
应的特征向量，得到模拟汽车点云数据的质心坐标r(x0，y0，z0)和独立障碍物点云数据的质心坐标pi(xi，yi，zi)，建立以独立模拟障碍物点云数据的质心坐标为原点，以特征向量为坐标轴的独立模拟障碍物点云数据坐标系；
34.503)在每个独立模拟障碍物点云数据坐标系内，分别计算每个独立模拟障碍物点云数据的边界值，并为独立模拟障碍物点云数据添加边界框；
35.504)利用惯性矩、偏心率和边界框组成独立模拟障碍物点云数据的有向包围盒；
36.505)将独立模拟障碍物点云数据的有向包围盒进行三维形状匹配，得到独立模拟障碍物形状。
37.所述的步骤六具体为：
38.根据下式
[0039][0040]
计算得到每个独立模拟障碍物点云数据和模拟汽车点云数据的欧氏距离di。
[0041]
所述的步骤九中具体为：
[0042]
根据下式
[0043][0044]
计算得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据在xoy平面的欧氏距离di。
[0045]
所述的步骤十中具体包括如下步骤：
[0046]
1001)对模拟汽车点云数据的质心坐标r(x0，y0，z0)和独立模拟障碍物点云数据的质心坐标pi(xi，yi，zi)去除z轴坐标值，得到a(x0，y0)和bi(xi，yi)；
[0047]
1002)利用a(x0，y0)和bi(xi，yi)的横坐标，根据下式
[0048]
δxi＝x
0-xiꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0049]
计算得到横坐标差值δxi；
[0050]
1003)利用a(x0，y0)和bi(xi，yi)的纵坐标，根据下式
[0051]
δyi＝y
0-yiꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0052]
计算得到纵坐标差值δyi；
[0053]
1004)根据反正切函数
[0054][0055]
计算得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据的方位角θi，其中，当判定模拟障碍物点云数据位于模拟汽车点云数据右前方θi度；当判定模拟障碍物点云数据位于模拟汽车点云数据左前方θi度；当θi等于0时，判定独立模拟障碍物点云数据位于模拟汽车点云数据正前方。
[0056]
所述的步骤十一中根据下式
[0057][0058]
计算模拟汽车点云数据到每个独立模拟障碍物点云数据的行驶时间ti。
[0059]
本发明的有益效果为：
[0060]
与现有技术相比，本发明利用三维相机组网与计算机通信得到模拟障碍物的视场点云数据，将得到的视场点云数据进行滤波去噪、欧式聚类和欧氏距离计算的处理，然后得到精准三维数据点云，由于本发明使用的三维相机是主动成像感知距离设备，克服了智能汽车受可见度低和无光源条件下无法精确预警路障的缺点。本发明利用三维相机组网分别获得每个路段的模拟障碍物的视场点云数据，利用路由器和网线组成三维相机组网，将每个路段的模拟障碍物视场点云数据反馈计算机，三维相机对场景内目标深度信息敏感，克服了复杂场景下无法获取二维图像进行道路预警，不能够保证自动驾驶车辆成功预警避障的缺点。
附图说明
[0061]
图1为本发明的基于三维相机组网的s型公路模拟障碍物预警方法的流程图；
[0062]
图2为本发明的三维相机与s型公路模拟障碍物预警系统图；
[0063]
图3为本发明中的组网三维相机的通信协议图；
[0064]
图4为本发明的独立模拟障碍物点云数据有向包围盒图；
[0065]
图5为本发明的第二个和第四个模拟障碍物点云数据距离信息图。
具体实施方式
[0066]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。现在结合附图对本发明作进一步详细的说明。
[0067]
本实施例提供一种基于三维相机组网的s型公路模拟障碍物预警方法，如图1所示该方法为：
[0068]
步骤1：首先搭建三维相机与s型公路模拟障碍物预警系统，然后建立组网三维相机与汽车控制系统之间的通信；
[0069]
具体通过以下步骤实现：
[0070]
步骤101、利用铁制矩形三维相机可升降支架、三维相机盒和三维相机，搭建室内三维相机与s型公路模拟障碍物预警系统；
[0071]
本实施例使用的三维相机是无锡微视传感mems-3d相机，型号是pca-p/s600，采用850nm红外光源，如图2所示为室内三维相机与s型公路模拟障碍物预警系统图。
[0072]
步骤102、设置铁制矩形三维相机可升降支架的高度为500mm，将三维相机固定在三维相机盒中，三维相机盒固定在支架顶部，并保证三维相机镜头向下，以俯视视角拍摄s型公路；
[0073]
步骤103、本实施例中，s型公路设有三个弯道，在s型公路第一个弯道处放置第一台三维相机，在s型公路第二个弯道处放置第二台三维相机，在s型公路第三个弯道处放置第三台三维相机；
[0074]
步骤104、将第一台三维相机、第二台三维相机和第三台三维相机，通过路由器和网线连接成组网三维相机；
[0075]
本实施例三维相机工作距离为300-600mm，数据接口是usb3.0和gige两种方式。
[0076]
步骤105、建立组网三维相机和汽车控制系统的通信连接，将组网三维相机采集到的数据通过网络发送给汽车控制系统。
[0077]
本实施例通过操作计算机与三维相机适配软件得到模拟障碍物的俯视视角点云数据，如图3所示为组网三维相机的通信协议图。
[0078]
步骤2：建立以每个三维相机为原点的系统坐标系，然后利用三维相机拍摄场景内的s型公路模拟障碍物和模拟汽车，得到视场点云数据；
[0079]
本实施例拍摄得到的视场点云数据分辨率为1280
×
1024，采集时间为0.4-0.8s，其三维相机系统坐标轴值为(200，400，600)，其坐标系以三维相机为原点，初始化三维相机参数，标定0为不输出pcd格式是灰度图，1为输出pcd格式是深度点云图。
[0080]
步骤3：对视场点云数据进行随机抽样一致估计算法得到去除参考面的视场点云数据，对去除参考面的视场点云数据进行直通滤波和体素滤波，得到滤波视场点云数据；
[0081]
具体通过以下步骤实现：
[0082]
步骤301、选取估计模型为参考面模型；
[0083]
本实施例参考面模型使用为平面模型，估计模型为视场点云数据的模型。
[0084]
步骤302、随机选取视场点云数据中部分点云为初始值，然后用此部分点云拟合估计模型，并利用部分点云与参考面模型的误差判断部分点云是否是参考面模型内部点云；
[0085]
步骤303、当部分点云是参考面模型内部点云时，则用参考面模型去测试所有其它的视场点云数据，如果某个点云适用于参考面模型，则认为这个点云也是参考面模型内部点云，将该点云扩充到初始值的部分点云中；当部分点云不是参考面模型内部点云时，则重新随机选取视场点云数据中部分点云为初始值；
[0086]
步骤304、当有至少85％的点云被归类为参考面模型内部点云时，那么估计模型就足够合理；
[0087]
步骤305、保存最终合理的估计模型为只包含参考面的视场点云数据；
[0088]
步骤306、用视场点云数据减去只包含参考面的视场点云数据可以得到去除参考面的视场点云数据；
[0089]
本实施例对视场点云数据的估计模型进行优化，参考面模型类型为平面模型。
[0090]
步骤307、手动输入直通滤波笛卡尔坐标系内三个坐标轴方向限定范围，然后对去除参考面的视场点云数据进行直通滤波得到直通滤波后的视场点云数据；
[0091]
本实施例采用的直通滤波通过设置笛卡尔坐标系内三个坐标轴方向的阈值参数，指定参数范围之内的点通过，将参数范围之外的点过滤掉，实现对去除参考面的视场点云数据的基本过滤。其中，笛卡尔坐标系内x轴范围为-200mm至200mm，笛卡尔坐标系内y轴范围为-400mm至400mm，笛卡尔坐标系内z轴范围为300mm至400mm，直通滤波的参数范围为300mm至400mm。
[0092]
步骤308、对直通滤波后的视场点云数据创建一个三维体素栅格，然后将每个体素栅格内所有的点都用该体素栅格内点集的重心来替换得到滤波视场点云数据。
[0093]
本实施例采用的体素滤波通过体素化栅格进行下采样数据，减少视场点云数据，
有利于下一步的点云分割聚类。将每个体素栅格内所有的点都用该体素栅格内点集的重心来替换，保留滤波后的目标点云数据，比较不同规格三维体素栅格阈值的筛选效果，确定体素滤波的最佳去噪阈值，本发明中体素滤波的最佳去噪阈值为1
×1×
1。
[0094]
步骤4：对滤波视场点云数据进行欧式聚类，得到模拟汽车点云数据、独立模拟障碍物点云数据和独立模拟障碍物数量i，其中，i表示大于等于1小于等于10的正整数；
[0095]
步骤401、选取滤波视场点云数据的某个点作为当前点；
[0096]
步骤402、通过高维树近邻搜索算法找到k个离当前点最近的点；
[0097]
步骤403、将当前点最近的点中满足设定阈值数量范围的点聚类到集合q中，并扩充集合q，直到集合q中点的数量不再增加，得到模拟汽车点云数据或独立模拟障碍物点云数据；
[0098]
本实施例对滤波视场点云数据进行储存，通过高维树近邻搜索对滤波视场点云数据进行高效索引，并且设定最小点云个数范围为五千，设定最大点云个数范围为四十万，当欧式聚类结果中的点云数据包含有五千到四十万个时，则被认定为模拟汽车点云数据或独立模拟障碍物点云数据，否则不被认定为模拟汽车点云数据和独立模拟障碍物点云数据。
[0099]
步骤404、将点的个数小于二十万的集合q视为模拟汽车点云数据，将点的个数大于等于二十万的集合q视为独立模拟障碍物点云数据，统计得到独立模拟障碍物的数量。
[0100]
本实施例对滤波点云数据去除模拟汽车点云数据，设置点云数据最小范围200000个点为独立模拟障碍物点云数据，得到的独立模拟障碍物数量i为4。
[0101]
步骤5：对独立模拟障碍物点云数据和模拟汽车点云数据分别进行边界框估计，得到独立模拟障碍物点云数据的质心坐标pi(xi，yi，zi)、模拟汽车点云数据的质心坐标r(x0，y0，z0)和独立模拟障碍物的形状，其中，xi、yi和zi分别表示独立模拟障碍物点云数据的质心坐标的x轴坐标、y轴坐标和z轴坐标，xi和yi均大于0厘米小于等于18厘米，zi大于0厘米小于等于35厘米，x0、y0和z0分别表示模拟汽车点云数据的质心坐标的x轴坐标、y轴坐标和z轴坐标，x0和y0均大于0厘米小于等于18厘米，z0大于0厘米小于等于35厘米；
[0102]
具体通过以下步骤实现：
[0103]
步骤501、对模拟汽车点云数据和独立模拟障碍物点云数据分别计算惯性矩和偏心率；
[0104]
步骤502、再对模拟汽车点云数据和独立模拟障碍物点云数据分别计算特征值以及对应的特征向量，得到模拟汽车点云数据的质心坐标r(x0，y0，z0)和独立障碍物点云数据的质心坐标pi(xi，yi，zi)，建立以独立模拟障碍物点云数据的质心坐标为原点，以特征向量为坐标轴的独立模拟障碍物点云数据坐标系；
[0105]
本实施例通过确定独立模拟障碍物点云数据的有向包围盒的主轴方向，获取独立模拟障碍物点云数据坐标的x轴和y轴分量，计算出协方差矩阵、特征值和对应特征向量，最大特征值对应的特征向量为独立模拟障碍物点云数据的有向包围盒的主轴方向，即为独立模拟障碍物坐标系的坐标轴。
[0106]
步骤503、在每个独立模拟障碍物点云数据坐标系内，分别计算每个独立模拟障碍物点云数据的边界值，并为独立模拟障碍物点云数据添加边界框；
[0107]
本实施例在每个独立模拟障碍物点云数据坐标系内，确定方向向量，并将独立模拟障碍物点云数据的坐标点投影到方向向量上，计算出每个独立模拟障碍物点云数据坐标
的x轴和y轴分量在每个方向上的最大值和最小值，即为每个独立模拟障碍物点云数据的边界值，然后为独立模拟障碍物点云数据添加边界框，其中，设置有向包围盒范围为2
×2×
2，迭代次数为300次。
[0108]
步骤504、利用惯性矩、偏心率和边界框组成独立模拟障碍物点云数据的有向包围盒；
[0109]
步骤505、将独立模拟障碍物点云数据的有向包围盒进行三维形状匹配，得到独立模拟障碍物形状。
[0110]
本实施例给每个独立模拟障碍物点云数据不同的灰度值，区分不同的独立障碍物形状，如图4所示为独立模拟障碍物点云数据有向包围盒图，其中，第一个障碍物为长方体，第二个障碍物为圆柱体，第三个障碍物为长方体，第四个障碍物为长方体。
[0111]
步骤6：利用每个独立障碍物点云数据的质心坐标和模拟汽车点云数据的质心坐标计算每个独立障碍物点云数据和模拟汽车点云数据的欧氏距离di，其中，di大于等于1厘米小于等于25厘米；
[0112]
具体通过以下实现：
[0113]
根据
[0114][0115]
计算得到每个独立障碍物点云数据和模拟汽车点云数据的欧氏距离di，其中，表示开平方，(
·
)2表示平方。
[0116]
本实施例中模拟汽车点云数据的质心坐标为(180.067，195.015，56.724)(mm)，其中mm表示毫米，第一个模拟障碍物点云数据的质心坐标为(95.675，87.211，37.316)(mm)，第二个模拟障碍物点云数据的质心坐标为(121.621，38.961，45.879)(mm)，第三个模拟障碍物点云数据的质心坐标为(56.320，126.597，69.123)(mm)，第四个模拟障碍物点云数据的质心坐标为(150.875，99.110，54.449)(mm)。设置欧氏距离迭代次数为400次，得到并储存每个独立模拟障碍物点云数据和模拟汽车点云数据的欧氏距离di，其中，d1＝138.276mm，d2＝180.432mm，d3＝141.056mm，d4＝100.272mm。
[0117]
步骤7：按照由小到大的顺序对di进行排序操作，得到每个独立障碍物点云数据和模拟汽车点云数据的最小欧氏距离d
min
和最大欧氏距离d
max
；
[0118]
本实施例中，d4＜d1＜d3＜d2，得到最小欧氏距离d
min
＝d4，即为100.272mm；得到最大欧氏距离d
max
＝d2，即为180.432mm。如图5所示为第二个和第四个模拟障碍物点云数据距离信息图。
[0119]
步骤8：对每个独立障碍物点云数据的质心坐标和模拟汽车点云数据的质心坐标去除z轴坐标值，得到只包含有x轴坐标值和y轴坐标值的每个独立障碍物点云数据的质心坐标bi(xi，yi)和模拟汽车点云数据的质心坐标a(x0，y0)；
[0120]
本实施例得到只包含有x轴坐标值和y轴坐标值的模拟汽车点云数据的质心坐标a(180.067，195.015)(mm)，只包含有x轴坐标值和y轴坐标值的第一个模拟障碍物点云数据的质心坐标b1(95.675，87.211)(mm)，只包含有x轴坐标值和y轴坐标值的第二个模拟障碍物点云数据的质心坐标b2(121.621，38.961)(mm)，只包含有x轴坐标值和y轴坐标值的第三个模拟障碍物点云数据的质心坐标b3(56.320，126.597)(mm)，只包含有x轴坐标值和y轴坐
标值的第四个模拟障碍物点云数据的质心坐标b4(150.875，99.110)(mm)。
[0121]
步骤9：利用只包含有x轴坐标值和y轴坐标值的每个独立障碍物点云数据的质心坐标和模拟汽车点云数据的质心坐标，计算得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据在xoy平面的欧氏距离di，其中，di大于等于1厘米小于等于20厘米；
[0122]
具体通过以下步骤实现：
[0123]
根据
[0124][0125]
计算得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据在xoy平面的欧氏距离di；
[0126]
本实施例得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据在xoy平面的欧氏距离di分别为d1＝137.437mm，d2＝167.722mm，d3＝141.905mm，d4＝100.578mm。
[0127]
步骤10：利用只包含有x轴坐标值和y轴坐标值的每个独立障碍物点云数据的质心坐标和模拟汽车点云数据的质心坐标和反正切函数，计算得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据的方位角；
[0128]
步骤1001、对模拟汽车点云数据的质心坐标r(x0，y0，z0)和模拟障碍物点云数据的质心坐标pi(xi，yi，zi)去除z轴坐标值，得到a(x0，y0)和bi(xi，yi)；
[0129]
步骤1002、利用a(x0，y0)和bi(xi，yi)的横坐标，根据
[0130]
δxi＝x
0-xiꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0131]
计算得到横坐标差值δxi，δxi大于等于负20厘米小于等于正20厘米；
[0132]
本实施例中x0与x1的横坐标差值δx1＝84.392mm，x0与x2的横坐标差值δx2＝58.446mm，x0与x3的横坐标差值δx3＝123.747mm，x0与x4的横坐标差值δx4＝29.192mm。
[0133]
步骤1003、利用a(x0，y0)和bi(xi，yi)的纵坐标，根据
[0134]
δyi＝y
0-yiꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0135]
计算得到纵坐标差值δyi，δyi大于等于负20厘米小于等于正20厘米；
[0136]
本实施例中y0与y1纵坐标差值δy1＝107.804mm，y0与y2纵坐标差值δy2＝156.054mm，y0与y3纵坐标差值δy3＝68.418mm、y0与y4纵坐标差值δy4＝95.905mm。
[0137]
步骤1004、根据反正切函数
[0138][0139]
计算得到每个独立模拟障碍物点云数据相对于模拟汽车点云数据的方位角θi，其中，∈(
·
)表示属于操作，当判定模拟障碍物点云数据位于模拟汽车点云数据右前方θi度；当判定模拟障碍物点云数据位于模拟汽车点云数据左前方θi度；当θi等于0时，判定模拟障碍物点云数据位于模拟汽车点云数据正前方。
[0140]
本实施例得出第一个模拟障碍物点云数据位于模拟汽车点云数据右前方51.9
°
，即θ1＝51.9
°
，第二个模拟障碍物点云数据位于模拟汽车点云数据右前方69.4
°
，即θ2＝
69.4
°
，第三个模拟障碍物点云数据位于模拟汽车点云数据右前方28.9
°
，即θ3＝28.9
°
，第四个模拟障碍物点云数据位于模拟汽车点云数据右前方73.0
°
，即θ4＝73.0
°
。
[0141]
步骤11：通过汽车控制系统读入模拟汽车行驶速度v，其中，v表示大于等于1厘米每秒小于等于5厘米每秒的模拟汽车行驶速度，根据
[0142][0143]
计算模拟汽车点云数据到每个独立模拟障碍物点云数据的行驶时间ti，其中，ti大于等于4秒小于等于20秒；
[0144]
本实施例模拟汽车行驶速度为3厘米每秒，计算得到t1＝4.58秒，t2＝5.59秒，t3＝4.73秒，t4＝4.33秒。
[0145]
步骤12：将独立模拟障碍物数量i、独立模拟障碍物的形状、每个独立障碍物点云数据和模拟汽车点云数据的最小欧氏距离d
min
和最大欧氏距离d
max
、每个独立模拟障碍物点云数据相对于模拟汽车点云数据在xoy平面的欧氏距离di、每个独立模拟障碍物点云数据相对于模拟汽车点云数据的方位角和模拟汽车点云数据到每个独立模拟障碍物点云数据的行驶时间ti传输给汽车控制系统，让汽车在s型公路行驶时提前进行预警；
[0146]
本实施例将得到的独立模拟障碍物数量i＝4；第一个障碍物为长方体，第二个障碍物为圆柱体，第三个障碍物为长方体，第四个障碍物为长方体；d1＝138.276mm，d2＝180.432mm，d3＝141.056mm，d4＝100.272mm；d
max
＝180.432mm，d
min
＝100.272mm；d1＝137.437mm，d2＝167.722mm，d3＝141.905mm，d4＝100.578mm；t1＝4.58秒，t2＝5.59秒，t3＝4.73秒，t4＝4.33秒；第一个模拟障碍物点云数据方位角θ1为右前方51.9
°
、第二个模拟障碍物点云数据方位角θ2为右前方69.4
°
、第三个模拟障碍物点云数据方位角θ3为右前方28.9
°
、第四个模拟障碍物点云数据方位角θ4为右前方73.0
°
的路况信息，通过组网三维相机发送给汽车控制系统，进而让汽车能够针对模拟障碍物提前预警。
[0147]
本发明的技术方案不局限于上述各实施例，凡采用等同替换方式得到的技术方案均落在本发明要求保护的范围内。