本技術(shù)涉及移動(dòng)機(jī)器人主動(dòng)定位，尤其涉及一種室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位方法和裝置。

背景技術(shù)：

1、隨著智慧康養(yǎng)，智能家居、倉(cāng)儲(chǔ)物流、醫(yī)療輔助等領(lǐng)域的發(fā)展，室內(nèi)移動(dòng)機(jī)器人在各行業(yè)中的應(yīng)用日益廣泛。為了在復(fù)雜多變的室內(nèi)環(huán)境中實(shí)現(xiàn)高效、魯棒的自主導(dǎo)航，精確且長(zhǎng)生命周期的主動(dòng)定位是關(guān)鍵技術(shù)之一，是室內(nèi)移動(dòng)機(jī)器人完成一切復(fù)雜任務(wù)的重要前提。室內(nèi)環(huán)境通常具有較高的動(dòng)態(tài)性和多樣性，如家具擺放的頻繁變化、人員活動(dòng)的干擾、臨時(shí)障礙物的出現(xiàn)、光照條件的變化等，這給移動(dòng)機(jī)器人的精準(zhǔn)定位帶來(lái)了諸多挑戰(zhàn)。

2、傳統(tǒng)的定位方法多基于被動(dòng)傳感，依賴于靜態(tài)環(huán)境特征，難以在環(huán)境變化或定位失效的情況下迅速恢復(fù)準(zhǔn)確的位姿。此外，室內(nèi)環(huán)境中的狹窄通道、多動(dòng)態(tài)目標(biāo)、小型障礙物及光照變化等因素進(jìn)一步增加了精準(zhǔn)定位的難度。

3、因此，本技術(shù)提供了一種室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位方法和裝置。

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

1、本技術(shù)實(shí)施例提供了一種室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位方法和裝置，以使移動(dòng)機(jī)器人在室內(nèi)動(dòng)態(tài)場(chǎng)景下精確主動(dòng)定位。

2、第一方面，本技術(shù)實(shí)施例提供了一種室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位方法，包括：

3、采集室內(nèi)環(huán)境數(shù)據(jù)并構(gòu)建室內(nèi)全局地圖，所述室內(nèi)全局地圖包括環(huán)境的幾何結(jié)構(gòu)和語(yǔ)義信息；

4、將移動(dòng)機(jī)器人實(shí)時(shí)采集的傳感數(shù)據(jù)與所述室內(nèi)全局地圖進(jìn)行匹配，實(shí)時(shí)估計(jì)所述移動(dòng)機(jī)器人在三維空間中的位姿，并計(jì)算位姿估計(jì)的不確定性指標(biāo)；

5、基于所述位姿估計(jì)的不確定性指標(biāo)評(píng)估不同潛在觀測(cè)點(diǎn)的信息增益，并選擇信息增益量最大的觀測(cè)點(diǎn)作為候選目標(biāo)點(diǎn)；

6、規(guī)劃到達(dá)所述候選目標(biāo)點(diǎn)的路徑；

7、所述移動(dòng)機(jī)器人采集所述候選目標(biāo)點(diǎn)處的傳感數(shù)據(jù)，并與所述室內(nèi)全局地圖重新匹配、更新所述移動(dòng)機(jī)器人的位姿；

8、實(shí)時(shí)監(jiān)測(cè)室內(nèi)環(huán)境變化，動(dòng)態(tài)更新所述室內(nèi)全局地圖，并且定期校正和優(yōu)化所述室內(nèi)全局地圖。

9、在一種可行的實(shí)現(xiàn)方式中，所述將移動(dòng)機(jī)器人實(shí)時(shí)采集的傳感數(shù)據(jù)與所述室內(nèi)全局地圖進(jìn)行匹配，包括：

10、從移動(dòng)機(jī)器人實(shí)時(shí)采集的傳感器數(shù)據(jù)中提取高置信度的特征點(diǎn)，通過(guò)特征匹配算法與室內(nèi)全局地圖中的特征點(diǎn)進(jìn)行對(duì)齊。

