一种电厂作业车辆轨迹估计方法及系统与流程

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


背景技术：

2.车辆安全是安全管理的重要部分，准确的估计车辆轨迹可以分析车辆行驶状态，及时发现车辆的异常状态。特别是在电场等大型工厂的日常运行过程中，大型作业车辆的安全管理至关重要，大型作业车辆的事故往往会导致严重的经济和安全损失，还可能产生恶劣的后续影响，带来不可估量的后续损失。对于电厂等大型工厂合理的预测大型作业车辆异常状态，推断大型作业车辆工作是否规范，预防大型作业车辆出现事故有重要作用，方便电厂等大型工厂进行车辆安全管理。
3.现有的车辆轨迹估计方法主要分为基于相邻帧关联和卡尔曼滤波的方法。相邻帧匹配是根据相邻帧检测到的目标进行关联，记录目标相应姿态得到轨迹，其中关联方法包括最近邻数据关联(nnda)、概率数据关联(pda)、联合概率数据关联(jpda)和多假设跟踪(mht)等方法。卡尔曼滤波方法通过建立卡尔曼滤波器，每一步对车辆位姿进行估计，补全未得到位姿的时间步信息，得到完整的轨迹。卡尔曼滤波器包括普通的卡尔曼滤波器、扩展卡尔曼滤波器和参数自适应卡尔曼滤波器等。
4.现有的车辆轨迹估计方法存在以下缺点：
5.1)现有的车辆轨迹估计方法都是二维轨迹估计，没有对车辆进行三维轨迹估计，难以满足车辆异常状态检测要求。
6.2)基于相邻帧关联的方法容易受到遮挡和缺失等数据断流情况的影响，导致同一目标轨迹中断，并且无法对车辆状态进行优化，导致方法精度较低，难以达到车辆轨迹估计的要求。


技术实现要素：

