[發(fā)明專利]一種對(duì)相對(duì)精度地圖進(jìn)行優(yōu)化的方法、設(shè)備及存儲(chǔ)介質(zhì)

有效

• 申請(qǐng)?zhí)枺?/td>: 201810312477.1
• 申請(qǐng)日: 2018-04-09
• 公開（公告）號(hào): CN108520543B
• 公開（公告）日: 2022-08-09
• 發(fā)明（設(shè)計(jì)）人: 王成;叢林;劉海偉
• 申請(qǐng)（專利權(quán)）人: 杭州易現(xiàn)先進(jìn)科技有限公司
• 主分類號(hào): G06T11/20
• 分類號(hào): G06T11/20;G01C21/00;G01S19/42
• 代理公司: 北京市中倫律師事務(wù)所 11410
• 代理人: 楊黎峰;鐘錦舜

1.一種對(duì)相對(duì)精度地圖進(jìn)行優(yōu)化的方法，該方法包括：

對(duì)特定環(huán)境區(qū)域中的至少一個(gè)關(guān)鍵地標(biāo)進(jìn)行絕對(duì)精度測(cè)量，得到至少一個(gè)關(guān)鍵地標(biāo)的位姿；

構(gòu)建所述特定環(huán)境區(qū)域的相對(duì)精度地圖，并建立所述相對(duì)精度地圖的位姿圖模型，所述位姿圖模型的起點(diǎn)和終點(diǎn)不重合；

利用所述至少一個(gè)關(guān)鍵地標(biāo)的位姿對(duì)所述位姿圖模型進(jìn)行優(yōu)化；

其中，利用所述至少一個(gè)關(guān)鍵地標(biāo)的位姿對(duì)所述位姿圖模型進(jìn)行優(yōu)化至少包括如下操作：第一操作，利用特定匹配算法在所述位姿圖模型中搜索與所述至少一個(gè)關(guān)鍵地標(biāo)的位姿相對(duì)應(yīng)的節(jié)點(diǎn)的位姿；第二操作，基于所述至少一個(gè)關(guān)鍵地標(biāo)的位姿與相對(duì)應(yīng)的節(jié)點(diǎn)的位姿之間的變換關(guān)系，對(duì)所述位姿圖模型進(jìn)行優(yōu)化；

其中，利用所述至少一個(gè)關(guān)鍵地標(biāo)的位姿對(duì)所述位姿圖模型進(jìn)行優(yōu)化，還包括：在第二操作對(duì)所述位姿圖模型進(jìn)行優(yōu)化之后，增大所述至少一個(gè)關(guān)鍵地標(biāo)的位姿和相對(duì)應(yīng)的節(jié)點(diǎn)的位姿的分辨率；重復(fù)進(jìn)行第一操作和第二操作，以利用所述至少一個(gè)關(guān)鍵地標(biāo)的位姿對(duì)所述優(yōu)化后的位姿圖模型進(jìn)行迭代優(yōu)化。

2.根據(jù)權(quán)利要求1所述的方法，其中，利用所述至少一個(gè)關(guān)鍵地標(biāo)的位姿對(duì)所述位姿圖模型進(jìn)行優(yōu)化包括：

檢測(cè)所述位姿圖模型的中間節(jié)點(diǎn)是否形成閉環(huán)；

若檢測(cè)到所述位姿圖模型的中間節(jié)點(diǎn)形成閉環(huán)，則基于形成閉環(huán)的中間節(jié)點(diǎn)對(duì)所述相對(duì)精度地圖的位姿圖模型進(jìn)行預(yù)優(yōu)化。

3.根據(jù)權(quán)利要求1所述的方法，其中，所述特定匹配算法包括迭代最近點(diǎn)(ICP)或射影變換n個(gè)點(diǎn)(PnP)。

