一种基于UWB和激光雷达传感器的车辆全方位跟随方法与流程

本发明属于电子信息技术领域，特别是一种基于uwb和激光雷达传感器的车辆全方位跟随方法。

背景技术：

自主车辆是智能交通系统中的一个重要组成部分和信息化的汽车产品。自主车辆具有一定的人工智能，需要实现车载通信、信息加工处理、环境探测、辅助控制四项功能，其执行的功能包括：准确沿着规定道路行驶并保持正确的车道位置，保持车与车之间的安全距离；根据交通状况和路面特征状况调整车速，避免与其他车辆碰撞和追尾；自动实现换道超车；在道路上安全停车；车载辅助驾驶系统在智能交通网络环境下能够找到到达目的地的最佳路径，以及遇险报警和自我救助等。

在自主车辆技术的发展中，自动跟随前车行驶技术是智能交通系统走向实际应用的基础和关键。自动跟随综合运用了避障、跟随、定位、智能通信等多种技术，功能逐渐多样化，满足了多数人群的需要。目前，自动跟随技术得到了一定的发展，例如通过采集识别人体携带的二维码标签纸来定位、基于深度彩色相机的视觉跟踪系统和基于超声波定位的自动跟随技术等，但目前的产品在仪器的稳定性、准确性和效率上还有欠缺，主要体现在：

(1)避障功能并未与智能控速、方向控制等功能达到很高程度上的结合，导致避障功能并未得到完美的体现；

(2)在数据处理上未达到数据的高效率处理，不能够高效地进行路径规划，导致路径规划方面欠缺实用性；

(3)自主车辆的反应机制难以支撑密集的工作指令，无法保持工作完整性，并不能实现真正的360度自动跟随。

技术实现要素：

本发明的目的在于提供一种算法简单、定位精度高的基于uwb和激光雷达传感器的车辆全方位跟随方法，实现自主车辆在使用者周围全方位的实时跟随。

实现本发明目的的技术解决方案为：一种基于uwb和激光雷达传感器的车辆全方位跟随方法，包括以下步骤：

步骤1、建立车辆坐标系、激光雷达坐标系和uwb传感器坐标系；

步骤2、建立世界坐标系，将世界坐标系与车辆坐标系之间进行坐标映射；

步骤3、车辆实时感知自身在以使用者为原点的世界坐标系中的位置和姿态；

步骤4、使用激光雷达传感器采集障碍物信息，将障碍物在激光雷达坐标系中的坐标转换为在车辆坐标系中的坐标，使用二值贝叶斯滤波算法构建占据栅格地图；

步骤5、车辆基于占据栅格地图，以及基于并行遗传算法的自动随行与避障算法，实现任意方位的实时随行。

进一步地，步骤1所述的建立车辆坐标系、激光雷达坐标系和uwb传感器坐标系，具体如下：

uwb传感器系统的基站1位于车辆平台的右后部；基站2位于车辆平台的左后部，与基站1关于车辆的中轴对称，且基站1与基站2的距离为m；基站3位于车辆平台的左前部，与基站1关于车辆的中心对称，且基站3与基站2的距离为n；uwb传感器坐标系x3o3y3即以基站2为坐标原点，基站2到基站1的方向为x轴正方向，基站2到基站3的方向为y轴正方向的坐标系；

激光雷达传感器位于车辆平台的前部且在车辆的中轴上，激光雷达传感器与车辆中心的距离为δl；激光雷达坐标系x2o2y2以激光雷达为原点，车辆中心到激光雷达为y轴正方向，车辆坐标系x1o1y1以车辆中心为原点，车辆中心到激光雷达为y轴正方向，激光雷达坐标系x2o2y2的x轴方向与车辆坐标系的方向相同。

进一步地，步骤2所述的建立世界坐标系，将世界坐标系与车辆坐标系之间进行坐标映射，具体如下：

步骤2.1、通过三点定位，计算出各个信标在车辆坐标系中的位置，即在车辆坐标系中的(x,y)坐标，并通过消元法消除信标和车辆自身局部坐标系的高度差h；

步骤2.2、以信标1，即手持设备一端作为世界坐标系中心，信标2，即手持设备另一端方向作为x轴正方向，根据右手螺旋定则确定世界坐标系y轴的方向，以此建立世界坐标系；

步骤2.3、计算信标1与信标2在车辆坐标系下的坐标(xu10,yu10)和(xu20,yu20)，世界坐标系到车辆坐标系的位移δx,δy，根据向量夹角公式，计算坐标系之间的旋转角度α，然后根据世界坐标系与车辆坐标系之间的转换公式进行坐标映射：