11、在一種可行的實(shí)現(xiàn)方式中，所述將移動(dòng)機(jī)器人實(shí)時(shí)采集的傳感數(shù)據(jù)與所述室內(nèi)全局地圖進(jìn)行匹配，實(shí)時(shí)估計(jì)所述移動(dòng)機(jī)器人在三維空間中的位姿，包括：

12、每一組傳感器觀測(cè)、回環(huán)檢測(cè)或里程計(jì)信息均視為對(duì)位姿的一項(xiàng)約束，即在室內(nèi)全局地圖中形成相應(yīng)的因子，進(jìn)而形成因子圖結(jié)構(gòu)，其中節(jié)點(diǎn)表示機(jī)器人位姿，因子表示約束關(guān)系；

13、對(duì)于所述因子圖結(jié)構(gòu)中的每個(gè)因子，根據(jù)特征匹配和傳感器觀測(cè)，定義相應(yīng)的殘差函數(shù)；

14、將所有因子的誤差加權(quán)求和，形成整體目標(biāo)函數(shù)；

15、利用非線性優(yōu)化方法迭代求解最小化問(wèn)題，每次迭代通過(guò)計(jì)算殘差和雅可比矩陣，不斷更新位姿向量，直至收斂，即獲得高精度的位姿估計(jì)結(jié)果；

16、所述殘差函數(shù)包括：

17、將機(jī)器人在不同時(shí)刻或關(guān)鍵幀處的位姿記為(x1,x2,...,xn)；

18、移動(dòng)機(jī)器人從位姿xi運(yùn)動(dòng)到位姿xj的預(yù)測(cè)變換為zij，傳感器實(shí)際觀測(cè)得到的相對(duì)變換為定義誤差向量其中xi與xj表示為在李群上的位姿，為測(cè)得的相對(duì)位姿變換。

19、在一種可行的實(shí)現(xiàn)方式中，將所有因子的誤差加權(quán)求和，形成整體目標(biāo)函數(shù)：

20、

21、其中ωij為信息矩陣或協(xié)方差矩陣的逆，c為所有存在約束的位姿對(duì)集合；

22、最小化所述整體目標(biāo)函數(shù)，獲得一組整體誤差最小的位姿估計(jì)x1,...,xn；

23、每次迭代通過(guò)計(jì)算殘差和雅可比矩陣，不斷更新位姿向量x，直至收斂，其中，

24、x(k+1)＝x(k)+δx,

25、其中δx根據(jù)迭代算法規(guī)則由當(dāng)前迭代步的梯度或海森矩陣近似解算得到。

26、在一種可行的實(shí)現(xiàn)方式中，計(jì)算位姿估計(jì)的不確定性指標(biāo)，包括：

27、在圖優(yōu)化求解位姿的過(guò)程中，通過(guò)計(jì)算近似海森矩陣或增量方程的逆，估計(jì)位姿的不確定性；

28、使用協(xié)方差矩陣或信息熵表示位姿的不確定性；

29、在圖優(yōu)化收斂后，其近似海森矩陣可表示為

30、

31、其中jij為誤差函數(shù)eij關(guān)于位姿變量的雅可比矩陣；

32、若將h的逆記為p＝h-1，則可視p為位姿估計(jì)的近似協(xié)方差矩陣，其對(duì)角塊pi或相應(yīng)子塊可視為第i個(gè)位姿的協(xié)方差，從而反映位姿估計(jì)精度；

33、和/或；

34、對(duì)于一高斯分布n(μ,p)，其信息熵可表示為：

35、

36、其中d為狀態(tài)維度，當(dāng)det(p)越大時(shí)，信息熵越高，說(shuō)明不確定性也越高。

37、在一種可行的實(shí)現(xiàn)方式中，基于當(dāng)前位姿的不確定性指標(biāo)，運(yùn)用信息熵減少量和互信息量評(píng)估不同潛在觀測(cè)點(diǎn)的信息增益，包括：

