一种路侧自动驾驶车辆的定位方法与流程

本发明涉及定位方法，特别涉及一种自动驾驶车辆的定位方法。

背景技术：

自动驾驶由于可以节省人力成本，降低事故率，减少油耗等优点，目前各界都在努力推动发展。随着各种车载传感器技术和人工智能技术的发展，自动驾驶技术也日渐趋于商用化。尤其在封闭、半封闭的应用场景，如自动化码头，机场，封闭园区等，自动驾驶技术有希望最先商业落地。

车辆的自动驾驶主要通过定位、感知、决策和控制来实现。目前自动驾驶车辆的定位技术多是通过车载各种传感器，如激光雷达、毫米波雷达、摄像头以及gps等，由于每种传感器有各自适用的场景，采用单一传感器容易失效，造成自动驾驶事故。为了提供自动驾驶的可靠性，现有技术多是采用多传感器融合的方案。车载传感器的定位方式存在信号遮挡的问题，例如在大型建筑物、大型机械附近，gps信号存在多路径效应，造成定位误差较大。如用车载激光雷达进行定位时，在空旷环境时，激光雷达提取不到特征信息，从而也会造成交大定位误差。

传感器可以安放在车辆上，也可以安防路侧，若是开放的应用场景，在路侧铺设传感器由于投入巨大而变得不现实，但是在封闭、半封闭场景，在路侧或者道路上铺设传感器是一种可行的方案。如在上海洋山港四期工程投入运营的自动导引车agv(automatedguidedvehicle)，用于运送集装箱，所采取的方案是在地面埋设大量的磁钉(现有专利申请号201811356784.6)，用于agv的定位和导航，该方案的显著优势是不受天气影响，系统运行可靠稳定，但是也有几个显著的缺点，需预先埋设大量的磁钉，投入成本巨大，另外，需对码头场地进行大规模施工，该方案不适用于老码头的改造。

另外现有技术(申请号201610800121.3)采用路侧无线定位技术，适用于老码头的改造，可灵活铺设该系统，但是由于码头堆放大量金属物体(集装箱)，信号吸收以及反射造成的多路径效应，从而造成定位较大的误差。

技术实现要素：

本发明的目的是为了解决现有自动驾驶定位系统运行成本高，旧场地改造适用范围小、目标定位效果不准确的问题，而提出一种路侧自动驾驶车辆的定位方法。

一种路侧自动驾驶车辆的定位方法，所述的定位方法通过以下步骤实现：

步骤一、在激光雷达有效视场内无目标车辆时，采集静态背景点云数据；

步骤二、激光雷达实时采集有效视场内包括目标车辆的激光点云数据；

步骤三、在包括目标车辆的激光点云数据中滤除背景激光点云数据；

步骤四、对滤除背景的点云数据，再对其作用聚类算法，目的是挑选出目标车辆的点云数据；

步骤五、对于步骤四中获得的不同类别标号的聚类点云数据和其对应的标号，计算聚类点云数据的线段端点之间的距离，通过与目标车辆长度、宽度的几何尺寸比较，筛选出目标车辆的点云；

步骤六、计算目标车辆坐标和航向角；

步骤七、将目标车辆的定位数据发送给车载终端；

步骤八、车载终端接收定位数据以对目标车辆做出控制决策。

本发明的有益效果为：

本发明申请提供一种路侧自动驾驶车辆的定位方法，能够提供基于路侧的高精度定位导航。激光雷达在有效视场内采集静态背景点云数据和动态车辆的点云数据，通过对背景点云的滤除与聚类算法，提取目标车辆点云数据，将计算出的车辆坐标与导航角发送车载终端。本发明方法步骤设计巧妙，计算过程简单，目标定位速度快，目标定位精度在5-10cm。

通过在路边铺设激光雷达的方法作为本发明定位方法的硬件支撑，具有系统铺设灵活、工程量小的优点，能够减少自动驾驶定位的成本投入。且本发明定位方法简便，成本低廉，不需大规模改造码头场地，适用于老码头的改造，具有适用范围大的优点。

