基于雷达测距识别岸桥的停车方法及系统、服务器及介质与流程

本发明涉及车辆控制以及自动泊车技术领域，具体涉及一种基于雷达测距识别岸桥的停车方法及系统、服务器及介质。

背景技术：

在许多场景中，自动驾驶汽车需要达到一次性精准停车，这种一次性停准的操作对人工来说非常困难，即使是经验丰富的司机也是非常难以达到的。现有的精准停车技术往往采用多个非常确定的路标/参考点，通过传感器识别路标来判断汽车所处的相对位置，这种方案思路对路标参考点铺设精度要求较高，尤其是停车位置是经常移动，这种相对位置定位会增加路标铺设的工作量，通用性不是很好。

技术实现要素：

有鉴于此，本发明提供一种无需要人为铺设路标，且环境通用性较好的基于雷达测距识别岸桥的停车方法及系统。

一种基于雷达测距识别岸桥的停车方法，所述基于雷达测距识别岸桥的停车方法包括以下步骤：

s1、在车辆顶部安装激光雷达，实时采集并存储激光雷达扫描得到的点云数据；

s2、当激光雷达在岸桥点云预估区域内检测到岸桥横梁的特征点云时，提取岸桥横梁靠近端点云预估区域内中距离车辆最近的点云的平均坐标；

s3、以平均坐标为中心原点，提取岸桥横梁本体点云预估区域的点云，并将所提取的三维点云投影至xy二维平面；

s4、利用最小二乘原理把投影在xy二维平面的点集拟合成直线，根据直线的斜率计算得到横梁相对车身的倾斜角，进而计算得到车辆与横梁之间的距离；

s5、统计所有参与直线拟合、且与直线之间的距离小于预设阀值的点云数量，是否大于所有参与拟合的点云总数的预设比例；

s6、如果比较结果为小于，判断为直线拟合为异常，则根据上一激光雷达扫描周期得到的车辆与目标停车点之间的精确预估距离计算得到车辆与目标停车点之间的粗略预估距离；

s7、如果比较结果为大于，判断为直线拟合为正常，则根据车辆与横梁之间的距离计算得到车辆与目标停车点之间的粗略预估距离；

s8、对车辆与目标停车点之间的粗略预估距离进行滤波优化得到车辆与目标停车点之间的精确预估距离，将车辆与目标停车点之间的精确预估距离d3与当前车身速度发送给车辆决策控制系统，由车辆决策控制系统对车体进行相应控制，实现精准定点停车。

一种基于雷达测距识别岸桥的停车系统，所述基于雷达测距识别岸桥的停车系统包括以下功能模块：

点云采集模块，用于在车辆顶部安装激光雷达，实时采集并存储激光雷达扫描得到的点云数据；

横梁点云提取模块，用于在激光雷达在岸桥点云预估区域内检测到岸桥横梁的特征点云时，提取岸桥横梁靠近端点云预估区域内中距离车辆最近的点云的平均坐标；

点云拟合模块，用于以平均坐标为中心，提取岸桥横梁本体点云预估区域的点云，将所提取的三维点云投影至xy二维平面，并利用最小二乘原理把投影在xy二维平面的点集拟合成直线；

距离计算模块，用于根据直线的斜率计算得到横梁相对车身的倾斜角，进而计算得到车辆与横梁之间的距离；

点云比例统计模块，用于统计所有参与直线拟合、且与直线之间的距离小于预设阀值的点云数量，是否大于所有参与拟合的点云总数的预设比例；

正常拟合计算模块，用于如果比较结果为大于，判断为直线拟合为正常，则根据车辆与横梁之间的距离计算得到车辆与目标停车点之间的粗略预估距离；

异常拟合计算模块，用于如果比较结果为小于，判断为直线拟合为异常，则根据上一激光雷达扫描周期得到的车辆与目标停车点之间的精确预估距离计算得到车辆与目标停车点之间的粗略预估距离；

精准停车模块，用于对车辆与目标停车点之间的粗略预估距离进行滤波优化得到车辆与目标停车点之间的精确预估距离，将车辆与目标停车点之间的精确预估距离与当前车身速度发送给车辆决策控制系统，由车辆决策控制系统对车体进行相应控制，实现精准定点停车。

