[發明專利]一種軌跡優化方法及裝置

有效

• 申請號: 202010766551.4
• 申請日: 2020-08-03
• 公開（公告）號: CN111862163B
• 公開（公告）日: 2021-07-23
• 發明（設計）人: 魯榮榮;楊再甫;林寶尉;王彥哲
• 申請（專利權）人: 湖北億咖通科技有限公司
• 主分類號: G06T7/246
• 分類號: G06T7/246;G06T7/73;G06T7/33;G06F16/29;G01C21/20;G01S19/42
• 代理公司: 北京柏杉松知識產權代理事務所(普通合伙) 11413
• 代理人: 項京;高鶯然

本發明實施例提供了一種軌跡優化方法及裝置，獲取目標軌跡的節點數據，節點數據包括:目標軌跡中各節點的時間戳、各節點的三維點云數據、各節點的GPS數據和各節點的位姿數據；按照各節點的時間戳的先后順序將各節點劃分到多條累計分段軌跡中；基于各節點的位姿數據和各節點的GPS數據，依次對每個累計分段軌跡中的節點進行位姿優化；基于初始優化軌跡中各節點的三維點云數據、位姿數據以及GPS有效節點的GPS數據，對初始優化軌跡進行全局回環優化，得到絕對軌跡?？梢?，以節點的GPS數據為約束，依次優化各個累計分段中節點的位姿，可逐步優化整條軌跡，再進行全局回環優化，能夠得到可直接用于構建高精度地圖的絕對軌跡。

技術領域

本發明涉及定位導航技術領域，特別是涉及一種軌跡優化方法及裝置。

背景技術

即時定位與建圖(Simultaneous Localization and Mapping,SLAM)技術作為一種前沿技術被廣泛應用于自動構建大規模高精度地圖。相比于視覺SLAM系統，利用多線激光雷達作為主要傳感器的激光SLAM系統在測距精度和感知范圍上都有明顯的優勢。

激光SLAM系統分為前端里程計與后端優化模塊，其中前端里程計主要通過三維點云配準技術估計相鄰幀的相對位姿變換關系，后端則利用回環檢測對整個軌跡進行優化。

然而，通過現有的SLAM技術得到的軌跡只是一條相對軌跡，換言之，采用該軌跡構建出的地圖與真實世界存在一個未知的剛體變換，因此不能直接用于構建高精度地圖。

發明內容

本發明實施例的目的在于提供一種軌跡優化方法及裝置，以實現生成可直接用于構建高精度地圖的絕對軌跡。具體技術方案如下：

為實現上述目的，本發明實施例提供了一種軌跡優化方法，所述方法包括：

獲取目標軌跡的節點數據，所述節點數據包括:目標軌跡中各節點的時間戳、各節點的三維點云數據、各節點的GPS數據和各節點的位姿數據；

按照各所述節點的時間戳的先后順序將各所述節點劃分到多條累計分段軌跡中，各累計分段軌跡中包含的第一個節點均為所述目標軌跡中的初始節點，各累計分段軌跡中包含上一累計分段軌跡中的所有節點和相對于上一累計分段軌跡新增的節點，且各累計分段軌跡相對于上一累計分段軌跡中新增的節點中包含至少一個GPS有效節點；

基于各所述節點的位姿數據和各所述節點的GPS數據，依次對每個累計分段軌跡中的節點進行位姿優化，直到完成所述目標軌跡中各節點的位姿優化，將優化后的所述目標軌跡作為初始優化軌跡；

基于所述初始優化軌跡中各節點的三維點云數據、位姿數據以及所述GPS有效節點的GPS數據，對所述初始優化軌跡進行全局回環優化，得到絕對軌跡。

可選的，所述基于各所述節點的位姿數據和各所述節點的GPS數據，依次對每個累計分段軌跡中的節點進行位姿優化的步驟，包括：

針對當前待優化的累計分段軌跡，確定第一GPS約束節點；所述第一GPS約束節點為當前待優化的累計分段軌跡相對于上一累計分段軌跡新增節點中GPS數據有效的節點；

針對每一所述第一GPS約束節點，基于該第一GPS約束節點的GPS數據和所述第一標準節點的GPS數據，計算該第一GPS約束節點與所述第一標準節點的相對位置；所述第一標準節點為預先確定的位于第一條累計分段軌跡且GPS數據有效的一個節點；

固定當前待優化的累計分段軌跡中包含的上一累計分段軌跡中所有節點的位姿，以當前待優化的累計分段軌跡相對于上一累計分段軌跡中新增的節點的位姿作為優化變量，構建第一最小優化誤差函數；所述第一最小優化誤差函數包括第一優化誤差和第二優化誤差；其中，所述第一優化誤差包含：所述當前待優化的累計分段軌跡中各個相鄰節點間的相對位姿誤差，所述第二優化誤差包含：每個所述第一GPS約束節點與所述第一標準節點間的相對位置誤差；