[发明专利]一种电厂作业车辆轨迹估计方法及系统

说明书

本发明提供了一种电厂作业车辆轨迹估计方法及系统，所述方法包括：实时获取车辆三维位姿信息；当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时，通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息；其中，所述上一时刻的车辆状态信息为通过预先构建的CTRV模型对上一时刻的车辆三维位姿信息进行转换得到；根据当前时刻的车辆状态信息对车辆空间轨迹进行更新。本发明通过将CTRV模型和卡尔曼滤波扩展到了三维形式，以对车辆轨迹进行三维估计，从而有效提高了车辆轨迹估计的准确性。

技术领域

本发明涉及车辆轨迹估计技术领域，尤其是涉及一种电厂作业车辆轨迹估计方法及系统。

背景技术

车辆安全是安全管理的重要部分，准确的估计车辆轨迹可以分析车辆行驶状态，及时发现车辆的异常状态。特别是在电场等大型工厂的日常运行过程中，大型作业车辆的安全管理至关重要，大型作业车辆的事故往往会导致严重的经济和安全损失，还可能产生恶劣的后续影响，带来不可估量的后续损失。对于电厂等大型工厂合理的预测大型作业车辆异常状态，推断大型作业车辆工作是否规范，预防大型作业车辆出现事故有重要作用，方便电厂等大型工厂进行车辆安全管理。

现有的车辆轨迹估计方法主要分为基于相邻帧关联和卡尔曼滤波的方法。相邻帧匹配是根据相邻帧检测到的目标进行关联，记录目标相应姿态得到轨迹，其中关联方法包括最近邻数据关联(NNDA)、概率数据关联(PDA)、联合概率数据关联(JPDA)和多假设跟踪(MHT)等方法。卡尔曼滤波方法通过建立卡尔曼滤波器，每一步对车辆位姿进行估计，补全未得到位姿的时间步信息，得到完整的轨迹。卡尔曼滤波器包括普通的卡尔曼滤波器、扩展卡尔曼滤波器和参数自适应卡尔曼滤波器等。

现有的车辆轨迹估计方法存在以下缺点：

1)现有的车辆轨迹估计方法都是二维轨迹估计，没有对车辆进行三维轨迹估计，难以满足车辆异常状态检测要求。

2)基于相邻帧关联的方法容易受到遮挡和缺失等数据断流情况的影响，导致同一目标轨迹中断，并且无法对车辆状态进行优化，导致方法精度较低，难以达到车辆轨迹估计的要求。

发明内容

本发明实施例旨在提供一种电厂作业车辆轨迹估计方法及系统，以解决上述技术问题，从而能够有效提高车辆轨迹估计的准确性。

为了解决上述技术问题，本发明提供了一种电厂作业车辆轨迹估计方法，包括：

实时获取车辆三维位姿信息；

当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时，通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息；其中，所述上一时刻的车辆状态信息为通过预先构建的CTRV模型对上一时刻的车辆三维位姿信息进行转换得到；

根据当前时刻的车辆状态信息对车辆空间轨迹进行更新；

所述CTRV模型用于建立车辆的运动学模型，将车辆位姿P_t转换为车辆状态S_t^-，所述车辆状态S_t^-表示为：

其中，Timestamp_t为t时刻对应的时间戳，x_t,y_t,z_t为车辆在世界坐标系的空间位置，Rx_t,Ry_t,Rz_t代表车辆的空间姿态；

基于所述车辆在世界坐标系的空间位置和车辆的空间姿态，求解所述车辆位姿P_t：