本發(fā)明涉及移動(dòng)機(jī)器人定位建圖領(lǐng)域，尤其涉及一種移動(dòng)機(jī)器人多雷達(dá)協(xié)同建圖方法、裝置、設(shè)備及介質(zhì)。

背景技術(shù)：

1、即時(shí)定位與建圖(slam)是智能移動(dòng)機(jī)器人實(shí)現(xiàn)自主定位和自主導(dǎo)航的關(guān)鍵技術(shù)。高精度的環(huán)境地圖能夠?yàn)橐苿?dòng)機(jī)器人的導(dǎo)航提供關(guān)鍵而有效的環(huán)境信息，指導(dǎo)移動(dòng)機(jī)器人在合適的空間內(nèi)完成移動(dòng)。

2、對(duì)于某些具有特殊結(jié)構(gòu)的移動(dòng)機(jī)器人而言，設(shè)計(jì)者無(wú)法將激光雷達(dá)安裝在合適的位置，例如在輔行移動(dòng)機(jī)器人上安裝激光雷達(dá)時(shí)，只能將雷達(dá)安放在機(jī)器人的側(cè)邊，并且無(wú)可避免地會(huì)出現(xiàn)雷達(dá)視野被機(jī)器人機(jī)身大面積遮擋的問(wèn)題，這會(huì)導(dǎo)致定位和建圖精度明顯降低。為解決該問(wèn)題，可以將多個(gè)激光雷達(dá)分別安裝在移動(dòng)機(jī)器人四周，通過(guò)提高雷達(dá)數(shù)量來(lái)彌補(bǔ)雷達(dá)視野的缺失。目前，移動(dòng)機(jī)器人平臺(tái)上常用的二維激光slam算法通常僅支持來(lái)自單個(gè)激光雷達(dá)的點(diǎn)云數(shù)據(jù)作為輸入信號(hào)。

技術(shù)實(shí)現(xiàn)思路

1、為至少一定程度上解決現(xiàn)有技術(shù)中存在的技術(shù)問(wèn)題之一，本發(fā)明的目的在于提供一種基于點(diǎn)云數(shù)據(jù)融合的移動(dòng)機(jī)器人多雷達(dá)協(xié)同建圖方法、裝置、設(shè)備及介質(zhì)。

2、本發(fā)明所采用的第一技術(shù)方案是：

3、一種移動(dòng)機(jī)器人多雷達(dá)協(xié)同建圖方法，包括以下步驟：

4、構(gòu)建機(jī)器人坐標(biāo)系，以及構(gòu)建每個(gè)雷達(dá)對(duì)應(yīng)的雷達(dá)坐標(biāo)系；機(jī)器人上安裝有多個(gè)激光雷達(dá)；

5、根據(jù)每個(gè)雷達(dá)坐標(biāo)系和機(jī)器人坐標(biāo)系的相對(duì)位置關(guān)系，建立旋轉(zhuǎn)矩陣pj和平移矩陣tj；

6、多個(gè)激光雷達(dá)在同一時(shí)刻分別采集環(huán)境點(diǎn)云數(shù)據(jù)，并將雷達(dá)坐標(biāo)系下的點(diǎn)云坐標(biāo)轉(zhuǎn)換到機(jī)器人坐標(biāo)系下；對(duì)于第i時(shí)刻雷達(dá)j中的點(diǎn)云坐標(biāo)sji，根據(jù)旋轉(zhuǎn)矩陣pj和平移矩陣tj轉(zhuǎn)換到機(jī)器人坐標(biāo)系下，得到點(diǎn)云坐標(biāo)sj′i；

7、將轉(zhuǎn)換到機(jī)器人坐標(biāo)系下的點(diǎn)云坐標(biāo)代入設(shè)計(jì)好的損失函數(shù)f(si)，迭代求解當(dāng)前時(shí)刻機(jī)器人的最優(yōu)位姿ξ*；

8、根據(jù)計(jì)算得到的不同時(shí)刻下的機(jī)器人位姿生成運(yùn)動(dòng)路徑，并隨著機(jī)器人的移動(dòng)完成地圖構(gòu)建。

9、進(jìn)一步地，所述構(gòu)建機(jī)器人坐標(biāo)系，包括：

10、獲取移動(dòng)機(jī)器人的底盤(pán)對(duì)稱(chēng)軸上任意點(diǎn)作為機(jī)器人坐標(biāo)系的原點(diǎn)o，按照右手坐標(biāo)系規(guī)則，令坐標(biāo)系y軸沿機(jī)器人前進(jìn)方向，坐標(biāo)系z(mì)軸沿豎直向上方向，構(gòu)建機(jī)器人坐標(biāo)系(xo,yo,zo)，將該坐標(biāo)系(xo,yo,zo)作為移動(dòng)機(jī)器人的基準(zhǔn)坐標(biāo)系。