38、當(dāng)前移動(dòng)機(jī)器人對(duì)環(huán)境的位姿估計(jì)分布為p(x)，其不確定性用熵h(x)表示；

39、若機(jī)器人計(jì)劃到達(dá)某個(gè)候選觀測(cè)點(diǎn)o后，獲得新的觀測(cè)數(shù)據(jù)zo，融合該觀測(cè)數(shù)據(jù)zo后，位姿估計(jì)的分布變?yōu)閜(x|zo)，對(duì)應(yīng)的熵變?yōu)閔(x|zo)，信息增益定義為：

40、ig(o)＝h(x)-h(x|zo).

41、在一種可行的實(shí)現(xiàn)方式中，所述規(guī)劃到達(dá)所述候選目標(biāo)點(diǎn)的路徑，包括：

42、采用路徑規(guī)劃方法，結(jié)合室內(nèi)全局地圖中的幾何結(jié)構(gòu)和障礙物信息，規(guī)劃從當(dāng)前位姿到目標(biāo)觀測(cè)點(diǎn)的最優(yōu)路徑；

43、將室內(nèi)全局地圖離散為柵格地圖或基于拓?fù)涔?jié)點(diǎn)構(gòu)建圖結(jié)構(gòu)；其中，對(duì)于柵格地圖，令每個(gè)可通行柵格為節(jié)點(diǎn)，每個(gè)相鄰柵格間建立邊；對(duì)于拓?fù)鋱D，將可行走區(qū)域的關(guān)鍵拐點(diǎn)、入口、通道交匯處作為節(jié)點(diǎn)建立圖結(jié)構(gòu)；

44、執(zhí)行a*算法，對(duì)每個(gè)柵格節(jié)點(diǎn)n維護(hù)f(n)＝g(n)+h(n)，其中g(shù)(n)表示從起始位置到n的實(shí)際代價(jià)，h(n)為預(yù)估到目標(biāo)觀測(cè)點(diǎn)的啟發(fā)式函數(shù)；

45、a*算法通過(guò)擴(kuò)展代價(jià)最小的節(jié)點(diǎn)并更新其鄰居節(jié)點(diǎn)的g值，直到到達(dá)目標(biāo)。

46、在一種可行的實(shí)現(xiàn)方式中，所述規(guī)劃到達(dá)所述候選目標(biāo)點(diǎn)的路徑，包括：

47、對(duì)規(guī)劃路徑進(jìn)行碰撞檢測(cè)和可行性驗(yàn)證；

48、在柵格地圖或幾何地圖中，將所述移動(dòng)機(jī)器人簡(jiǎn)化為一個(gè)圓形或多邊形模型，并在規(guī)劃得到的路徑上依次對(duì)各個(gè)節(jié)點(diǎn)或離散段進(jìn)行碰撞檢測(cè)，若檢測(cè)到所述移動(dòng)機(jī)器人某處與障礙物重疊或距離小于安全閾值，則判定為路徑不可行；

49、將動(dòng)態(tài)障礙物的未來(lái)位置納入安全區(qū)域判斷，若預(yù)測(cè)動(dòng)態(tài)障礙物會(huì)與所述移動(dòng)機(jī)器人規(guī)劃路徑發(fā)生沖突，則重新規(guī)劃或局部?jī)?yōu)化路徑；

50、在狹窄通道場(chǎng)景下，檢測(cè)移動(dòng)機(jī)器人與通道兩側(cè)的最小距離是否滿足一定的安全余量。

51、在一種可行的實(shí)現(xiàn)方式中，根據(jù)實(shí)時(shí)環(huán)境變化，應(yīng)用梯度下降法對(duì)初步規(guī)劃路徑進(jìn)行細(xì)化，提升路徑的平滑性和適應(yīng)性，包括：

52、將由a*或dijkstra獲得的離散路徑節(jié)點(diǎn)記為{p0,p1,...,pn}；