x′＝xcosα+xsinα-(δxcosα+δysinα)

y′＝-xsinα+ycosα-(-δxsinα+δycosα)

其中，x′为世界坐标系中的横轴坐标，y′是世界坐标系中的纵轴坐标，δx、δy分别为位移的横、纵坐标，α为坐标系之间旋转角度。

进一步地，步骤4中所述的使用激光雷达传感器采集障碍物信息，具体如下：

激光雷达采集到的障碍物在x2o2y2坐标系中的坐标为(xb,yb)，根据几何关系将数据坐标统一至在以车辆中心为原点的x1o1y1坐标系中：

xbo1＝xb

ybo1＝yb+δd

其中，xbo1、ybo1为障碍物在车辆坐标系中的横、纵坐标，δd为x1o1y1和x2o2y2两坐标原点的相对距离。

本发明与现有技术相比，其显著优点为：(1)将uwb与激光雷达结合起来，通过坐标系之间的映射实现机器人的定姿和定位，并通过激光雷达的数据建立周围环境的占据栅格地图，建图精度高，定位效果好，能满足实际操作需求；(2)使用uwb传感器，解决了前向随行的复杂路径规划问题；(3)减少了需要处理的数据量，降低了计算难度，提高了计算效率，可以实现自主车辆在使用者周围全方位的实时跟随。

附图说明

图1是本发明基于uwb和激光雷达传感器的车辆全方位跟随方法的流程示意图。

图2是本发明中车辆坐标系示意图。

图3是本发明中四点定位示意图。

图4是本发明中障碍物相对于激光雷达位置图。

具体实施方式

本发明基于uwb和激光雷达传感器的车辆全方位跟随方法，包括以下步骤：

步骤1、建立车辆坐标系、激光雷达坐标系和uwb传感器坐标系；

步骤2、建立世界坐标系，将世界坐标系与车辆坐标系之间进行坐标映射；

步骤3、车辆实时感知自身在以使用者为原点的世界坐标系中的位置和姿态；

步骤4、使用激光雷达传感器采集障碍物信息，将障碍物在激光雷达坐标系中的坐标转换为在车辆坐标系中的坐标，使用二值贝叶斯滤波算法构建占据栅格地图；

步骤5、车辆基于占据栅格地图，以及基于并行遗传算法的自动随行与避障算法，实现任意方位的实时随行。

进一步地，步骤1所述的建立车辆坐标系、激光雷达坐标系和uwb传感器坐标系，具体如下：

uwb传感器系统的基站1位于车辆平台的右后部；基站2位于车辆平台的左后部，与基站1关于车辆的中轴对称，且基站1与基站2的距离为m；基站3位于车辆平台的左前部，与基站1关于车辆的中心对称，且基站3与基站2的距离为n；uwb传感器坐标系x3o3y3即以基站2为坐标原点，基站2到基站1的方向为x轴正方向，基站2到基站3的方向为y轴正方向的坐标系；

激光雷达传感器位于车辆平台的前部且在车辆的中轴上，激光雷达传感器与车辆中心的距离为δl；激光雷达坐标系x2o2y2以激光雷达为原点，车辆中心到激光雷达为y轴正方向，车辆坐标系x1o1y1以车辆中心为原点，车辆中心到激光雷达为y轴正方向，激光雷达坐标系x2o2y2的x轴方向与车辆坐标系的方向相同。

进一步地，步骤2所述的建立世界坐标系，将世界坐标系与车辆坐标系之间进行坐标映射，具体如下：

步骤2.1、通过三点定位，计算出各个信标在车辆坐标系中的位置，即在车辆坐标系中的(x,y)坐标，并通过消元法消除信标和车辆自身局部坐标系的高度差h；

步骤2.2、以信标1，即手持设备一端作为世界坐标系中心，信标2，即手持设备另一端方向作为x轴正方向，根据右手螺旋定则确定世界坐标系y轴的方向，以此建立世界坐标系；

步骤2.3、计算信标1与信标2在车辆坐标系下的坐标(xu10,yu10)和(xu20,yu20)，世界坐标系到车辆坐标系的位移δx,δy，根据向量夹角公式，计算坐标系之间的旋转角度α，然后根据世界坐标系与车辆坐标系之间的转换公式进行坐标映射：

x′＝xcosα+xsinα-(δxcosα+δysinα)

y′＝-xsinα+ycosα-(-δxsinα+δycosα)

其中，x′为世界坐标系中的横轴坐标，y′是世界坐标系中的纵轴坐标，δx、δy分别为位移的横、纵坐标，α为坐标系之间旋转角度。

