[發(fā)明專(zhuān)利]一種軌跡優(yōu)化方法及裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 202010766551.4
• 申請(qǐng)日: 2020-08-03
• 公開(kāi)（公告）號(hào): CN111862163B
• 公開(kāi)（公告）日: 2021-07-23
• 發(fā)明（設(shè)計(jì)）人: 魯榮榮;楊再甫;林寶尉;王彥哲
• 申請(qǐng)（專(zhuān)利權(quán)）人: 湖北億咖通科技有限公司
• 主分類(lèi)號(hào): G06T7/246
• 分類(lèi)號(hào): G06T7/246;G06T7/73;G06T7/33;G06F16/29;G01C21/20;G01S19/42
• 代理公司: 北京柏杉松知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11413
• 代理人: 項(xiàng)京;高鶯然

1.一種軌跡優(yōu)化方法，其特征在于，所述方法包括：

獲取目標(biāo)軌跡的節(jié)點(diǎn)數(shù)據(jù)，所述節(jié)點(diǎn)數(shù)據(jù)包括:目標(biāo)軌跡中各節(jié)點(diǎn)的時(shí)間戳、各節(jié)點(diǎn)的三維點(diǎn)云數(shù)據(jù)、各節(jié)點(diǎn)的GPS數(shù)據(jù)和各節(jié)點(diǎn)的位姿數(shù)據(jù)；

按照各所述節(jié)點(diǎn)的時(shí)間戳的先后順序?qū)⒏魉龉?jié)點(diǎn)劃分到多條累計(jì)分段軌跡中，各累計(jì)分段軌跡中包含的第一個(gè)節(jié)點(diǎn)均為所述目標(biāo)軌跡中的初始節(jié)點(diǎn)，各累計(jì)分段軌跡中包含上一累計(jì)分段軌跡中的所有節(jié)點(diǎn)和相對(duì)于上一累計(jì)分段軌跡新增的節(jié)點(diǎn)，且各累計(jì)分段軌跡相對(duì)于上一累計(jì)分段軌跡中新增的節(jié)點(diǎn)中包含至少一個(gè)GPS有效節(jié)點(diǎn)；

基于各所述節(jié)點(diǎn)的位姿數(shù)據(jù)和各所述節(jié)點(diǎn)的GPS數(shù)據(jù)，依次對(duì)每個(gè)累計(jì)分段軌跡中的節(jié)點(diǎn)進(jìn)行位姿優(yōu)化，直到完成所述目標(biāo)軌跡中各節(jié)點(diǎn)的位姿優(yōu)化，將優(yōu)化后的所述目標(biāo)軌跡作為初始優(yōu)化軌跡；

基于所述初始優(yōu)化軌跡中各節(jié)點(diǎn)的三維點(diǎn)云數(shù)據(jù)、位姿數(shù)據(jù)以及所述GPS有效節(jié)點(diǎn)的GPS數(shù)據(jù)，對(duì)所述初始優(yōu)化軌跡進(jìn)行全局回環(huán)優(yōu)化，得到絕對(duì)軌跡；

所述基于各所述節(jié)點(diǎn)的位姿數(shù)據(jù)和各所述節(jié)點(diǎn)的GPS數(shù)據(jù)，依次對(duì)每個(gè)累計(jì)分段軌跡中的節(jié)點(diǎn)進(jìn)行位姿優(yōu)化的步驟，包括：

針對(duì)當(dāng)前待優(yōu)化的累計(jì)分段軌跡，確定第一GPS約束節(jié)點(diǎn)；所述第一GPS約束節(jié)點(diǎn)為當(dāng)前待優(yōu)化的累計(jì)分段軌跡相對(duì)于上一累計(jì)分段軌跡新增節(jié)點(diǎn)中GPS數(shù)據(jù)有效的節(jié)點(diǎn)；

針對(duì)每一所述第一GPS約束節(jié)點(diǎn)，基于該第一GPS約束節(jié)點(diǎn)的GPS數(shù)據(jù)和第一標(biāo)準(zhǔn)節(jié)點(diǎn)的GPS數(shù)據(jù)，計(jì)算該第一GPS約束節(jié)點(diǎn)與所述第一標(biāo)準(zhǔn)節(jié)點(diǎn)的相對(duì)位置；所述第一標(biāo)準(zhǔn)節(jié)點(diǎn)為預(yù)先確定的位于第一條累計(jì)分段軌跡且GPS數(shù)據(jù)有效的一個(gè)節(jié)點(diǎn)；

