[發(fā)明專利]一種多傳感器融合定位系統(tǒng)的位姿優(yōu)化方法和裝置

在審

• 申請(qǐng)?zhí)枺?/td>: 202011060481.7
• 申請(qǐng)日: 2020-09-30
• 公開(kāi)（公告）號(hào): CN114322996A
• 公開(kāi)（公告）日: 2022-04-12
• 發(fā)明（設(shè)計(jì)）人: 韓冰;張濤;邊威;黃帥
• 申請(qǐng)（專利權(quán)）人: 阿里巴巴集團(tuán)控股有限公司
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/20
• 代理公司: 北京思格頌知識(shí)產(chǎn)權(quán)代理有限公司 11635
• 代理人: 楊超

1.一種多傳感器融合定位系統(tǒng)的位姿優(yōu)化方法，包括：

將獲取到的預(yù)設(shè)時(shí)間段內(nèi)的視覺(jué)傳感器拍攝的圖像數(shù)據(jù)與慣導(dǎo)測(cè)量數(shù)據(jù)融合，得到相對(duì)世界坐標(biāo)系下的第一位姿；

基于所述第一位姿和全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)位置和觀測(cè)航向，對(duì)所述第一位姿轉(zhuǎn)換到絕對(duì)世界坐標(biāo)系下的第二位姿和相對(duì)世界坐標(biāo)系與絕對(duì)世界坐標(biāo)系間的旋轉(zhuǎn)外參進(jìn)行迭代優(yōu)化，得到所述第二位姿的優(yōu)化值和旋轉(zhuǎn)外參的優(yōu)化值，所述旋轉(zhuǎn)外參的優(yōu)化值用于下一時(shí)間段內(nèi)的視覺(jué)傳感器拍攝的圖像數(shù)據(jù)與慣導(dǎo)測(cè)量數(shù)據(jù)融合。

2.如權(quán)利要求1所述的方法，所述對(duì)所述第一位姿轉(zhuǎn)換到絕對(duì)世界坐標(biāo)系下的第二位姿和相對(duì)世界坐標(biāo)系與絕對(duì)世界坐標(biāo)系間的旋轉(zhuǎn)外參進(jìn)行迭代優(yōu)化，得到所述第二位姿的優(yōu)化值和旋轉(zhuǎn)外參的優(yōu)化值，具體包括：

迭代優(yōu)化所述第二位姿中的第二位置和第二航向角及所述旋轉(zhuǎn)外參，得到所述第二位置的優(yōu)化值、第二航向角的優(yōu)化值和旋轉(zhuǎn)外參的優(yōu)化值。

3.如權(quán)利要求1所述的方法，所述基于所述第一位姿和全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)位置和觀測(cè)航向，對(duì)所述第一位姿轉(zhuǎn)換到絕對(duì)世界坐標(biāo)系下的第二位姿和相對(duì)世界坐標(biāo)系與絕對(duì)世界坐標(biāo)系間的旋轉(zhuǎn)外參進(jìn)行迭代優(yōu)化，具體包括：

根據(jù)所述第一位姿和全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)位置和觀測(cè)航向，建立下述至少一項(xiàng)殘差，將建立的各殘差組合成目標(biāo)殘差：

所述第一位姿中的第一位置與所述第一位姿轉(zhuǎn)換到絕對(duì)世界坐標(biāo)系下的第二位姿中的第二位置的相對(duì)位置變化殘差；

所述第一位姿中的第一姿態(tài)與所述第一位姿轉(zhuǎn)換到絕對(duì)世界坐標(biāo)系下的第二位姿中的第二姿態(tài)的相對(duì)姿態(tài)變化殘差；

所述第一姿態(tài)的絕對(duì)姿態(tài)轉(zhuǎn)換殘差；

全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)位置的絕對(duì)位置殘差；

全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)航向的絕對(duì)姿態(tài)殘差。

4.如權(quán)利要求3所述的方法，所述相對(duì)位置變化殘差，通過(guò)下述步驟建立：

將相鄰兩幀圖像拍攝時(shí)刻的第一位置的差值，根據(jù)所述兩幀中的前一幀圖像拍攝時(shí)刻慣導(dǎo)坐標(biāo)系與相對(duì)世界坐標(biāo)系的旋轉(zhuǎn)外參，轉(zhuǎn)換成所述前一幀圖像拍攝時(shí)刻慣導(dǎo)坐標(biāo)系下的第一差值；

根據(jù)所述兩幀圖像拍攝時(shí)刻的待優(yōu)化的第二位姿中的第二位置的差值與所述前一幀圖像拍攝時(shí)刻第二位姿中的第二姿態(tài)，確定所述前一幀圖像拍攝時(shí)刻慣導(dǎo)坐標(biāo)系下的第二差值；

將所述第一差值與所述第二差值間的差值確定為相對(duì)位置變化殘差。

5.如權(quán)利要求3所述的方法，所述相對(duì)姿態(tài)變化殘差，通過(guò)下述步驟建立：

根據(jù)相鄰兩幀圖像拍攝時(shí)刻的待優(yōu)化的第二位姿中的第二姿態(tài)，和分別在所述兩幀圖像拍攝時(shí)刻慣導(dǎo)坐標(biāo)系與相對(duì)世界坐標(biāo)系的旋轉(zhuǎn)外參，確定相對(duì)姿態(tài)變化殘差。

6.如權(quán)利要求3所述的方法，所述第一姿態(tài)的絕對(duì)姿態(tài)變化殘差，通過(guò)下述步驟建立：

根據(jù)慣導(dǎo)坐標(biāo)系與相對(duì)世界坐標(biāo)系的旋轉(zhuǎn)外參、待優(yōu)化的第二位姿中的第二姿態(tài)和相對(duì)世界坐標(biāo)系與絕對(duì)世界坐標(biāo)系間的旋轉(zhuǎn)外參，確定所述第一姿態(tài)的絕對(duì)姿態(tài)變化殘差。

7.如權(quán)利要求3所述的方法，全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)位置的絕對(duì)位置殘差，通過(guò)下述步驟建立：

根據(jù)全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)位置、待優(yōu)化的第二位姿中的第二位置和第二姿態(tài)，及慣導(dǎo)坐標(biāo)系與絕對(duì)世界坐標(biāo)系間的旋轉(zhuǎn)外參，確定全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)位置的絕對(duì)位置殘差。

8.如權(quán)利要求3所述的方法，全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)航向的絕對(duì)姿態(tài)殘差，通過(guò)下述步驟建立：

根據(jù)全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)航向和待優(yōu)化的第二位姿中的第二姿態(tài)，確定全球定位系統(tǒng)在絕對(duì)世界坐標(biāo)系下的觀測(cè)航向的絕對(duì)姿態(tài)殘差。