一种服务器，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述基于雷达测距识别岸桥的停车方法的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述基于雷达测距识别岸桥的停车方法的步骤。

本发明所述基于雷达测距识别岸桥的停车方法，其利用激光雷达获取车辆附近确定的障碍物点云来计算确定车辆行进位置，通过识别车辆正前方横梁特征点云并对其进行跟踪测距，结合识别点云中横梁特征测距及车身can总线获取的速度计算目标停车距离，达到实时描述车辆行进位置的目的，相较现有精准停车技术而言，不需要人为铺设路标，可以连续性的，实时的测量，环境通用性较好。

附图说明

图1是本发明所述基于雷达测距识别岸桥的停车方法的步骤流程图；

图2是本发明所述基于雷达测距识别岸桥的停车方法的流程框图；

图3是本发明所述基于雷达测距识别岸桥的停车系统的功能模块框图；

图4为本发明实施例提供的服务器的结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明，应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例提供一种基于雷达测距识别岸桥的停车方法，用于解决现有精准停车技术中需要人为铺设路标，且环境通用性较差的技术问题。

在进行本发明所述基于雷达测距识别岸桥的停车方法之前，需要预先提前采集岸桥所在环境点云，对环境点云进行离线分析结合经验得出岸桥点云预估区域、岸桥横梁靠近端点云预估区域以及岸桥横梁本体点云预估区域，根据测量得到，设置岸桥点云预估区域为{{x,-10,10},{y,3,50},{z,5,15}}，设置岸桥横梁靠近端点云预估区域为{{x,-1,1},{y,3,50},{z,5,10}}，设置岸桥横梁本体点云预估区域为{{x0,-10,10},{y0,-0.1,5},{z0,-3,3}}

并设置岸桥点云区域预估的方法，即当车辆接近岸桥数十米距离的时候，如果激光雷达检测到正前方上方的岸桥点云预估区域内出现几根较均匀的线条时，则判断检测到岸桥横梁的特征点云，也就能够判断当前岸桥点云预估区域为准确的岸桥点云区域；同时，还需要预先按照由车辆靠近目标停车点方向为递增顺序对岸桥横梁进行编号，并测量每一个横梁到目标停车点的直线距离。

如图1和图2所示，所述基于雷达测距识别岸桥的停车方法包括以下步骤：

s1、在车辆顶部安装激光雷达，实时采集并存储激光雷达扫描得到的点云数据。

s2、当激光雷达在岸桥点云预估区域内检测到岸桥横梁的特征点云时，提取岸桥横梁靠近端点云预估区域内中距离车辆最近的点云的平均坐标。

当车辆接近岸桥数十米距离的时候，如果激光雷达检测到岸桥点云预估区域出现出现几根较均匀的线条，则判断激光雷达检测到准确的岸桥点云区域，所述较均匀的线条为岸桥横梁的特征点云。

当激光雷达检测到上述岸桥横梁特征点云后，为了减少客观现实中的噪声影响，避免小概率的误判，提取岸桥横梁靠近端点云预估区域{{x,-1,1},{y,3,50},{z,5,10}}范围中距离车辆最近的10(基于经验统计选取10～30皆可的)个点云，计算所述10个点云的平均坐标{x0，y0，z0}。

s3、以平均坐标为中心原点，提取岸桥横梁本体点云预估区域的点云，并将所提取的三维点云投影至xy二维平面。

以平均坐标{x0，y0，z0}为中心原点，自岸桥点云预估区域{{x,-10,10},{y,3,50},{z,5,15}}范围中提取出岸桥横梁本体点云预估区域{{x0,-10,10},{y0,-0.1,5},{z0,-3,3}}范围内的线状点云，并将这些线状点云由z轴方向投影到xy二维平面上。

s4、利用最小二乘原理把投影在xy二维平面的点集拟合成直线，根据直线的斜率计算得到横梁相对车身的倾斜角，进而计算得到车辆与横梁之间的距离。

