[发明专利]一种基于UWB和激光雷达传感器的车辆全方位跟随方法

权利要求书

1.一种基于UWB和激光雷达传感器的车辆全方位跟随方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

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

激光雷达传感器位于车辆平台的前部且在车辆的中轴上，激光雷达传感器与车辆中心的距离为ΔL；激光雷达坐标系X₂O₂Y₂以激光雷达为原点，车辆中心到激光雷达为y轴正方向，车辆坐标系X₁O₁Y₁以车辆中心为原点，车辆中心到激光雷达为y轴正方向，激光雷达坐标系X₂O₂Y₂的x轴方向与车辆坐标系的方向相同。

2.根据权利要求1所述的基于UWB和激光雷达传感器的车辆全方位跟随方法，其特征在于，步骤2所述的建立世界坐标系，将世界坐标系与车辆坐标系之间进行坐标映射，具体如下：

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

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

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

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

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

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

3.根据权利要求1所述的基于UWB和激光雷达传感器的车辆全方位跟随方法，其特征在于，步骤4中所述的使用激光雷达传感器采集障碍物信息，具体如下：

激光雷达采集到的障碍物在X₂O₂Y₂坐标系中的坐标为(x_b,y_b)，根据几何关系将数据坐标统一至在以车辆中心为原点的X₁O₁Y₁坐标系中：

x_bo1＝x_b

y_bo1＝y_b+Δd

其中，x_bo1、y_bo1为障碍物在车辆坐标系中的横、纵坐标，Δd为X₁O₁Y₁和X₂O₂Y₂两坐标原点的相对距离。