[發(fā)明專利]一種高精度軌跡運算方法及系統(tǒng)

在審

• 申請?zhí)枺?/td>: 201710731072.7
• 申請日: 2017-08-23
• 公開（公告）號: CN107577646A
• 公開（公告）日: 2018-01-12
• 發(fā)明（設(shè)計）人: 劉太臣;張旭東;於路;王順利;駱柏翰;楊佳樂;吳正宇;姚娟
• 申請（專利權(quán)）人: 上海莫斐信息技術(shù)有限公司
• 主分類號: G06F17/18
• 分類號: G06F17/18;G01S19/37
• 代理公司: 北京卓唐知識產(chǎn)權(quán)代理有限公司11541
• 代理人: 唐海力,韓來兵

1.一種高精度軌跡運算方法，其特征在于，包括以下步驟：

獲得通過時間戳標記的GPS軌跡和點云數(shù)據(jù)；

根據(jù)所述點云數(shù)據(jù)進行位姿估計，得到SLAM軌跡和點云地圖；

對所述GPS軌跡進行坐標系的變換，同時將所述SLAM軌跡與變換后的GPS軌跡對齊后，通過帶權(quán)重的ICP算法進行匹配并校正；

根據(jù)校正結(jié)果在所述點云地圖中進行軌跡坐標值的計算，并對所述軌跡坐標值進行坐標系的逆變換，得到高精度軌跡。

2.根據(jù)權(quán)利要求1所述的高精度軌跡運算方法，其特征在于，所述SLAM軌跡的獲得包括以下步驟：

定義三維空間R³中分別含有M、N個坐標點的兩組點集為P₁和P₂；其中，P₁＝{P₁₁，P₁₂，P₁₃……P_1M}，P₂＝{P₂₁，P₂₂，P₂₃……P_2N}；

對點集P₁中的坐標點進行旋轉(zhuǎn)后平移，得到P₂中相對應(yīng)的坐標點；則單點變換有P_2i＝R P_1j+T；其中，R是角度旋轉(zhuǎn)矩陣，T是平移矩陣；

求解兩組所述點集P₁、P₂中坐標點的最優(yōu)化問題，得到SLAM軌跡；

argminR,TΣi=1N||P2i-(RP1j+T)||2]]>

3.根據(jù)權(quán)利要求1所述的高精度軌跡運算方法，其特征在于，所述坐標系的變換包括以下步驟：

根據(jù)GPS軌跡坐標值和預(yù)先的定義，求得該坐標值處的帶數(shù)N以及中央經(jīng)線L₀；

N＝[L/3+0.5]，其中，L表示測得的經(jīng)度值；L₀＝3N；

通過UTM投影將處于地心坐標系下的所述GPS軌跡坐標值變換至局部空間坐標系下的局部空間坐標值(x，y)；

x＝k₀(M+NtanB(A²/2+(5-T+9C+4C²)*A⁴/24)+(61-58T+T²+600C-330e^‘2)*A⁶/720)，

y＝k₀N(A+(1-T+C)*A³/6+(5-18T+T²+72C-58e^‘2)*A⁵/120)，

其中，A表示經(jīng)度值，B表示緯度值，a表示地球長半軸，b表示地球短半軸，k₀＝0.9996，T＝tan²B，A＝(L-L₀)cosB，C＝e^‘2cos²B，