设岸桥横梁投影到xy平面后的直线方程原型为：y＝k*x+l，将岸桥横梁上的点集在xy平面投影组成的新点集{(x1,y1),(x2,y2)...(xn,yn)}代入所述直线方程做最小二乘直线拟合，估算得到拟合直线的斜率k和截距l，根据斜率k和截距l计算出横梁相对车身的倾斜角θ＝artan(k)；由于横梁默认是垂直于道路的方向，因此根据横梁相对车身的倾斜角θ计算得到车辆与横梁之间的距离d1＝l*cos(θ)。

s5、统计所有参与直线拟合、且与直线之间的距离小于预设阀值的点云数量，是否大于所有参与拟合的点云总数的预设比例。

根据经验所得，所述预设阈值优选为0.5米，所述预设比例优选为80％。

s6、如果比较结果为小于，判断为直线拟合为异常，则直接根据上一激光雷达扫描周期得到的车辆与目标停车点之间的精确预估距离计算得到车辆与目标停车点之间的粗略预估距离。

即当所有参与直线拟合、且与直线之间的距离小于预设阀值的点云数量小于所有参与拟合的点云总数的预设比例(80％)时，所述当前扫描周期中车辆与目标停车点之间的粗略预估距离d2＝d3-v*t，其中，v为车身can总线输出的实时速度，t为激光雷达获取点云帧的周期。

s7、如果比较结果为大于，判断为直线拟合为正常，则根据车辆与横梁之间的距离计算得到车辆与目标停车点之间的粗略预估距离。

即当所有参与直线拟合、且与直线之间的距离小于预设阀值的点云数量大于所有参与拟合的点云总数的预设比例(80％)时，所述车辆与目标停车点之间的粗略预估距离d2＝d1+offset[i]，其中，i为由车辆靠近目标停车点方向为递增顺序的横梁编号；offset[i]代表编号为i横梁与目标停车点之间的预先测量距离。

优选的，当直线拟合为正常时，进一步判断上一激光雷达扫描周期得到的车辆与目标停车点之间的粗略预估距离d2是否等于车辆与目标停车点之间的精准预估距离d3的预设初始最大值，设d3的预设初始最大值为nan，如果d2＝nan，则代表车辆刚开始接近第一个横梁，此时i＝0，当前周期中车辆与目标停车点之间的粗略预估距离d2＝d1+offset[i]；如果判断结果d2≠nan，则计算当前周期中车辆与目标停车点之间的粗略预估距离d2＝(d1+offset[i])与上一周期的目的停车点距离d2＝(d1+offset[i-1])的差值c＝(d1+offset[i])-(d1+offset[i-1])，并再进一步判断车辆与目标停车点之间的粗略预估距离d2之差c是否大于预设门限值，所述预设门限值根据经验值优选为10米；如果车辆与目标停车点之间的粗略预估距离d2之差c大于预设门限值，表示车体的激光雷达已经从前一个横梁测量跳变到测量后一个新横梁，即车辆行驶完成一个横梁距离，则当前周期中车辆与目标停车点之间的粗略预估距离d2＝(d1+offset[i+1])；如果车辆与目标停车点之间的粗略预估距离d2之差c小于预设门限值，则当前周期中车辆与目标停车点之间的粗略预估距离d2＝(d1+offset[i])。

s8、对车辆与目标停车点之间的粗略预估距离进行滤波优化得到车辆与目标停车点之间的精确预估距离，将车辆与目标停车点之间的精确预估距离d3与当前车身速度发送给车辆决策控制系统，由车辆决策控制系统对车体进行相应控制，实现精准定点停车。

利用高斯噪声模型的卡尔曼滤波器对车辆与目标停车点之间的粗略预估距离d2进行滤波优化得到车辆与目标停车点之间的精确预估距离d3，具体的，所述卡尔曼滤波的高斯噪声模型如下：

状态量：[d3；v]＝[1,-t；0,1]*[d3；v]+系统高斯噪声

观测量：[d2；v]＝[1,0；0,1]*[d3；v]+测量高斯噪声

