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

权利要求书

1.一种车挂箱状态的判断方法，其特征在于，包括以下步骤：

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

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

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

还包括以下步骤：

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

根据点云，鲁棒拟合一个平面，根据这个平面对应的法向量，可以计算出车挂箱和车头的角度值；

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

2.根据权利要求1所述的车挂箱状态的判断方法，其特征在于，还包括以下步骤：时间序列滤波，其为：

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

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

3.根据权利要求2所述的车挂箱状态的判断方法，其特征在于，还包括反馈步骤；

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

4.根据权利要求2所述的车挂箱状态的判断方法，其特征在于，在根据点云，鲁棒拟合一个平面的步骤中，考虑上一时刻的结果。

5.一种车挂箱状态的判断系统,其特征在于，包括：

ToF摄像头，其安装在车头的后侧，来观测车挂箱的前壁，通过ToF摄像头得出车挂箱与车头之间的夹角和角速度；

判定模块，其通过夹角和角速度，由车头的状态来判定车挂箱的状态；

还包括：

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

鲁棒拟合模块，其根据点云，鲁棒拟合一个平面，根据这个平面对应的法向量，可以计算出车挂箱和车头的角度值；

转动角度计算模块，其根据当前时刻的点云pk，和上一时刻的点云pk-1，通过做点云配准算法，来计算两时刻之间的车挂箱的转动角度Δθk＝h(pk,pk-1)；

瞬时角速度计算模块，其可以通过如下计算得到，其中ΔT是两帧的时间间隔，

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

6.根据权利要求5所述的车挂箱状态的判断系统,其特征在于，时间序列滤波模块，其在每个时刻k都可以得到一个挂车角度θk和挂车角速度的测量值；对上述挂车角度θk和挂车角速度的测量值进行时间滤波。

7.根据权利要求5所述的车挂箱状态的判断系统,其特征在于，反馈模块，其将最终输出每个时刻车挂箱相对车头的角度和角速度结果进入反馈，做鲁棒性拟合的时候，考虑上一时刻的结果。

8.根据权利要求5所述的车挂箱状态的判断系统,其特征在于，在根据点云，鲁棒拟合一个平面的步骤中，考虑上一时刻的结果。