进一步地，步骤4中所述的使用激光雷达传感器采集障碍物信息，具体如下：

激光雷达采集到的障碍物在x2o2y2坐标系中的坐标为(xb,yb)，根据几何关系将数据坐标统一至在以车辆中心为原点的x1o1y1坐标系中：

xbo1＝xb

ybo1＝yb+δd

其中，xbo1、ybo1为障碍物在车辆坐标系中的横、纵坐标，δd为x1o1y1和x2o2y2两坐标原点的相对距离。

下面结合附图和具体实施例对本发明作进一步的详细说明。

实施例

结合图1，本发明基于uwb和激光雷达传感器的车辆全方位跟随方法，包括以下步骤：

步骤1、建立车辆坐标系、激光雷达坐标系和uwb传感器坐标系；

结合图2，车辆的激光雷达坐标系与uwb传感器坐标系分别是x3o3y3，x2o2y2；

uwb传感器系统的基站1位于车辆平台的右后部；基站2位于车辆平台的左后部，与基站1关于车辆的中轴对称，且基站1与基站2的距离为m；基站3位于车辆平台的左前部，与基站1关于车辆的中心对称，且基站3与基站2的距离为n；uwb传感器系统坐标系x3o3y3即以基站2为坐标原点，基站2到基站1的方向为x轴正方向，基站2到基站3的方向为y轴正方向的坐标系。

激光雷达传感器位于车辆平台的前部且在车辆的中轴上，其与车辆中心的距离为δl，激光雷达传感器系统坐标系x2o2y2即以激光雷达(o2)为原点，车辆中心(o1)到激光雷达(o2)为y轴正方向，x轴方向与车辆坐标系的方向相同的坐标系。

步骤2、建立世界坐标系，进行坐标映射；

步骤2.1、结合图3，计算出各个信标在车辆坐标系中的位置，即在车辆坐标系中的(x,y)坐标，并通过消元法消除信标和车辆坐标系的高度差h；

设定信标相对于基站1，2，3所建立的三维坐标系的坐标点为(xu,yu,h)，可以列出如下方程组：

x1²＝h²+xu²+(yu-n)²

x3²＝h²+yu²+(xu-m)²

x2²＝xu²+yu²+h²

相减可把h消去：

x2²-x1²＝2yun-n²

x2²-x3²＝2xum-m²

步骤2.2、以信标1，即手持设备一端作为世界坐标系中心，信标2，即手持设备另一端方向作为x轴正方向，根据右手螺旋定则确定世界坐标系y轴的方向，以此建立世界坐标系；

步骤2.3、计算信标1与信标2在车辆坐标系下的坐标(xu10,yu10)和(xu20,yu20)，世界坐标系到车辆坐标系的位移δx,δy，根据向量夹角公式，计算坐标系之间的旋转角度α，然后根据世界坐标系与车辆坐标系之间的转换公式进行坐标映射：

x′＝xcosα+xsinα-(δxcosα+δysinα)

y′＝-xsinα+ycosα-(-δxsinα+δycosα)

其中，x′为世界坐标系中的横轴坐标，y′是世界坐标系中的纵轴坐标，δx，δy分别为位移的横、纵坐标，α为坐标系之间旋转角度；

最后可得信标相对于以基站为平面的车辆坐标系的坐标点：

步骤3、车辆实时感知到其在以人为原点的世界坐标系中的位置和姿态；

uwb传感器采集到数据后，将数据坐标统一至在以车辆中心为原点的车辆坐标系x1o1y1中：

即：

步骤4、使用激光雷达传感器采集障碍物信息，将障碍物在激光雷达坐标系中的坐标转换为在车辆坐标系中的坐标，使用二值贝叶斯滤波算法构建占据栅格地图；

结合图4，以激光雷达为原点建立直角坐标系，检测到障碍物相对于激光雷达中心点的距离d和相对于x轴的角度θ；

激光雷达坐标系的极坐标为：

xbo2＝f(θ,d)

ybo2＝w(θ,d)

激光雷达坐标系的直角坐标系为：

xbo2＝d*cosθ

ybo2＝d*sinθ

车辆的直角坐标系为：

xbo1＝xbo2

ybo1＝ybo2+δl

在激光雷达坐标系中，把(xbo2,ybo2)的坐标由极坐标系表示转换成直角坐标系表示，然后将ybo2加上激光雷达原点和车辆原点的距离差δl，即可得到障碍物在车辆坐标系x1o1y1中的位置为(xbo1,ybo1)。

步骤5、车辆基于占据栅格地图，以及基于并行遗传算法的自动随行与避障算法，实现任意方位的实时随行。