附图说明

图1为本发明的方法流程图；

图2为本发明涉及的激光雷达扫描车辆轮廓点云示意图；

图3为本发明涉及的定位计算原理图；

具体实施方式

具体实施方式一：

本实施方式的一种路侧自动驾驶车辆的定位方法，所述的定位方法通过以下步骤实现：

步骤一、在激光雷达有效视场内无目标车辆时，采集静态背景点云数据；

步骤二、激光雷达实时采集有效视场内包括目标车辆的激光点云数据；

步骤三、在包括目标车辆的激光点云数据中滤除背景激光点云数据；

步骤四、对滤除背景的点云数据，再对其作用聚类算法，挑选出目标车辆的点云数据

步骤五、对于步骤四中获得的不同类别标号的聚类点云数据和其对应的标号，计算聚类点云数据的线段端点之间的距离，通过与目标车辆长度、宽度的几何尺寸比较，筛选出目标车辆的点云；

步骤六、计算目标车辆坐标和航向角；

步骤七、将目标车辆的定位数据发送给车载终端；

步骤八、车载终端接收定位数据以对目标车辆做出控制决策。

具体实施方式二：

与具体实施方式一不同的是，本实施方式的一种路侧自动驾驶车辆的定位方法，所述的步骤一中，为了更加有效的处理车辆目标点云数据，需采集定位场景的背景点云数据，具体为：

激光雷达安装调试完毕，在激光雷达有效视场内无目标车辆时采集激光点云数据，作为背景帧fb，并将fb作为技术文档保存。

具体实施方式三：

与具体实施方式二不同的是，本实施方式的一种路侧自动驾驶车辆的定位方法，所述的步骤二中，激光雷达实时采集有效视场内包括目标车辆的激光点云数据的过程是指，本发明所述的激光雷达工作时，目标车辆进入激光雷达的有效视场内，激光雷达实时采集激光点云数据，发送给所述的激光雷达据处理单元。

具体实施方式四：

与具体实施方式三不同的是，本实施方式的一种路侧自动驾驶车辆的定位方法，所述的步骤三中，在包括目标车辆的激光点云数据中滤除背景激光点云数据的过程中，为了更加有效的处理车辆目标点云数据，需在实时的激光点云数据中滤除掉背景点云数据，其过程是指：

(1)数据处理单元将接收到每一帧点云数据作为数据帧fd，遍历其所有点di，减去背景帧fb对应点bi，得到一组差值数据k，

ki＝di-bi

(2)判断是否满足：

ki＜th

若是，则将数据帧fd，对应的i点数据置零，若否，则fd对应点数据点保持不变，获取相对背景变化的点云数据fu。

具体实施方式五：

与具体实施方式四不同的是，本实施方式的一种路侧自动驾驶车辆的定位方法，所述的步骤四中，对滤除背景的点云数据，再对其作用聚类算法，目的是挑选出目标车辆的点云数据；

其中，聚类算法具体步骤如下：

步骤四一、遍历fu中的每一个点i，判断点i是否有类别标号label，如果没有类别标号，该点记为ui，如果有类别标号，则选择fu中其他无类别标号的点，记为ui；

步骤四二、计算fu中除了ui之外的所有点uk与ui之间的欧式距离di，

di＝||ui-uk||

设定阈值r，r选取的范围为0.1-0.5；

判断是否满足：

di＜r

若是，将ui其放入集合nn中；若否，舍弃；

步骤四三、取出集合nn中一点uj，判断uj和ui是否有类别标号，

如果uj有类别标号，而且ui没有类别标号，则将label(ui)赋值为label(uj)，其中label(ui)代表ui的类别标号；

如果uj和ui都有类别标号，而且label(ui)不等于label(uj)，则将label(uj)赋值为label(ui)；