4.根據(jù)權(quán)利要求1所述的方法，其中，基于所述至少一個(gè)關(guān)鍵地標(biāo)的位姿與相對(duì)應(yīng)的節(jié)點(diǎn)的位姿之間的變換關(guān)系，對(duì)所述位姿圖模型進(jìn)行優(yōu)化包括：

建立所述至少一個(gè)關(guān)鍵地標(biāo)的位姿與相對(duì)應(yīng)的節(jié)點(diǎn)的位姿之間的變換關(guān)系，并將所建立的所述至少一個(gè)關(guān)鍵地標(biāo)的位姿與相對(duì)應(yīng)的節(jié)點(diǎn)的位姿之間的變換關(guān)系作為新邊；

構(gòu)建所述位姿圖模型中相鄰節(jié)點(diǎn)之間的變換關(guān)系，并將所述相鄰節(jié)點(diǎn)之間的變換關(guān)系作為原邊；

通過特定圖模型優(yōu)化算法對(duì)由所述新邊和原邊組合形成的位姿圖模型進(jìn)行優(yōu)化。

5.根據(jù)權(quán)利要求4所述的方法，其中，所述特定圖模型優(yōu)化算法包括最小二乘法或列文伯格算法(LM)。

6.根據(jù)權(quán)利要求1所述的方法，其中，以分辨率翻倍的方式增大所述至少一個(gè)關(guān)鍵地標(biāo)的位姿和相對(duì)應(yīng)的節(jié)點(diǎn)的位姿的分辨率。

7.根據(jù)權(quán)利要求1所述的方法，其中，所述第一操作還包括：確定搜索閾值；檢測(cè)所述搜索閾值是否大于預(yù)定閾值；若檢測(cè)到所述搜索閾值大于預(yù)定閾值，則在所述位姿圖模型中以所述搜索閾值對(duì)應(yīng)搜索范圍來搜索與所述至少一個(gè)關(guān)鍵地標(biāo)的位姿相對(duì)應(yīng)的節(jié)點(diǎn)的位姿。

8.根據(jù)權(quán)利要求7所述的方法，其中，所述方法進(jìn)一步包括：確定所述迭代優(yōu)化過程中上一輪的優(yōu)化幅度；根據(jù)所述優(yōu)化幅度的大小來動(dòng)態(tài)調(diào)整所述搜索閾值。

9.根據(jù)權(quán)利要求8所述的方法，其中，根據(jù)所述優(yōu)化幅度的大小來動(dòng)態(tài)調(diào)整所述搜索閾值包括：若上一輪的優(yōu)化幅度大于上一輪優(yōu)化時(shí)的特定幅度閾值，則以第一調(diào)整幅度減少下一輪優(yōu)化時(shí)的搜索閾值；若上一輪的優(yōu)化幅度不大于上一輪優(yōu)化時(shí)的特定幅度閾值，則以第二調(diào)整幅度減少或不調(diào)整下一輪優(yōu)化時(shí)的搜索閾值；其中，所述第一調(diào)整幅度的取值大于所述第二調(diào)整幅度的取值。

10.根據(jù)權(quán)利要求7所述的方法，其中，所述方法還包括：當(dāng)檢測(cè)到所述搜索閾值小于預(yù)定閾值時(shí)，停止迭代優(yōu)化并輸出優(yōu)化后的位姿圖模型。

11.根據(jù)權(quán)利要求1所述的方法，其中，在重復(fù)進(jìn)行第一操作和第二操作，以利用所述至少一個(gè)關(guān)鍵地標(biāo)的位姿對(duì)所述優(yōu)化后的位姿圖模型進(jìn)行迭代優(yōu)化的同時(shí)，統(tǒng)計(jì)迭代次數(shù)。

12.根據(jù)權(quán)利要求11所述的方法，其中，所述方法進(jìn)一步包括：當(dāng)所統(tǒng)計(jì)的迭代次數(shù)達(dá)到迭代次數(shù)閾值時(shí)，停止迭代優(yōu)化并輸出優(yōu)化后的位姿圖模型。