7.本发明实施例旨在提供一种电厂作业车辆轨迹估计方法及系统，以解决上述技术问题，从而能够有效提高车辆轨迹估计的准确性。
8.为了解决上述技术问题，本发明提供了一种电厂作业车辆轨迹估计方法，包括：
9.实时获取车辆三维位姿信息；
10.当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时，通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息；其中，所述上一时刻的车辆状态信息为通过预先构建的ctrv模型对上一时刻的车辆三维位姿信息进行转换得到；
11.根据当前时刻的车辆状态信息对车辆空间轨迹进行更新。
12.进一步地，所述通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息，具体为：
13.根据所述上一时刻的车辆状态信息以及预先存储的状态协方差矩阵进行计算，得到当前时刻的预测车辆状态信息和预测状态协方差矩阵；
14.利用预设的卡尔曼滤波器对所述预测车辆状态信息进行优化得到所述当前时刻的车辆状态信息，并对所述预测状态协方差矩阵进行更新并存储。
15.进一步地，在所述实时获取车辆三维位姿信息之后，在所述当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时之前，还包括：
16.当判断当前时刻的车辆三维位姿信息没有缺失且为起点位姿时，对预设的卡尔曼滤波器进行初始化。
17.进一步地，在所述当判断当前时刻的车辆三维位姿信息没有缺失且为起点位姿时之后，在所述当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时之前，还包括：
18.当判断当前时刻的车辆三维位姿信息没有缺失且为非起点位姿时，构建ctrv模型，并通过所述ctrv模型将所述车辆三维位姿信息转换为车辆状态信息。
19.为了解决相同的技术问题，本发明还提供了一种电厂作业车辆轨迹估计系统，包括：
20.信息获取模块，用于实时获取车辆三维位姿信息；
21.位姿预测模块，用于当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时，通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息；其中，所述上一时刻的车辆状态信息为通过预先构建的ctrv模型对上一时刻的车辆三维位姿信息进行转换得到；
22.轨迹更新模块，用于根据当前时刻的车辆状态信息对车辆空间轨迹进行更新。
23.进一步地，所述位姿预测模块具体包括：
24.车辆状态预测单元，用于根据所述上一时刻的车辆状态信息以及预先存储的状态协方差矩阵进行计算，得到当前时刻的预测车辆状态信息和预测状态协方差矩阵；
25.车辆状态优化单元，用于利用预设的卡尔曼滤波器对所述预测车辆状态信息进行优化得到所述当前时刻的车辆状态信息，并对所述预测状态协方差矩阵进行更新并存储。
26.进一步地，所述的电厂作业车辆轨迹估计系统还包括初始化模块，用于当判断当前时刻的车辆三维位姿信息没有缺失且为起点位姿时，对预设的卡尔曼滤波器进行初始化。
27.进一步地，所述的电厂作业车辆轨迹估计系统还包括信息转换模块，用于当判断当前时刻的车辆三维位姿信息没有缺失且为非起点位姿时，构建ctrv模型，并通过所述ctrv模型将所述车辆三维位姿信息转换为车辆状态信息。
28.与现有技术相比，本发明具有如下有益效果：
29.本发明实施例提供了一种电厂作业车辆轨迹估计方法及系统，所述方法包括：实时获取车辆三维位姿信息；当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时，通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息；其中，所述上一时刻的车辆状态信息为通过预先构建的ctrv模型对上一时刻的车辆三维位姿信息进行转换得到；根据当前时刻的车辆状态信息对车辆空间轨迹进行更新。本发明通过将ctrv模型和卡尔曼滤波扩展到了三维形式，以对车辆轨迹进行三维估计，从而有效提高了车辆轨迹估计的准确性。
附图说明
30.图1是本发明一实施例提供的电厂作业车辆轨迹估计方法的流程示意图；
31.图2是本发明一实施例提供的电厂作业车辆轨迹估计方法的另一流程示意图；
32.图3是本发明一实施例提供的ctrv模型示意图；
33.图4是本发明一实施例提供的车辆轨迹预测步骤的流程示意图；
34.图5是本发明一实施例提供的电厂作业车辆轨迹估计方法系统的结构示意图。
具体实施方式
35.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
36.请参见图1，本发明实施例提供了一种电厂作业车辆轨迹估计方法，包括步骤：
37.s101、实时获取车辆三维位姿信息。
38.s102、当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时，通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息；其中，所述上一时刻的车辆状态信息为通过预先构建的ctrv模型对上一时刻的车辆三维位姿信息进行转换得到。
39.在本发明实施例中，进一步地，所述通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息，具体为：
40.根据所述上一时刻的车辆状态信息以及预先存储的状态协方差矩阵进行计算，得到当前时刻的预测车辆状态信息和预测状态协方差矩阵；
41.利用预设的卡尔曼滤波器对所述预测车辆状态信息进行优化得到所述当前时刻的车辆状态信息，并对所述预测状态协方差矩阵进行更新并存储。
42.s103、根据当前时刻的车辆状态信息对车辆空间轨迹进行更新。
43.进一步地，在所述实时获取车辆三维位姿信息之后，在所述当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时之前，还包括：
44.s1011、当判断当前时刻的车辆三维位姿信息没有缺失且为起点位姿时，对预设的卡尔曼滤波器进行初始化。
45.进一步地，在所述当判断当前时刻的车辆三维位姿信息没有缺失且为起点位姿时之后，在所述当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时之前，还包括：
46.s1012、当判断当前时刻的车辆三维位姿信息没有缺失且为非起点位姿时，构建ctrv模型，并通过所述ctrv模型将所述车辆三维位姿信息转换为车辆状态信息。
47.基于上述方案，为便于更好的理解本发明实施例提供的电厂作业车辆轨迹估计方法，以下进行详细说明：
48.首先为了方便说明，先对车辆轨迹测量问题进行定义，车辆轨迹测量(估计)就是当车辆出现遮挡或者数据断流时，通过前段时间步的车辆位姿数据预测缺失时间步的车辆位姿，补全车辆位姿信息，形成车辆轨迹。
49.请参见图2，本发明实施例针对三维车辆位姿进行三维车辆轨迹测量的步骤如下：
50.s1：输入三维车辆位姿：
51.p
t
＝p
xt
,p
yt
,p
zt
,r
xt
,r
yt,zt
]
52.s.t.||[r
xt
,r
yt
,r
zt
]||2＝1
[0053]
其中，[p
xt
,p
yt
,p
zt
]是空间位置信息，表示t时刻车辆中心在世界坐标系三个坐标轴上的位移量；[r
xt
,r
yt
,r
zt
]是空间姿态信息，表示t时刻车辆纵轴在世界坐标系下的空间朝向的单位向量。
[0054]
s2：判断输入的三维车辆位姿是否有缺陷，如果是进入s3的判断，否则进入s4的判断。
[0055]
s3：判断有缺失的三维车辆位姿是否是起点位姿，如果是，由于起点位姿需要明确，重新输入下一时间步的位姿；否则进行卡尔曼滤波器预测和更新。
[0056]
s4：判断没有缺失的三维车辆位姿是否是起点位姿，如果是则进行卡尔曼滤波器初始化；否则进行ctrv(恒定转弯率和速度幅度模型)建模。
[0057]
s5：ctrv建模，根据ctrv模型建立车辆的运动学模型，将车辆位姿p
t
转换得到车辆状态s
t
‑
。ctrv模型的示意图如图3所示，车辆状态s
t
‑
表示为：
[0058][0059]
其中timestamp
t
为t时刻对应的时间戳，即系统绝对时间，在t时刻，x
t
,y
t
,z
t
代表此时车辆的在世界坐标系的空间位置，rx
t
,ry
t
,rz
t
代表车辆的空间姿态，基于前述位姿估计算法求解的状态p
t
可得：
[0060]
[x
t
,y
t
,z
t
,rx
t
,ry
t
,rz
t
]＝[p
xt
,p
yt
,p
zt
,r
xt
,r
yt
,r
zt
]，
[0061]
v
t
,φ
t
,ω
t
分别为t时刻的车辆速度(标量)、俯仰角和横摆角，如图3所示。和分别为车辆俯仰角和横摆角的角加速度。
[0062]
上述参数的具体计算公式为：
[0063]
δt＝timestamp
t
‑
timestamp
t
‑1[0064][0065][0066][0067][0068][0069][0070]
得到车辆在时刻的状态观测值作为卡尔曼滤波器的输入。
[0071]
s6：如果车辆位姿是第一次无缺失的车辆位姿，对卡尔曼滤波器进行初始化，即记录下该时刻的车辆状态。
[0072]
s7：卡尔曼滤波器预测和更新，根据上一时刻观测值和ctrv模型对该时刻的车辆状态进行预测并更新车辆状态和状态协方差矩阵。
[0073]
s8：将更新后的车辆状态输出，并输出到s7步骤，作为下一时间步的输入。
[0074]
s9：车辆空间轨迹更新，记录下输出的车辆状态。
[0075]
请参见图4，上述步骤s7的具体过程如下：
[0076]
s7.1：将t
‑
1时刻的车辆状态作为输入。
[0077]
s7.2：根据t
‑
1时刻的车辆状态计算雅可比矩阵，具体计算公式如下所示：
[0078][0079]
其中每一项分别为g(
·
)对s
t
‑1的偏导数，具体如下：
[0080]
δt＝timestamp
t
‑
timestamp
t
‑1[0081][0082][0083][0084][0085]
[0086][0087][0088][0089][0090][0091][0092][0093][0094][0095][0096]
得到雅可比矩阵g。
[0097]
s7.3：通过t
‑
1时刻的车辆状态和状态协方差矩阵预测t时刻的车辆状态和状态协方差矩阵，具体预测公式如下所示：
[0098]
[0099][0100][0101]
其中为预测的状态协方差矩阵，g为s7.2计算得到的雅可比矩阵，q为预测误差矩阵，根据实际应用情况的方差给出。
[0102]
得到t时刻的预测车辆状态和预测状态协方差矩阵
[0103]
s7.4：判断是否有t时刻的车辆状态，即t时刻车辆是否被遮挡。
[0104]
s7.5：如果没有t时刻的车辆状态，则将t时刻的预测车辆状态作为t时刻的观测车辆状态s
t
。
[0105]
s7.6：更新车辆状态和状态协方差矩阵，优化t时刻的车辆状态，具体公式如下：
[0106][0107][0108][0109][0110]
其中h为观测矩阵，一般h为单位矩阵。k为卡尔曼增益，一定程度上代表着预测值与测量值之间的置信度比例，r
k
为测量噪声矩阵，需要根据应用情况自己提供，s
t
为卡尔曼滤波器更新后的车辆状态，p
t
为更新的状态协方差矩阵。
[0111]
将更新后的s
t
作为车辆状态输出。
[0112]
与现有技术相比，本发明实施例针对现有车辆轨迹估计技术局限于二维空间，导致轨迹估计精度不高，难以满足车辆异常状态检测要求的问题，将ctrv模型和扩展卡尔曼滤波扩展到了三维形式，可以对车辆轨迹进行三维估计，提高了车辆轨迹估计的准确性，同时车辆三维轨迹估计为车辆异常状态检测提供了更多信息。本发明针对现有部分车辆轨迹估计技术难以应对遮挡、状态缺失的问题，使用扩展卡尔曼滤波，对车辆状态缺失部分进行预测，提高了算法的鲁棒性。
[0113]
需要说明的是，对于以上方法或流程实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明实施例并不受所描述的动作顺序的限制，因为依据本发明实施例，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于可选实施例，所涉及的动作并不一定是本发明实施例所必须的。
[0114]
请参见图5，为了解决相同的技术问题，本发明还提供了一种电厂作业车辆轨迹估计系统，包括：
[0115]
信息获取模块1，用于实时获取车辆三维位姿信息；
[0116]
位姿预测模块2，用于当判断当前时刻的车辆三维位姿信息有缺失且为非起点位姿时，通过预设的卡尔曼滤波器根据上一时刻的车辆状态信息进行预测得到当前时刻的车辆状态信息；其中，所述上一时刻的车辆状态信息为通过预先构建的ctrv模型对上一时刻的车辆三维位姿信息进行转换得到；
[0117]
轨迹更新模块3，用于根据当前时刻的车辆状态信息对车辆空间轨迹进行更新。
[0118]
进一步地，所述位姿预测模块2具体包括：
[0119]
车辆状态预测单元，用于根据所述上一时刻的车辆状态信息以及预先存储的状态协方差矩阵进行计算，得到当前时刻的预测车辆状态信息和预测状态协方差矩阵；
[0120]
车辆状态优化单元，用于利用预设的卡尔曼滤波器对所述预测车辆状态信息进行优化得到所述当前时刻的车辆状态信息，并对所述预测状态协方差矩阵进行更新并存储。
[0121]
进一步地，所述的电厂作业车辆轨迹估计系统还包括初始化模块，用于当判断当前时刻的车辆三维位姿信息没有缺失且为起点位姿时，对预设的卡尔曼滤波器进行初始化。
[0122]
进一步地，所述的电厂作业车辆轨迹估计系统还包括信息转换模块，用于当判断当前时刻的车辆三维位姿信息没有缺失且为非起点位姿时，构建ctrv模型，并通过所述ctrv模型将所述车辆三维位姿信息转换为车辆状态信息。
[0123]
可以理解的是上述系统项实施例，是与本发明方法项实施例相对应的，本发明实施例提供的一种电厂作业车辆轨迹估计方法系统，可以实现本发明任意一项方法项实施例提供的电厂作业车辆轨迹估计方法。
[0124]
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。