步骤四四、如果ui有类别标号，而且uj没有类别标号，则将label(uj)赋值为label(ui)；

步骤四五、判断集合nn中是否所有点都被选取过，即遍历nn的所有点，

如果集合nn中所有点都被选取过，则执行步骤四六；否则选取集合nn中其他的点，记为uj，执行步骤四三；

步骤四六、如果ui没有类别标号，则建立新的类别标号newlabel给ui，将label(ui)赋值为newlabel；同时将集合nn中所有点的类别标号赋值为newlabel；

步骤四七、判断fu中所有点是否都被选取过，即遍历fu，

如果u中所有点都选取过，则终止，并且输出fu中所有点的类别标号；否则选取fu中其他的点，记为ui，执行步骤四一。

具体实施方式六：

与具体实施方式五不同的是，本实施方式的一种路侧自动驾驶车辆的定位方法，所述的步骤五中，所述的对于步骤四中获得的不同类别标号的聚类点云数据和其对应的标号，计算聚类点云数据的线段端点之间的距离，通过与目标车辆长度、宽度的几何尺寸比较，筛选出目标车辆的点云具体为：

步骤五一、遍历所有类别标号，对于类别标号为i的数据，遍历其所有点，计数点的个数count，当count大于阈值e，(e的数值可取10-30)，执行步骤五二，若小于e，则遍历类别号为j的数据；

步骤五二、判断探测到的车辆轮廓是l型还是i型，判断方法是：

遍历类别标号为i的聚类数据，选取相邻点pi、pj、pk，计算直线pipj和pjpk的夹角

若大于设定的阈值k(如40°-100°)，认为是l的拐点，若识别到拐点，为l型点云，若无拐点为i型点云；

步骤五三、获取线段两端点坐标p1(ρ1,θ1)和p2(ρ2,θ2)，其中，ρ表示距离，θ表示角度，即为极坐标(ρ,θ)选取端点坐标的方法是当聚类点云数据为i型，则聚类点云的首尾点分别为p1，p2；

当聚类点云数据为l型，拐点作为p1，首尾点可分别作为p2进行下一步判断；

步骤五四、计算线段的长度dist，

步骤五五、判断线段的长度dist的长度是否等于车辆的长度或者宽度，

如果dist等于宽度b，则中止，标号为i的数据是目标车辆的数据；其中，b是目标车辆短边长度；

如果dist等于a，则中止，标号为i的数据是目标车辆的数据，其中，a是目标车辆长边长度。

具体实施方式七：

与具体实施方式六不同的是，本实施方式的一种路侧自动驾驶车辆的定位方法，所述的步骤六中，计算目标车辆坐标和航向角的过程包括，获得的是车辆的长度的值和获得的是车辆的长度的值两种情况分别求取目标车辆坐标和航向角，且两种求值过程相同，当获得的是车辆的宽度的值时，dist等于车辆的短边，如图3所示，具体为：

步骤六一、将p1、p2的极坐标转换为笛卡尔坐标p1(x1，y1)，p2(x2，y2)，设定位车辆的几何中心n(x，y)，通过如下计算公式：

得到导航中心的坐标n(x，y)；

步骤六二、计算车辆航向角α，根据得到车辆航向角α＝90°-γ；车辆航向角α是指车辆行进方向与笛卡尔坐标系横坐标轴的夹角；

同理，当获得的是车辆的长度的值，dist等于车辆的长度的值时，计算目标车辆坐标和航向角。

具体实施方式八：

与具体实施方式七不同的是，本实施方式的一种路侧自动驾驶车辆的定位方法，所述的步骤八中，车载终端接收定位数据以对目标车辆做出控制决策的过程，具体为：车载终端接收到点云数据之后，考虑计算和通讯延时，运用延时模型(如匀速直线运动模型)做延时推算处理，再经过坐标系变换，将路测探测单元坐标转化成车体坐标系，再用于车辆决策和控制，完成自动驾驶功能。