以上噪声通过调优过程给于一定的经验值即可，然后将得到的车辆与目标停车点之间的精确预估距离d3与当前车身速度发送给车辆决策控制系统，由车辆决策控制系统对车体进行相应控制，实现精准定点停车。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

本发明所述基于雷达测距识别岸桥的停车方法，其利用激光雷达获取车辆附近确定的障碍物点云来计算确定车辆行进位置，通过识别车辆正前方横梁特征点云并对其进行跟踪测距，结合识别点云中横梁特征测距及车身can总线获取的速度计算目标停车距离，达到实时描述车辆行进位置的目的，相较现有精准停车技术而言，不需要人为铺设路标，可以连续性的，实时的测量，环境通用性较好。

上面主要描述了一种基于雷达测距识别岸桥的停车方法，下面将对一种基于雷达测距识别岸桥的停车系统进行详细描述。

图3示出了本发明实施例提供一种基于雷达测距识别岸桥的停车系统的一个实施例结构图。如图3所示，所述一种基于雷达测距识别岸桥的停车系统，所述基于雷达测距识别岸桥的停车系统包括以下功能模块：

点云采集模块10，用于在车辆顶部安装激光雷达，实时采集并存储激光雷达扫描得到的点云数据；

横梁点云提取模块20，用于在激光雷达在岸桥点云预估区域内检测到岸桥横梁的特征点云时，提取岸桥横梁靠近端点云预估区域内中距离车辆最近的点云的平均坐标；

点云拟合模块30，用于以平均坐标为中心，提取岸桥横梁本体点云预估区域的点云，将所提取的三维点云投影至xy二维平面，并利用最小二乘原理把投影在xy二维平面的点集拟合成直线；

距离计算模块40，用于根据直线的斜率计算得到横梁相对车身的倾斜角，进而计算得到车辆与横梁之间的距离；

点云比例统计模块50，用于统计所有参与直线拟合、且与直线之间的距离小于预设阀值的点云数量，是否大于所有参与拟合的点云总数的预设比例；

正常拟合计算模块60，用于如果比较结果为大于，判断为直线拟合为正常，则根据车辆与横梁之间的距离计算得到车辆与目标停车点之间的粗略预估距离；

异常拟合计算模块70，用于如果比较结果为小于，判断为直线拟合为异常，则根据上一激光雷达扫描周期得到的车辆与目标停车点之间的精确预估距离计算得到车辆与目标停车点之间的粗略预估距离；

精准停车模块80，用于对车辆与目标停车点之间的粗略预估距离进行滤波优化得到车辆与目标停车点之间的精确预估距离，将车辆与目标停车点之间的精确预估距离与当前车身速度发送给车辆决策控制系统，由车辆决策控制系统对车体进行相应控制，实现精准定点停车。

图4是本发明一实施例提供的基于雷达测距识别岸桥的停车的服务器结构的示意图。所述服务器为提供计算服务器的设备，通常指具有较高计算能力，通过网络提供给多个用户使用的计算机。如图4所示，该实施例的服务器4包括：存储器41、处理器42以及系统总线43，所述存储器41包括存储其上的可运行的程序411，本领域技术人员可以理解，图4中示出的终端设备结构并不构成对终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。

下面结合图4对终端设备的各个构成部件进行具体的介绍：

存储器41可用于存储软件程序以及模块，处理器42通过运行存储在存储器41的软件程序以及模块，从而执行终端的各种功能应用以及数据处理。存储器41可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据终端的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器41可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

在存储器41上包含基于智能指针的动态配置更新方法的可运行程序411，所述可运行程序411可以被分割成一个或多个模块/单元，所述一个或多个模块/单元被存储在所述存储器41中，并由处理器42执行，以完成通知的传递并获取通知实现过程，所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序411在所述服务器4中的执行过程。例如，所述计算机程序411可以被分割为获取模块、比对模块、拼接模块和发送模块。