11、進(jìn)一步地，所述構(gòu)建每個(gè)雷達(dá)對(duì)應(yīng)的雷達(dá)坐標(biāo)系，包括：

12、獲取每個(gè)雷達(dá)的幾何中心作為坐標(biāo)系原點(diǎn)oj，對(duì)于第j個(gè)的激光雷達(dá)，其坐標(biāo)原點(diǎn)記為oj；按照右手坐標(biāo)系規(guī)則，令坐標(biāo)系z(mì)軸沿豎直向上方向，坐標(biāo)系x和y軸位于同一平面，構(gòu)建雷達(dá)坐標(biāo)系(xj,yj,zj)。

13、進(jìn)一步地，所述根據(jù)每個(gè)雷達(dá)坐標(biāo)系和機(jī)器人坐標(biāo)系的相對(duì)位置關(guān)系，建立旋轉(zhuǎn)矩陣pj和平移矩陣tj，包括：

14、由于每個(gè)雷達(dá)安裝在機(jī)器人機(jī)身上后即固定不動(dòng)，雷達(dá)坐標(biāo)系原點(diǎn)oj和機(jī)器人坐標(biāo)系原點(diǎn)o之間的平移矩陣tj固定，故其中為機(jī)器人坐標(biāo)系原點(diǎn)o在雷達(dá)坐標(biāo)系下的坐標(biāo)；

15、由于每個(gè)激光雷達(dá)安裝在水平面上的角度不同，所以需要確定雷達(dá)坐標(biāo)系(xj,yj,zj)和機(jī)器人坐標(biāo)系(xo,yo,zo)之間的旋轉(zhuǎn)角度θj，對(duì)于每個(gè)固定在機(jī)器人機(jī)身上的雷達(dá)而言都存在相對(duì)應(yīng)的θj，則旋轉(zhuǎn)矩陣

16、進(jìn)一步地，所述將雷達(dá)坐標(biāo)系下的點(diǎn)云坐標(biāo)轉(zhuǎn)換到機(jī)器人坐標(biāo)系下，包括：

17、確定好雷達(dá)坐標(biāo)系(xj,yj,zj)和機(jī)器人坐標(biāo)系(xo,yo,zo)間的旋轉(zhuǎn)矩陣rj和平移矩陣tj后，根據(jù)坐標(biāo)在不同坐標(biāo)系的相對(duì)變換公式可知，雷達(dá)坐標(biāo)系的點(diǎn)云坐標(biāo)sji轉(zhuǎn)換到機(jī)器人坐標(biāo)系下的點(diǎn)云坐標(biāo)s′ji＝rjsji+tj。

18、進(jìn)一步地，所述將轉(zhuǎn)換到機(jī)器人坐標(biāo)系下的點(diǎn)云坐標(biāo)代入設(shè)計(jì)好的損失函數(shù)f(si)，迭代求解當(dāng)前時(shí)刻機(jī)器人的最優(yōu)位姿ξ*，包括：

19、根據(jù)同一時(shí)刻來(lái)自多個(gè)激光雷達(dá)的點(diǎn)云數(shù)據(jù)，計(jì)算移動(dòng)機(jī)器人當(dāng)前時(shí)刻位姿；其中損失函數(shù)為：

20、

21、式中，m(si(ξ))為經(jīng)典的hector-slam算法中的地圖占有可能性指標(biāo)，該值越大代表ξ*越接近機(jī)器人真實(shí)位姿；δξ為當(dāng)前時(shí)刻機(jī)器人位姿和上一時(shí)刻機(jī)器人位姿之差；m為激光雷達(dá)數(shù)量，n為激光雷達(dá)分辨率；

22、為求解使得損失函數(shù)f(si)→0時(shí)的最優(yōu)ξ*，先將m(sji(ξ+δξ))在ξ處進(jìn)行一階泰勒展開(kāi)，再使用高斯牛頓法求解得到最優(yōu)的δξ，將上一時(shí)刻的ξ與δξ相加即得到當(dāng)前時(shí)刻的ξ*。

23、進(jìn)一步地，所述根據(jù)計(jì)算得到的不同時(shí)刻下的機(jī)器人位姿生成運(yùn)動(dòng)路徑，并隨著機(jī)器人的移動(dòng)完成地圖構(gòu)建，包括：

