[發明專利]一種電廠作業車輛軌跡估計方法及系統

有效

• 申請號: 202110682787.4
• 申請日: 2021-06-18
• 公開（公告）號: CN113610263B
• 公開（公告）日: 2023-06-09
• 發明（設計）人: 鄒祥波;秦士偉;饒睦敏;葉驥;王群
• 申請（專利權）人: 廣東能源集團科學技術研究院有限公司
• 主分類號: G06F17/10
• 分類號: G06F17/10;G06F17/16;G06Q50/06
• 代理公司: 廣州三環專利商標代理有限公司 44202
• 代理人: 郭浩輝;顏希文

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模型將所述車輛三維位姿信息轉換為車輛狀態信息。