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

权利要求书

1.一种电厂作业车辆轨迹估计方法，其特征在于，包括：

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

当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时，通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息；其中，所述上一时刻的车辆状态信息为通过预先构建的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：

[x_t,y_t,z_t,Rx_t,Ry_t,Rz_t]＝[P_xt,P_yt,P_zt,R_xt,R_yt,R_zt]；

其中，v_t,φ_t,ω_t分别为t时刻的车辆速度、俯仰角和横摆角，和分别为车辆俯仰角和横摆角的角加速度；

所述通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息，具体为：

根据t-1时刻的车辆状态信息，计算得到雅克比矩阵G：

其中，

ΔT＝Timestamp_t-Timestamp_t-1；

通过t-1时刻的车辆状态和状态协方差矩阵预测t时刻的车辆状态和状态协方差矩阵

其中，为预测的状态协方差矩阵，Q为预测误差矩阵；

判断是否有t时刻的车辆状态；

当没有t时刻的车辆状态时，将t时刻的预测车辆状态作为t时刻的观测车辆状态；

通过所述卡尔曼滤波器对t时刻的预测车辆状态信息和状态协方差矩阵进行更新：

其中，H为观测矩阵，K为卡尔曼增益，R_k为测量噪声矩阵，S_t为卡尔曼滤波器更新后的车辆状态，P_t为更新的状态协方差矩阵；

并将卡尔曼滤波器更新后的车辆状态作为预测得到的当前时刻的车辆状态信息。

2.根据权利要求1所述的电厂作业车辆轨迹估计方法，其特征在于，在所述实时获取车辆三维位姿信息之后，在所述当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时之前，还包括：

当判断当前时刻的车辆三维位姿信息没有缺失且为起点位姿时，对预设的卡尔曼滤波器进行初始化。

3.根据权利要求2所述的电厂作业车辆轨迹估计方法，其特征在于，在所述当判断当前时刻的车辆三维位姿信息没有缺失且为起点位姿时之后，在所述当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时之前，还包括：

当判断当前时刻的车辆三维位姿信息没有缺失且为非起点位姿时，构建CTRV模型，并通过所述CTRV模型将所述车辆三维位姿信息转换为车辆状态信息。