53、將路徑使用參數(shù){pi}描述，為使路徑平滑并遠(yuǎn)離障礙物，定義目標(biāo)函數(shù)：

54、

55、其中：第一項(xiàng)為路徑長(zhǎng)度約束；第二項(xiàng)為避障約束，d(pi,o)為點(diǎn)pi到障礙物o的距離的某種罰函數(shù)，若pi靠近障礙物則該值增大；第三項(xiàng)為平滑度約束，通過(guò)二階差分項(xiàng)保證路徑的平滑；α,β為權(quán)重系數(shù)，用于平衡各約束的影響；

56、將{pi}設(shè)置為a*/dijkstra的離散解或插值后的初值；

57、對(duì)目標(biāo)函數(shù)e({pi})關(guān)于pi求梯度：

58、分別計(jì)算路徑長(zhǎng)度項(xiàng)、避障項(xiàng)、平滑度項(xiàng)的偏導(dǎo)數(shù)獲得具體梯度表達(dá)式；

59、

60、其中η為學(xué)習(xí)率(或步長(zhǎng))，重復(fù)迭代，直至收斂或達(dá)到迭代次數(shù)上限，在迭代結(jié)束后，或得平滑、避障且滿足實(shí)時(shí)環(huán)境約束的路徑{pi}。

61、第二方面，本技術(shù)實(shí)施例提供了一種室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位裝置，應(yīng)用如第一方面所述的室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位方法，包括：

62、控制器；

63、地圖構(gòu)建模塊，與所述控制器電連接，所述地圖構(gòu)建模塊用于采集室內(nèi)環(huán)境數(shù)據(jù)并構(gòu)建室內(nèi)全局地圖，所述室內(nèi)全局地圖包括環(huán)境的幾何結(jié)構(gòu)和語(yǔ)義信息；

64、并且所述地圖構(gòu)建模塊實(shí)時(shí)監(jiān)測(cè)室內(nèi)環(huán)境變化，動(dòng)態(tài)更新所述室內(nèi)全局地圖；

65、位姿評(píng)估模塊，與所述控制器電連接，所述位姿評(píng)估模塊用于將移動(dòng)機(jī)器人實(shí)時(shí)采集的傳感數(shù)據(jù)與所述室內(nèi)全局地圖進(jìn)行匹配，實(shí)時(shí)估計(jì)所述移動(dòng)機(jī)器人在三維空間中的位姿，并計(jì)算位姿估計(jì)的不確定性指標(biāo)；

66、信息增益評(píng)估模塊，與所述控制器電連接，所述信息增益評(píng)估模塊基于所述位姿估計(jì)的不確定性指標(biāo)評(píng)估不同潛在觀測(cè)點(diǎn)的信息增益，并選擇信息增益量最大的觀測(cè)點(diǎn)作為候選目標(biāo)點(diǎn)；

67、路徑規(guī)劃模塊，與所述控制器電連接，所述路徑規(guī)劃模塊用于規(guī)劃到達(dá)所述候選目標(biāo)點(diǎn)的路徑；

68、調(diào)整模塊，與所述控制器電連接，所述調(diào)整模塊將所述移動(dòng)機(jī)器人采集的所述候選目標(biāo)點(diǎn)處的傳感數(shù)據(jù)與所述室內(nèi)全局地圖重新匹配并更新所述移動(dòng)機(jī)器人的位姿。

