本技術屬于非圓形底盤的機器人窄道脫困路徑規(guī)劃，更具體地說，是涉及一種窄道脫困路徑規(guī)劃方法及智能機器人。

背景技術：

1、在室內(nèi)環(huán)境中，機器人經(jīng)常需要面對通過狹窄通道的挑戰(zhàn)。這些通道可能由于家具布局、建筑結構或其他障礙物的存在而變得異常狹窄，對機器人的導航能力提出了極高的要求。當機器人在執(zhí)行復雜的導航任務時，有時可能會因為環(huán)境復雜或傳感器數(shù)據(jù)不準確等原因，導致導航失敗。此時，如果下一個任務目標點恰好位于機器人的后方，那么機器人就必須執(zhí)行掉頭操作，以便重新規(guī)劃路線并繼續(xù)執(zhí)行任務。例如清潔機器人、消防機器人、巡查機器人、運輸機器人等。

2、在現(xiàn)有技術中，智能機器人的導航通常都是以圓形底盤為物理模型，圓形底盤的運動中心位于底盤中心位置，并利用每個柵格代價值進行搜索脫困路徑，因為該方法的本質(zhì)是代價優(yōu)先。

3、然而，針對非圓形底盤機器人在不規(guī)則窄道中的場景，所述非圓形底盤包括矩形、梯形、三角形、菱形、平行四邊形以及非規(guī)則形狀的底盤，非圓形底盤機器人通常為兩輪差速底盤，其驅(qū)動輪不在中心位置，運動中心位于底盤幾何中心的后方。由于窄道寬度限制機器人無法原地掉頭，因此，對于采用非圓形底盤設計的機器人來說，如何在狹窄的不規(guī)則通道中成功脫離困境，是一個亟待解決的技術難題。

技術實現(xiàn)思路

1、本技術實施例的目的在于提供一種窄道脫困路徑規(guī)劃方法及智能機器人，以解決現(xiàn)有技術對非圓形底盤機器人不規(guī)則窄道中如何脫困的技術問題。

2、為實現(xiàn)上述目的，本技術采用的技術方案是：提供一種窄道脫困路徑規(guī)劃方法，所述窄道脫困路徑規(guī)劃方法，包括：

3、步驟1：獲取機器人的底盤的最小矩形框，對最小矩形框的長邊做同步拉伸處理得到機器人的物理模型v1，基于代價地圖向前生成規(guī)劃路徑s1；

4、步驟2：初始化隊列f以及集合r，其中集合r用于存儲已訪問的節(jié)點c，將規(guī)劃路徑s1的終點作為初始起點o加入隊列f，以物理模型v1計算隊列f中每一節(jié)點的自由旋轉角a；

5、步驟3：從隊列f取出自由旋轉角a最大的節(jié)點c進行訪問，且將該節(jié)點c的未訪問鄰居節(jié)點n全部加入隊列f，循環(huán)本步驟直至訪問到自由旋轉角a大于脫困旋轉角amin的節(jié)點c作為脫困點e；

6、步驟4：基于代價地圖以所述最小矩形框為機器人的物理模型v2生成初始起點o至脫困點e的規(guī)劃路徑s2以及脫困點e至任務地t的規(guī)劃路徑s3；

7、步驟5：基于規(guī)劃路徑s1、規(guī)劃路徑s2以及規(guī)劃路徑s3拼接成完整路徑s。

8、在本技術另一個實施例中，基于代價地圖生成規(guī)劃路徑s1的方法，包括步驟：

9、計算機器人前方、左側、右側三個候選點，候選點的世界坐標為（x’，y’），則計算公式為：

10、

11、其中，(rx,ry)是機器人的世界坐標，yaw是機器人方向，stepx和stepy分別表示機器人在x方向和y方向上的步長；候選點在前方，步長設定為stepx=a米，stepy=0.00米；候選點在左側，步長設定為stepx=a米，stepy=a米；候選點在右側，步長設定為stepx=a米，stepy=-a米，a為大于零的固定參數(shù)；

12、計算出三個候選點的坐標后，依次獲取該三個候選點在代價地圖上的代價值，選擇代價最小的候選點作為新目標點循環(huán)遍歷；

13、循環(huán)遍歷結束后將所有新目標點納入規(guī)劃路徑s1，依據(jù)新目標點的遍歷順序構成規(guī)劃路徑s1。

14、在本技術另一個實施例中，在步驟2中，以物理模型v1計算隊列f中每一節(jié)點的自由旋轉角a的方法，包括步驟：