24、隨著機(jī)器人的移動(dòng)，按照預(yù)設(shè)的頻率實(shí)時(shí)解算機(jī)器人當(dāng)前位姿ξ*，每在一個(gè)時(shí)刻下計(jì)算得到ξ*，即將該時(shí)刻的點(diǎn)云數(shù)據(jù)作為障礙物邊緣輸出到地圖圖像上，連續(xù)的ξ*即為機(jī)器人的運(yùn)動(dòng)軌跡，連續(xù)輸出的點(diǎn)云數(shù)據(jù)映射到圖片上即為所構(gòu)建的環(huán)境地圖。

25、本發(fā)明所采用的第二技術(shù)方案是：

26、一種移動(dòng)機(jī)器人多雷達(dá)協(xié)同建圖裝置，包括：

27、坐標(biāo)系構(gòu)建模塊，用于構(gòu)建機(jī)器人坐標(biāo)系，以及構(gòu)建每個(gè)雷達(dá)對(duì)應(yīng)的雷達(dá)坐標(biāo)系；機(jī)器人上安裝有多個(gè)雷達(dá)；

28、矩陣計(jì)算模塊，用于根據(jù)每個(gè)雷達(dá)坐標(biāo)系和機(jī)器人坐標(biāo)系的相對(duì)位置關(guān)系，建立旋轉(zhuǎn)矩陣pj和平移矩陣tj；

29、坐標(biāo)轉(zhuǎn)換模塊，用于多個(gè)激光雷達(dá)在同一時(shí)刻分別采集環(huán)境點(diǎn)云數(shù)據(jù)，并將雷達(dá)坐標(biāo)系下的點(diǎn)云坐標(biāo)轉(zhuǎn)換到機(jī)器人坐標(biāo)系下；

30、位姿求解模塊，用于將轉(zhuǎn)換到機(jī)器人坐標(biāo)系下的點(diǎn)云坐標(biāo)代入設(shè)計(jì)好的損失函數(shù)f(si)，迭代求解當(dāng)前時(shí)刻機(jī)器人的最優(yōu)位姿ξ*；

31、地圖構(gòu)建模塊，用于根據(jù)計(jì)算得到的不同時(shí)刻下的機(jī)器人位姿生成運(yùn)動(dòng)路徑，并隨著機(jī)器人的移動(dòng)完成地圖構(gòu)建。

32、本發(fā)明所采用的第三技術(shù)方案是：

33、一種電子設(shè)備，所述電子設(shè)備包括處理器和存儲(chǔ)器，所述存儲(chǔ)器中存儲(chǔ)有至少一條指令、至少一段程序、代碼集或指令集，所述至少一條指令、所述至少一段程序、所述代碼集或指令集由所述處理器加載并執(zhí)行，以實(shí)現(xiàn)如上所述的一種移動(dòng)機(jī)器人多雷達(dá)協(xié)同建圖方法。

34、本發(fā)明所采用的第四技術(shù)方案是：

35、一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，所述存儲(chǔ)介質(zhì)中存儲(chǔ)有至少一條指令、至少一段程序、代碼集或指令集，所述至少一條指令、所述至少一段程序、所述代碼集或指令集由處理器加載并執(zhí)行以實(shí)現(xiàn)如上所述的一種移動(dòng)機(jī)器人多雷達(dá)協(xié)同建圖方法。

36、本發(fā)明所采用的第五技術(shù)方案是：

37、一種計(jì)算機(jī)程序產(chǎn)品或計(jì)算機(jī)程序，該計(jì)算機(jī)程序產(chǎn)品或計(jì)算機(jī)程序包括計(jì)算機(jī)指令，該計(jì)算機(jī)指令存儲(chǔ)在計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中。計(jì)算機(jī)設(shè)備的處理器可以從計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)讀取該計(jì)算機(jī)指令，處理器執(zhí)行該計(jì)算機(jī)指令，使得該計(jì)算機(jī)設(shè)備執(zhí)行上述的方法。

38、本發(fā)明有如下優(yōu)點(diǎn)和有益效果：

39、1)本發(fā)明的一種基于點(diǎn)云數(shù)據(jù)融合的移動(dòng)機(jī)器人多雷達(dá)協(xié)同建圖方法，使用多雷達(dá)點(diǎn)云數(shù)據(jù)建圖，克服了一些場(chǎng)景下單雷達(dá)建圖時(shí)雷達(dá)視野受遮擋導(dǎo)致的定位不準(zhǔn)的問(wèn)題。

40、2)本發(fā)明對(duì)雷達(dá)安裝的方向沒(méi)有限制，便于應(yīng)用在各種具有特殊機(jī)械結(jié)構(gòu)的移動(dòng)機(jī)器人機(jī)身上。

41、3)本發(fā)明對(duì)比現(xiàn)有技術(shù)不復(fù)雜，對(duì)硬件算力要求不高，便于在各種移動(dòng)機(jī)器人平臺(tái)上實(shí)現(xiàn)。