69、第一方面，本技術(shù)實(shí)施例提供了一種室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位方法，包括：采集室內(nèi)環(huán)境數(shù)據(jù)并構(gòu)建室內(nèi)全局地圖，其中室內(nèi)全局地圖包括環(huán)境的幾何結(jié)構(gòu)和語(yǔ)義信息。通過(guò)高精度的室內(nèi)全局地圖獲取與構(gòu)建，提供了豐富的環(huán)境信息，提升了移動(dòng)機(jī)器人位姿估計(jì)的準(zhǔn)確性和可靠性。將移動(dòng)機(jī)器人實(shí)時(shí)采集的傳感數(shù)據(jù)與所述室內(nèi)全局地圖進(jìn)行匹配，實(shí)時(shí)估計(jì)所述移動(dòng)機(jī)器人在三維空間中的位姿，并計(jì)算位姿估計(jì)的不確定性指標(biāo)，為移動(dòng)機(jī)器人移動(dòng)提供了決策基礎(chǔ)；基于位姿估計(jì)的不確定性指標(biāo)評(píng)估不同潛在觀測(cè)點(diǎn)的信息增益，并選擇信息增益量最大的觀測(cè)點(diǎn)作為候選目標(biāo)點(diǎn)。可以理解的是，通過(guò)評(píng)估不同觀測(cè)點(diǎn)的信息增益，選擇最能降低位姿不確定性的目標(biāo)點(diǎn)，可優(yōu)化移動(dòng)機(jī)器人定位過(guò)程。實(shí)時(shí)監(jiān)測(cè)室內(nèi)環(huán)境變化，動(dòng)態(tài)更新所述室內(nèi)全局地圖，提升系統(tǒng)在動(dòng)態(tài)環(huán)境中的適應(yīng)性和魯棒性，確保機(jī)器人定位的持續(xù)準(zhǔn)確。規(guī)劃到達(dá)候選目標(biāo)點(diǎn)的路徑；移動(dòng)機(jī)器人采集所述候選目標(biāo)點(diǎn)處的傳感數(shù)據(jù)，并與室內(nèi)全局地圖重新匹配、更新所述移動(dòng)機(jī)器人的位姿。移動(dòng)機(jī)器人結(jié)合實(shí)時(shí)環(huán)境變化進(jìn)行路徑優(yōu)化，確保路徑的可行性和安全性。

70、第二方面，本技術(shù)實(shí)施例提供了一種室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位裝置，應(yīng)用如第一方面所述的室內(nèi)動(dòng)態(tài)場(chǎng)景的移動(dòng)機(jī)器人主動(dòng)定位方法，包括控制器、地圖構(gòu)建模塊、位姿評(píng)估模塊、信息增益評(píng)估模塊、路徑規(guī)劃模塊和調(diào)整模塊。其中，地圖構(gòu)建模塊與控制器電連接，地圖構(gòu)建模塊用于采集室內(nèi)環(huán)境數(shù)據(jù)并構(gòu)建室內(nèi)全局地圖，室內(nèi)全局地圖包括環(huán)境的幾何結(jié)構(gòu)和語(yǔ)義信息；并且地圖構(gòu)建模塊實(shí)時(shí)監(jiān)測(cè)室內(nèi)環(huán)境變化，動(dòng)態(tài)更新室內(nèi)全局地圖；位姿評(píng)估模塊與控制器電連接，位姿評(píng)估模塊用于將移動(dòng)機(jī)器人實(shí)時(shí)采集的傳感數(shù)據(jù)與室內(nèi)全局地圖進(jìn)行匹配，實(shí)時(shí)估計(jì)移動(dòng)機(jī)器人在三維空間中的位姿，并計(jì)算位姿估計(jì)的不確定性指標(biāo)；信息增益評(píng)估模塊與控制器電連接，信息增益評(píng)估模塊基于位姿估計(jì)的不確定性指標(biāo)評(píng)估不同潛在觀測(cè)點(diǎn)的信息增益，并選擇信息增益量最大的觀測(cè)點(diǎn)作為候選目標(biāo)點(diǎn)；路徑規(guī)劃模塊與控制器電連接，路徑規(guī)劃模塊用于規(guī)劃到達(dá)候選目標(biāo)點(diǎn)的路徑；調(diào)整模塊與控制器電連接，調(diào)整模塊將移動(dòng)機(jī)器人采集的候選目標(biāo)點(diǎn)處的傳感數(shù)據(jù)與室內(nèi)全局地圖重新匹配并更新移動(dòng)機(jī)器人的位姿。