15、提取機器人當前位置周圍的障礙物柵格，以運動中心為原點轉換為極坐標，所述運動中心為機器人的兩輪差速底盤的驅(qū)動輪軸中點；

16、計算機器人旋轉任意角度α時輪廓點與障礙物的最小距離d(α)；

17、若最小距離d(α)≥0，定義該α屬于安全角度；

18、找到包含α=0且連續(xù)的安全角度區(qū)間[αleft,αright]，其中α=0為機器人在當前節(jié)點時的方向；

19、則自由旋轉角a=αright-αleft。

20、在本技術另一個實施例中，在步驟4中，若基于代價地圖以物理模型v2生成規(guī)劃路徑s2或規(guī)劃路徑s3失敗，則對最小矩形框的前端區(qū)域做橫向切除處理得到機器人的物理模型v3，基于代價地圖以物理模型v3重新生成初始起點o至脫困點e的規(guī)劃路徑s2以及脫困點e至任務地t的規(guī)劃路徑s3。

21、在本技術另一個實施例中，在步驟5之后，還包括步驟：

22、將拼接后的完整路徑s離散化為若干路徑點{p1，p2，…pn}；

23、對于每個路徑點pi都通過其鄰近三點pi-1、pi、pi+1計算其曲率ki；

24、對路徑點pi-1與路徑點pi+1向靠近pi的切線方向進行偏移處理，直至其鄰近三點pi-1、pi、pi+1的曲率ki滿足：

25、

26、其中，rmin為機器人最小轉彎半徑。

27、在本技術另一個實施例中，在步驟5之后，還包括步驟：

28、將拼接后的完整路徑s離散化為若干路徑點{p1，p2，…pn}；

29、對于每個路徑點pi都通過其鄰近三點pi-1、pi、pi+1計算其曲率ki；

30、對路徑點pi-1與路徑點pi+1向靠近pi的切線方向進行偏移處理，直至其鄰近三點pi-1、pi、pi+1曲率ki滿足：

31、

32、其中，rback-min為機器人倒車過程中的最小轉彎半徑。

33、在本技術另一個實施例中，在步驟1之前，還包括步驟：

34、于當前規(guī)劃路徑上選擇一個與機器人的歐氏距離超過閾值tdistance的路徑點p；

35、利用路徑點p來計算當前規(guī)劃路徑的路徑方向θ；

36、計算路徑方向θ與機器人方向yaw的夾角δθ；

37、若存在，則觸發(fā)窄道脫困模式，其中trad為夾角閾值。

38、在本技術另一個實施例中，所述機器人的底盤為矩形，閾值tdistance為機器人底盤中心點至機器人頭部的距離，路徑點p滿足：

39、

40、其中(px,py)代表當前規(guī)劃路徑上路徑點p的世界坐標，(rx,ry)代表機器人的世界坐標。

41、在本技術另一個實施例中，計算路徑方向θ與機器人方向yaw的夾角δθ的方法，包括步驟：

42、先采用以下公式計算路徑方向θ：

43、

44、再用以下公式計算夾角δθ：

45、。

46、本技術還提供一種智能機器人，基于如上所述的窄道脫困路徑規(guī)劃方法，所述智能機器人包括兩輪差速底盤，所述兩輪差速底盤的運動中心位于底盤幾何中心的后方。

47、本技術提供的一種窄道脫困路徑規(guī)劃方法與現(xiàn)有技術相比的有益效果在于：

48、1、通過同步拉伸處理機器人的物理模型v1，以反向思維限制轉向能力，快速定位窄道中的轉角區(qū)域，以提前發(fā)現(xiàn)潛在風險與探索脫困點的機遇。

49、2、通過以自由旋轉角a為優(yōu)先級指標的轉向優(yōu)先策略，每一步都獨立判斷，提升了實時避障的能力，結合貪婪蔓延策略通過動態(tài)回溯機制確保探索方向始終朝向最優(yōu)解，提升了脫困點e的搜索效率，從而在不規(guī)則窄道中兼具實時避障和全局優(yōu)化能力。

50、3、針對矩形、梯形等非圓形底盤機器人（尤其是驅(qū)動輪偏離中心的機型），解決了因運動中心與幾何中心不重合導致的轉向控制難題。

51、本技術提供的一種智能機器人與現(xiàn)有技術相比的有益效果在于：智能機器人在該不規(guī)則窄道中時，當目標點出現(xiàn)在智能機器人后方時，所述兩輪差速底盤結合所述窄道脫困路徑規(guī)劃方法尋找脫困點進行掉頭，以快速完成窄道脫困。