[发明专利]一种车挂箱状态的判断方法和系统

说明书

本发明公开了一种车挂箱状态的判断方法和系统，其中方法包括以下步骤：在车头的后侧安装ToF摄像头，来观测车挂箱的前壁；通过ToF摄像头得出车挂箱与车头之间的夹角和角速度；通过夹角和角速度，由车头的状态来判定车挂箱的状态。本发明的优点是：在车头安装感知传感器，对挂车的状态可进行实时估计判断，同时：不用改挂箱，成本相对较低，有车规级的传感器。

技术领域

本发明涉及车辆运输领域，更确切地说一种车挂箱状态的判断方法和系统。

背景技术

自动驾驶中，对车本身的状态(如位置，速度等)的估计是非常重要信息。目前有很多种车辆是铰接式(例如，半挂式卡车铰接式清扫车)，如图1所示。自动驾驶系统中，需要准确的对车头和挂箱的状态(包括，位置、线性速度和旋转速度)做出准确的估计。一般来说，自动驾驶系统，对车体定位的系统往往都装在车头上。所以，如何能准确的估计车挂箱的状态就是一个必须要解决的问题。

现有技术中，一般会有以下两种结构：

1、在车的挂箱上面安装对位设备，例如GPS+IMU，该结构存在以下缺点：

a.增加成本；

b.必须改装挂箱，安装角度设备；

c.很多运营场景无法适应，如甩挂运营(就是车头和车尾的匹配，在运营中是比较随机的)；

2、在车的铰接链接处安装角度传感器，该结构存在以下缺点：

a.必须改装挂箱，安装定位设备；

b.很多运营场景无法适应，如甩挂运营(就是车头和车尾的匹配，在运营中是比较随机的)；

c.解决方案不成熟，没有过车规的角度传感器。

发明内容

本发明的目的是提供一种车挂箱状态的判断方法和系统，其可以解决现有技术中的成本较高，需要改装挂箱的缺点。

本发明采用以下技术方案：

一种车挂箱状态的判断方法，包括以下步骤：

在车头的后侧安装ToF摄像头，来观测车挂箱的前壁；

通过ToF摄像头得出车挂箱与车头之间的夹角和角速度；

通过夹角和角速度，由车头的状态来判定车挂箱的状态。

还包括以下步骤：

在每个时刻，获得ToF摄像头在车挂箱前壁的点云图；

在每个时刻k，获得ToF摄像头在车挂箱前壁的点云pk的图；

根据这个平面对应的法向量，可以计算出车挂箱和车头的角度值；

根据当前时刻的点云pk，和上一时刻的点云pk-1，通过做点云配准算法，来计算两时刻之间的车挂箱的转动角度Δθk＝h(pk,pk-1)；瞬时角速度的测量可以通过如下计算得到，其中ΔT是两帧的时间间隔，

输出当前时刻k的车挂箱相对车头的角度和角速度的测量值。

还包括以下步骤：时间序列滤波，其为：

每个时刻k都可以得到一个挂车角度θk和挂车角速度的测量值；

对上述挂车角度θk和挂车角速度的测量值进行时间滤波。

还包括反馈步骤；

将最终输出每个时刻车挂箱相对车头的角度和角速度结果进入反馈，做鲁棒性拟合的时候，考虑上一时刻的结果。

在根据点云，鲁棒拟合一个平面的步骤中，考虑上一时刻的结果。