处理器42是服务器的控制中心，利用各种接口和线路连接整个终端设备的各个部分，通过运行或执行存储在存储器41内的软件程序和/或模块，以及调用存储在存储器41内的数据，执行终端的各种功能和处理数据，从而对终端进行整体监控。可选的，处理器42可包括一个或多个处理单元；优选的，处理器42可集成应用处理器和调制解调处理器，其中，应用处理器主要处理操作系统、应用程序等，调制解调处理器主要处理无线通信。可以理解的是，上述调制解调处理器也可以不集成到处理器42中。

系统总线43是用来连接计算机内部各功能部件，可以传送数据信息、地址信息、控制信息，其种类可以是例如pci总线、isa总线、vesa总线等。处理器42的指令通过总线传递至存储器41，存储器41反馈数据给处理器42，系统总线43负责处理器42与存储器41之间的数据、指令交互。当然系统总线43还可以接入其他设备，例如网络接口、显示设备等。

所述服务器应至少包括cpu、芯片组、内存、磁盘系统等，其他构成部件在此不再赘述。

在本发明实施例中，该终端所包括的处理器42执行的可运行程序具体为：一种基于雷达测距识别岸桥的停车方法，所述基于雷达测距识别岸桥的停车方法包括如下步骤：

步骤1.在车辆顶部安装激光雷达，实时采集并存储激光雷达扫描得到的点云数据，进入步骤2；

步骤2.预设岸桥点云预估区域、岸桥横梁靠近端点云预估区域以及岸桥横梁本体点云预估区域，进入步骤3；

步骤3.当激光雷达在岸桥点云预估区域内检测到岸桥横梁的特征点云时，提取岸桥横梁靠近端点云预估区域内中距离车辆最近的点云的平均坐标，进入步骤4；

步骤4.以平均坐标为中心原点，提取岸桥横梁本体点云预估区域的点云，并将所提取的三维点云投影至xy二维平面，进入步骤5；

步骤5.利用最小二乘原理把投影在xy二维平面的点集拟合成直线，根据直线的斜率计算得到横梁相对车身的倾斜角，进而计算得到车辆与横梁之间的距离，进入步骤6；

步骤6.统计所有参与直线拟合、且与直线之间的距离小于预设阀值的点云数量，是否大于所有参与拟合的点云总数的预设比例，如果比较结果为小于，判断为直线拟合为异常，则直接根据上一激光雷达扫描周期得到的车辆与目标停车点之间的精确预估距离计算得到车辆与目标停车点之间的粗略预估距离d2＝d3-v*t，进入步骤9；如果比较结果为大于，判断为直线拟合为正常，则进入步骤7；

步骤7.判断上一激光雷达扫描周期得到的车辆与目标停车点之间的粗略预估距离d2是否等于车辆与目标停车点之间的精准预估距离d3的预设初始最大值nan，如果d2＝nan，则当前周期中车辆与目标停车点之间的粗略预估距离d2＝d1+offset[i]，进入步骤9；如果判断结果d2≠nan，则计算当前周期中车辆与目标停车点之间的粗略预估距离d2＝(d1+offset[i])与上一周期的目的停车点距离d2＝(d1+offset[i-1])的差值c＝(d1+offset[i])-(d1+offset[i-1])，进入步骤8；

步骤8.判断车辆与目标停车点之间的粗略预估距离d2之差c是否大于预设门限值，如果车辆与目标停车点之间的粗略预估距离d2之差c大于预设门限值，则当前周期中车辆与目标停车点之间的粗略预估距离d2＝(d1+offset[i+1])，进入步骤9；如果车辆与目标停车点之间的粗略预估距离d2之差c小于预设门限值，则当前周期中车辆与目标停车点之间的粗略预估距离d2＝(d1+offset[i])，进入步骤9；

步骤9.对车辆与目标停车点之间的粗略预估距离进行滤波优化得到车辆与目标停车点之间的精确预估距离，进入步骤10；

步骤10.将车辆与目标停车点之间的精确预估距离d3与当前车身速度发送给车辆决策控制系统，由车辆决策控制系统对车体进行相应控制，实现精准定点停车。

以上装置实施例与方法实施例是一一对应的，装置实施例简略之处，参见方法实施例即可。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能性一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应超过本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机储存器、内存、只读存储器、电可编程rom、电可檫除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其他形式的存储介质中。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。