固定當(dāng)前待優(yōu)化的累計(jì)分段軌跡中包含的上一累計(jì)分段軌跡中所有節(jié)點(diǎn)的位姿，以當(dāng)前待優(yōu)化的累計(jì)分段軌跡相對(duì)于上一累計(jì)分段軌跡中新增的節(jié)點(diǎn)的位姿作為優(yōu)化變量，構(gòu)建第一最小優(yōu)化誤差函數(shù)；所述第一最小優(yōu)化誤差函數(shù)包括第一優(yōu)化誤差和第二優(yōu)化誤差；其中，所述第一優(yōu)化誤差包含：所述當(dāng)前待優(yōu)化的累計(jì)分段軌跡中各個(gè)相鄰節(jié)點(diǎn)間的相對(duì)位姿誤差，所述第二優(yōu)化誤差包含：每個(gè)所述第一GPS約束節(jié)點(diǎn)與所述第一標(biāo)準(zhǔn)節(jié)點(diǎn)間的相對(duì)位置誤差；

求解所述第一最小優(yōu)化誤差函數(shù)，確定當(dāng)前待優(yōu)化的累計(jì)分段軌跡相對(duì)于上一累計(jì)分段軌跡中新增的節(jié)點(diǎn)的優(yōu)化后位姿。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述第一最小優(yōu)化誤差函數(shù)為：

其中，E₁表示所述第一最小優(yōu)化誤差函數(shù)，ξ₁表示當(dāng)前待優(yōu)化的累計(jì)分段軌跡相對(duì)于上一累計(jì)分段軌跡中新增的節(jié)點(diǎn)的位姿的李代數(shù)，i和j表示節(jié)點(diǎn)的序號(hào)，e_ij表示節(jié)點(diǎn)i和節(jié)點(diǎn)j之間的相對(duì)位姿誤差，表示預(yù)設(shè)的第一信息矩陣，Ξ表示相鄰節(jié)點(diǎn)對(duì)和一階跳邊節(jié)點(diǎn)對(duì)的集合，e_i1表示所述第一GPS約束節(jié)點(diǎn)與所述第一標(biāo)準(zhǔn)節(jié)點(diǎn)間的相對(duì)位置誤差，U₁表示第一GPS約束節(jié)點(diǎn)的集合，表示預(yù)設(shè)的第二信息矩陣，(·)^T表示矩陣轉(zhuǎn)置；

其中，節(jié)點(diǎn)i和節(jié)點(diǎn)j之間的相對(duì)位姿誤差e_ij通過(guò)如下公式進(jìn)行計(jì)算：

其中，ΔT_ij表示根據(jù)節(jié)點(diǎn)i和節(jié)點(diǎn)j的優(yōu)化前的位姿計(jì)算出的節(jié)點(diǎn)i和節(jié)點(diǎn)j之間的相對(duì)位姿，T_i表示節(jié)點(diǎn)i的待優(yōu)化的位姿，T_j表示節(jié)點(diǎn)j的待優(yōu)化的位姿；ξ_ij表示ΔT_ij的李代數(shù)，ξ_i表示T_i的李代數(shù)，ξ_j表示T_j的李代數(shù)；(·)^∧表示李代數(shù)運(yùn)算中向量轉(zhuǎn)矩陣運(yùn)算，(·)^∨表示李代數(shù)運(yùn)算中矩陣轉(zhuǎn)向量運(yùn)算；

所述第一GPS約束節(jié)點(diǎn)i與所述第一標(biāo)準(zhǔn)節(jié)點(diǎn)間的相對(duì)位置誤差e_i1通過(guò)如下公式進(jìn)行計(jì)算：

e_i1＝T′_i1p-[x_i1,y_i1,z_i1,1]^T，p＝[0,0,0,1]^T；

其中，T′_i1表示所述第一GPS約束節(jié)點(diǎn)i與所述第一標(biāo)準(zhǔn)節(jié)點(diǎn)間的相對(duì)位姿，(x_i1,y_i1,z_i1)表示所述第一GPS約束節(jié)點(diǎn)i與所述第一標(biāo)準(zhǔn)節(jié)點(diǎn)間的相對(duì)位置的空間坐標(biāo)。