複雜環境下六足機器人的人機共融智能運動規劃方法

2023-05-04 13:35:48

1.本發明屬於機器人技術領域。


背景技術：

2.足式機器人以其靈活的運動性能常被人們期許於能應用於複雜環境，如救災搶險、野外運輸、軍事作戰、星球探測等場景。且其離散落足優勢使得其能夠較好的適應於幾何和物理特徵存在突變的地形環境，具備輪式機器人所不具備的突出優勢。特別地，六足機器人相對於四足、雙足機器人來說，具備穩定性好、負載能力強、控制可靠，地形適應能力強的特點，具備極強的在複雜場景中廣泛應用的潛力。
3.然而，要想充分發揮六足機器人的潛力，現實世界中複雜多變的環境對其自主感知、自主決策、自主規劃等自主智能方面提出了很高的要求。受限於當前人工智慧技術的發展水平，現有的自主智能算法的泛化性通用性還不強，達不到完全不出錯的程度，自主系統極易受到環境意外變化的影響，尤其是當環境較為惡劣，不確定性增大時，對感知和決策系統都有著極大的挑戰。


技術實現要素：

4.本發明目的是為了解決六足機器人對複雜環境的適應能力差的問題，提出了一種複雜環境下六足機器人的人機共融智能運動規劃方法。
5.本發明所述的複雜環境下六足機器人的人機共融智能運動規劃方法，具體包括：
6.步驟一、根據所需探測環境地圖及目標位置，建立導引路徑；
7.步驟二、採用三次均勻b樣條算法對導引路徑進行平滑，獲取平滑的b樣條曲線路徑；
8.步驟三、計算平滑的b樣條曲線路徑的環境約束代價和曲線特性約束代價，對b樣條曲線路徑進行優化，獲取優化後的行走路徑；
9.步驟四、根據六足機器人的離散接觸狀態方程和優化後的行走路徑，隨機生成多個狀態序列，並採用單步可達算法判斷所有狀態序列的可達性，若是存在可達的狀態序列，執行步驟五，否則，重新建立引導路徑，返回執行步驟二；
10.步驟五，採用深度優先剪枝搜索算法在可達的狀態序列中搜索獲取存在冗餘的主狀態序列；
11.步驟六、通過分治搜索算法對所述主狀態序列進行優化，實現對六足機器人的運動規劃。
12.進一步地，本發明中，步驟一中，根據所需探測環境地圖及目標位置，建立導引路徑的方法包括：兩種方法，第一種方法：
13.首先確定目標位置p，通過快速擴展隨機樹算法生成一條初步避障路徑，利用公式：
[0014][0015]
計算獲取機器人的前進方向α和前進距離進而獲得一條導引路徑；
[0016]
第二種方法：確定目標點位置p，採用人為手繪的方式獲取一初步條路徑，並利用公式：
[0017]
v＝kmd0
[0018]
獲取機器人的速度矢量和位移矢量；獲取一條導引路徑，式中，d0為把手端部相對於初始點的位移矢量，為虛擬控制對象的速度矢量，km為比例係數。
[0019]
進一步地，本發明中，步驟二中，獲取平滑的b樣條曲線路徑的具體方法為：
[0020]
利用標準b樣條方程：
[0021][0022]
對引導路徑進行平滑，獲取平滑的b樣條曲線路徑，式中,di為第i個控制點，n
i,k
為k次正則b樣條的基函數，n為控制點的個數，u代表一維的狀態參數，p(u)代表該狀態參數u下對應的機器人狀態。
[0023]
進一步地，本發明中，步驟三中，
[0024]
環境約束代價包括：障礙物約束代價；
[0025]
曲線特性約束代價包括：幾何可行落足點密度約束代價，落足點物理特徵約束代價，路徑平滑約束代價和最短路徑約束代價。
[0026]
進一步地，本發明中，步驟三中，對b樣條曲線路徑進行優化的具體方法方法為：
[0027]
建立考慮多約束條件的優化問題模型：
[0028]
min∑q
(.)
(θ)
[0029][0030]
採用梯度下降法對考慮多約束條件的優化問題模型求解，計算獲取滿足代價函數最小的路徑，實現對b樣條曲線路徑進優化；式中，i代表第i個離散點，n為路徑離散點數量，q(.)為障礙物約束代價、幾何可行落足點密度約束代價、落足點物理特徵約束代價、路徑平滑約束代價和最短路徑約束代價中任一代價遍歷離散路徑中所有點後的代價函數，q
(.)
(θi)為θi點的代價；θ為x的歸一化離散路徑點，x為六足機器人身體在笛卡爾空間下實際遵循的離散路徑點序列。
[0031]
進一步地，本發明中，採用梯度下降法對考慮多約束條件的優化問題模型求解的過程為：
[0032]
步驟三一、對θ處朝著最優化方向的各分量的變化量進行估計：
[0033][0034][0035]
表示θ中第i個離散點θi處朝著最優化的變化量的估計，處朝著最優化的變化量的估計，表示以θ
為基準生成的第k條歸一化的隨機路徑離散點序列；ε
θ,k
表示相對於θ的噪聲序列，k表示參與當前迭代輪次估計的隨機路徑離散點序列數量；ε
θ,k
服從均值向量為0，協方差矩陣為r-1
的n維多元正態隨機分布，其中，r採用公式：
[0036][0037]
確定，p
k,i
表示由中第i個歸一化離散路徑點的代價值所決定的概率值；p
k,i
採用公式：
[0038][0039]
確定，式中，表示第k個歸一化隨機路徑離散點序列中第i個離散點；表示處的代價，其中，表示第m個歸一化隨機路徑離散點序列中第i個離散點；λ為給定的參數，給定的參數，
[0040][0041]
對指數項歸一化計算：
[0042][0043]
其中，h為常數；
[0044]
步驟三二、對θ處朝著最優化方向的各分量變化量的估計量進行平滑處理，獲得最終的代價增量δθ：
[0045][0046]
其中，表示組成的各分量增量序列m表示平滑矩陣；所述平滑矩陣為：
[0047][0048]
步驟三三、令θ＝θ+δθ，返回步驟三一，直至考慮多約束條件的優化問題模型的總代價收斂，獲得優化後的離散路徑θ。
[0049]
進一步地，本發明中，步驟四中，六足機器人的離散接觸狀態方程為：
[0050]
φ＝《wrb，wrb，wpf，ff，σf》
[0051]
式中，φ為機器人的離散接觸狀態；wrb為當前狀態機器人機體坐標系∑b相對於世界坐標系∑w的旋轉矩陣；wrb為當前狀態機器人機體坐標系相對於世界坐標系的位移；wpf為當前狀態六個足端相對於世界坐標系的位移；ff為容錯狀態向量，反映當前狀態六條腿的出錯狀態；σf為物理接觸狀態向量，反映當前狀態下各足端與地面的物理接觸情況。
[0052]
進一步地，本發明中，步驟四中，根據六足機器人的離散接觸狀態方程和優化後的行走路徑，隨機生成多個狀態序列，並採用單步可達算法判斷所有狀態序列的可達性的具體方法為：
[0053]
步驟四一、計算每條狀態序列的準靜態穩定與運動學約束，判斷每條狀態序列的準靜態穩定與運動學約束的邏輯狀態是否為真，若是，則執行步驟四二，否則，該條狀態序列不可達；
[0054]
步驟四二、計算狀態序列的無碰撞約束，判斷無碰撞約束的邏輯狀態是否為真，若是，則執行步驟四三，否則，該條狀態序列不可達；
[0055]
步驟四三、通過公式：
[0056]
s＝{j∈z:1≤j≤6,||d(j)||≤ε,σ
f,0
(j)＝σ
f,1
(i)＝1}
[0057]
|s|≥3
[0058]
計算狀態序列中相鄰兩個狀態的公共接觸點數量是否大於等於3，若是，則兩狀態可達，執行步驟四四，否則，該條狀態序列不可達；其中，j為機器人的第j條機械腿，d為相鄰兩狀態足端位置的差值，d(j)為d的第j列，ε為給定的小閾值，σ
f,0
(j)和σ
f,1
(i)均為j號機械腿足端與地面的物理接觸情況，σ
f,1
(i)表示σ
f,s
(j)在集合s中索引的值為1，σ
f,0
(j)表示在集合s中索引σ
f,s
(j)的值為0；
[0059]
步驟四四、判斷機器人相鄰兩狀態的離散接觸狀態方程的運動學約束狀態是否為真，若是，則所述相鄰兩狀態單步可達，否則，單步不可達。
[0060]
進一步地，本發明中，步驟五，採用深度優先剪枝搜索算法在可達的狀態序列中搜索獲取存在冗餘的狀態序列的具體方法為：
[0061]
步驟五一、根據優化後的行走路徑和當前狀態φ
t
確定前進方向vb；
[0062][0063]
其中，r
track
為路徑的跟蹤係數，δrb表示一段路徑的微元，xi代表路徑x上第i個待跟蹤的離散點，r
b,t
表示t時刻機器人坐標系相對於世界坐標系的位移；
[0064]
步驟五二、根據當前狀態φ
t
確定狀態轉移算子中支撐擺動狀態向量cf的所有取值，並通過條件1和條件2對支撐擺動狀態向量cf的所有取值進行篩選，獲取支撐擺動狀態向量cf取值集合sc；
[0065]
條件1：支撐擺動狀態向量cf的某一位索引為1時，必須滿足當前狀態下各腿與地面的物理接觸狀態σf,t對應位索引為1且各腿的容錯狀態ff,t對應位索引為0，表示當某一機械腿要作為支撐腿時必須滿足其與地面物理接觸且不為容錯腿；
[0066]
條件2：支撐擺動狀態向量cf中索引為1的數量大於等於3；表示任一狀態時都至少有3條機械足同時與地面接觸；
[0067]
步驟五三、基於vb和sc初步確定與每一個狀態φ
t
具有狀態單步可達性的下一個狀態φ
t+1
的子節點集合s
φ,t
；
[0068]
步驟五四、計算集合s
φ,t
中最大前進步長l
max
；
[0069]
l
max
＝argmax(||φ:r
b-φ
t
:rb||),φ∈s
φ,t
ꢀꢀꢀ
(0.37)
[0070]
其中，rb為當前狀態機器人世界坐標系的位移，並判斷所述最大前進步長l
max
是否小於給定步長閾值ε0，若是，則停止對下一個狀態的搜索，完成存在冗餘的狀態序列的搜索；否則，執行步驟五五；
[0071]
步驟五五、判斷當前狀態的所有子節點的位置與優化後的行走路徑的末端位置的距離是否小於極小閾值ε1；若是，完成存在冗餘的狀態序列的搜索；否則，執行步驟五六；
[0072]
步驟五六、判斷搜索擴展的次數是否超出極限閾值次數tmax，若是，則完成存在冗餘的狀態序列的搜索，否則，返回執行步驟五三。
[0073]
進一步地，本發明中，步驟六中通過分治搜索算法對所述主狀態序列進行優化的方法為：
[0074]
步驟六一：通過一個隨機狀態轉移算子，考慮無碰撞和運動學等約束，生成趨向於終點的一系列單步可達的離散接觸狀態，形成一條隨機序列；
[0075]
步驟六二：重複步驟六一，直至達到序列生成終止條件，獲取a條隨機序列；執行步驟六三；
[0076]
序列生成終止條件為：1、重複次數達到預設值；2、當隨機序列的離散接觸狀態點聚集在一個狀態點周圍，不再繼續前進，3、當隨機序列的所有離散接觸狀態點與目標狀態點的距離小於距離閾值；
[0077]
步驟六三：對所有狀態節點進行狀態間單步可達性判斷，對單步可達的兩個狀態節點進行連接並標記，獲取多個單步可達的狀態節點；
[0078]
步驟六四：結合所述多個單步可達的狀態節點，使用採用經典的貝爾曼-福特算法求解從起點狀態到終點狀態的最短狀態序列，完成對所述主狀態序列進行優化。
[0079]
本發明中路徑規劃包含兩個步驟：備選路徑集生成和路徑選擇，備選路徑集生成由人機協同的方式進行；路徑選擇的整個框架從人類導引路徑信息輸入開始，直到算法將處理好的信息反饋給人形成一個人機協同的閉環，首先通過觸覺設備人為給出參考的導引路徑，然後進行路徑平滑，對給出的導引路徑進行初步的平滑，以消除導引路徑的突變、抖動、不自然。接著進行路徑優化，特異性的針對六足機器人，考慮複雜環境約束代價和曲線特性約束代價對路徑進行進一步的優化。路徑評估模塊基於深度優先剪枝搜索的主序列生成算法快速生成一條存在狀態冗餘的非最優化狀態序列，並在有限時間內通過分治搜索對其進行序列優化，儘可能逼近蒙特卡洛樹搜索算法得到的最優結果，進而對實際跟蹤序列做出規劃。
附圖說明
[0080]
圖1是本發明所述方法人機協同框架的示意圖；
[0081]
圖2是為直線引導路徑初始路徑示意圖；
[0082]
圖3是由人機協同rrt生成引導路徑初始路徑示意圖；
[0083]
圖4是由人手直接繪製的引導路徑初始路徑示意圖；
[0084]
圖5是分治搜索主序列優化過程示意圖；
[0085]
圖6是實現本發明所述方法的軟硬體架構示意圖。
具體實施方式
[0086]
下面將結合本發明實施例中的附圖，對本發明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發明一部分實施例，而不是全部的實施例。基於本發明中的實施例，本領域普通技術人員在沒有作出創造性勞動的前提下所獲得的所有其他實施例，都屬於本發明保護的範圍。
[0087]
需要說明的是，在不衝突的情況下，本發明中的實施例及實施例中的特徵可以相
互組合。
[0088]
具體實施方式一：下面結合圖1說明本實施方式，本實施方式所述複雜環境下六足機器人的人機共融智能運動規劃方法，具體包括：
[0089]
步驟一、根據所需探測環境地圖及目標位置，建立導引路徑；
[0090]
步驟二、採用三次均勻b樣條算法對導引路徑進行平滑，獲取平滑的b樣條曲線路徑；
[0091]
步驟三、計算平滑的b樣條曲線路徑的環境約束代價和曲線特性約束代價，對b樣條曲線路徑進行優化，獲取優化後的行走路徑；
[0092]
步驟四、根據六足機器人的離散接觸狀態方程和優化後的行走路徑，隨機生成多個狀態序列，並採用單步可達算法判斷所有狀態序列的可達性，若是存在可達的狀態序列，執行步驟五，否則，重新建立引導路徑，返回執行步驟二；
[0093]
步驟五，採用深度優先剪枝搜索算法在可達的狀態序列中搜索獲取存在冗餘的主狀態序列；
[0094]
步驟六、通過分治搜索算法對所述主狀態序列進行優化，實現對六足機器人的運動規劃。
[0095]
進一步地，本發明中，步驟一中，根據所需探測環境地圖及目標位置，建立導引路徑的方法包括：兩種方法，第一種方法：
[0096]
首先確定目標位置p，通過快速擴展隨機樹算法生成一條初步避障路徑，利用公式：
[0097][0098]
計算獲取機器人的前進方向α和前進距離進而獲得一條導引路徑；
[0099]
第二種方法：確定目標點位置p，採用人為手繪的方式獲取一初步條路徑，並利用公式：
[0100]
v＝kmd0
[0101]
獲取機器人的速度矢量；獲取一條導引路徑，式中，d0為把手端部相對於初始點的位移矢量，v為虛擬控制對象的速度矢量，km為比例係數。
[0102]
初始導引路徑生成方法，有兩個基本要求，一是對人類有較輕的認知負擔，二是能夠便捷直觀的將人類意圖轉化為計算平臺所理解的輸入信息。基於上述考慮，對人類較為直觀的方式是將六足機器人當成一個質點，藉助主端機器人生成導引路徑。在這個過程中，人類不需要有過重的心智負擔，僅需按照意圖圖2提出的輸入方式輸入目標點和初始點進行初始路徑輸入即可。
[0103]
基於人類輸入信息的導引路徑生成的第一種輸入方式是半自主的輸入，通過人機協同的方式確定一條初始路徑。人類僅需通過主端機器人設定一目標位置p，然後通過快速擴展隨機樹(rrt)算法快速生成一條粗略的可行避障路徑，如圖3所示。具體來說，操作員通過觸覺設備的前進方向α和前進距離然後可計算得到目標位置p，其映射關係如式：
[0104]
[0105]
另一種輸入方法是手動繪製路徑。當自主規划算法規劃的路徑不能滿足人類的需求時，例如控制機器人接近障礙區域或迫使機器人走特定路徑，可完全手動輸入方法如圖4。虛擬控制對象的速度可以由觸覺設備末端映射得到：
[0106]
v＝k
md[0107]
式中，d為把手端部相對於初始點的位移矢量，v為虛擬控制對象的速度矢量。
[0108]
進一步地，本發明中，步驟二中，獲取平滑的b樣條曲線路徑的具體方法為：
[0109]
利用標準b樣條方程：
[0110][0111]
對引導路徑進行平滑，獲取平滑的b樣條曲線路徑，式中,為第ib個控制點，為k次正則b樣條的基函數，n為控制點的個數，u代表一維的狀態參數，p(u)代表該狀態參數u下對應的機器人狀態。
[0112]
進一步地，本發明中，步驟三中，
[0113]
環境約束代價包括：障礙物約束代價；
[0114]
曲線特性約束代價包括：幾何可行落足點密度約束代價，落足點物理特徵約束代價，路徑平滑約束代價和最短路徑約束代價。
[0115]
本實施方式中，特別針對六足機器人面對極端稀疏落足點的複雜環境，提煉出如下幾個代價：
[0116]
a)障礙物約束代價：將完全不可通行的區域比如超出六足機器人尺寸極限而無法跨越的物體、大坡度地形等處理為障礙物，然後建立障礙物地圖的有符號梯度場(sdf)，sdf反映了點到障礙物邊界的最近距離，障礙物外部的sdf值為正值，障礙物內部的sdf值為負值；其代價為：
[0117][0118]
這個代價保證了優化的路徑朝著不與障礙物發生碰撞的趨勢進行。該項代價值越小，機器人越不容易與障礙物發生碰撞，式中，其中xi為離散路徑點序列中第i個元素，||x
i+1-xi||表示面對障礙物代價較大的地方應該更快的通過，六足機器人可被一個半徑為r，球心在機器人中心的球包絡，基於安全性的考量，留出安全裕度ε1，為障礙物約束代價函數，θi為路徑第i個離散點。
[0119]
b)幾何可行落足點密度約束代價：在對感知到的環境進行處理後，可以得到二值化的六足機器人可落足和不可落足的離散點，為了使六足機器人更容易通過極端稀疏的離散落足點地形，該項代價值越小，路徑更加傾向於落足點密度較大的區域。進而有可行落足點密度代價如式：
[0120]qgf
(θi)＝k
gf
(n
s,max-ds(θi))
[0121]
式中，ds(θi)表示θi對應的xi周圍面積為s的區域內離散可落足點數目，n
s,max
表示理論上對應區域內落足點數目的最大值；k
gf
表示可行落足點密度代價的權重。
[0122]
c)落足點物理特徵約束代價：在野外環境中機器人不僅需要處理幾何障礙物，還需要面對具有不同物理性質的地形，而不同物理性質的地形具有不同的通行代價，例如在
乾燥的石板上行走的成本遠高於在溼滑的冰上行走的成本。因此，規劃的路徑應該傾向於走較為安全的地形而避免到不安全可能造成失穩的地形附近。考慮了物理特徵成本得到的代價公式為：
[0123][0124]
式中，k
j,slip
為歸一化數值，表示地面材料切向的性質，k
j,fall
表示地面材料法向性質，k
pf
為環境物理特徵代價的權重。j是路徑第j個離散點θj的標號；
[0125]
d)路徑平滑約束代價：為避免在優化過程中對路徑的平滑性造成影響，這裡引入平滑代價，定量由沿路徑的加速度平方和表示；該項代價值越小，則路徑曲線的加速度平方和越小，路徑越光滑。
[0126]qsm
(θ)＝k
sm
θ
t
aa
t
θ
[0127]
式中，k
sm
為路徑平滑代價的權重，θ為x的歸一化離散路徑點；
[0128][0129]
e)最短路徑約束代價：實踐表明，僅考慮上述代價而不考慮最短路徑代價時會造成路徑在面對複雜環境時的迭代過程中出現過分的纏繞與彎曲，進而使得路徑特性不能達到預期效果。在此引入最短路徑代價，與其他代價共同決定最終的生成路徑。最短路徑代價表示如式：
[0130][0131]
式中，k
sp
表示最短路徑代價的權重，n為路徑離散點數量，θn為離散路徑第n個離散點，θj離散路徑第j個離散點。
[0132]
進一步地，本發明中，步驟三中，對b樣條曲線路徑進行優化的具體方法方法為：
[0133]
建立考慮多約束條件的優化問題模型：
[0134]
min∑q
(.)
(θ)
[0135][0136]
採用梯度下降法對考慮多約束條件的優化問題模型求解，計算獲取滿足代價函數最小的路徑，實現對b樣條曲線路徑進優化；式中，i代表第i個離散點θi的標號，n為路徑離散點總數量，q(.)為障礙物約束代價、幾何可行落足點密度約束代價、落足點物理特徵約束代價、路徑平滑約束代價和最短路徑約束代價中任一代價遍歷離散路徑中所有點後的代價函數，q
(.)
(θi)為θi點的代價；θ為x的歸一化離散路徑點，x為六足機器人身體在笛卡爾空間下實際遵循的離散路徑點序列。
[0137]
進一步地，本發明中，採用梯度下降法對考慮多約束條件的優化問題模型求解的過程為：
[0138]
步驟三一、對θ處朝著最優化方向的各分量的變化量進行估計：
[0139][0140][0141]
表示θ中第i個離散點θi處朝著最優化的變化量的估計，處朝著最優化的變化量的估計，表示以θ為基準生成的第k條歸一化的隨機路徑離散點序列；ε
θ,k
表示相對於θ的噪聲序列，k表示參與當前迭代輪次估計的隨機路徑離散點序列數量；ε
θk
服從均值向量為0，協方差矩陣為r-1
的n維多元正態隨機分布，其中，r採用公式：
[0142][0143]
確定，p
k,i
表示由中第i個歸一化離散路徑點的代價值所決定的概率值；p
k,i
採用公式：
[0144][0145]
確定，式中，表示第k個歸一化隨機路徑離散點序列中第i個離散點；表示處的總代價，其中，表示第m個歸一化隨機路徑離散點序列中第i個離散點；λ為給定的參數，處的總代價：
[0146][0147]
其中，為處的障礙物約束代價，為幾何可行落足點密度約束代價，為落足點物理特徵約束代價，q
sm
(θ)為路徑平滑約束代價，q
sp
(θ)為最短路徑約束代價；
[0148]
對指數項歸一化計算：
[0149][0150]
其中，h為常數；
[0151]
步驟三二、對θ處朝著最優化方向的各分量變化量的估計量進行平滑處理，獲得最終的代價增量δθ：
[0152][0153]
其中，表示組成的各分量增量序列m表示平滑矩陣；所述平滑矩陣為：
[0154][0155]
步驟三三、令θ＝θ+δθ，返回步驟三一，直至考慮多約束條件的優化問題模型的總
代價收斂，獲得優化後的離散路徑θ。
[0156]
進一步地，本發明中，步驟四中，六足機器人的離散接觸狀態方程為：
[0157]
φ＝《wrb，wrb，wpf，ff，σf》
[0158]
式中，φ為機器人的離散接觸狀態；wrb為當前狀態機器人機體坐標系∑b相對於世界坐標系∑w的旋轉矩陣；wrb為當前狀態機器人機體坐標系相對於世界坐標系的位移；wpf為當前狀態六個足端相對於世界坐標系的位移；ff為容錯狀態向量，反映當前狀態六條腿的出錯狀態；σf為物理接觸狀態向量，反映當前狀態下各足端與地面的物理接觸情況。
[0159]
進一步地，本發明中，步驟四中，根據六足機器人的離散接觸狀態方程和優化後的行走路徑，隨機生成多個狀態序列，並採用單步可達算法判斷所有狀態序列的可達性的具體方法為：
[0160]
步驟四一、計算每條狀態序列的準靜態穩定與運動學約束，判斷每條狀態序列的準靜態穩定與運動學約束的邏輯狀態是否為真，若是，則執行步驟四二，否則，該條狀態序列不可達；
[0161]
步驟四二、計算狀態序列的無碰撞約束，判斷無碰撞約束的邏輯狀態是否為真，若是，則執行步驟四三，否則，該條狀態序列不可達；
[0162]
步驟四三、通過公式：
[0163]
s＝{j
foot
∈z:1≤j
foot
≤6,||d(j
foot
)||≤ε2,σ
f,0
(j
foot
)＝σ
f,1
(i
foot
)＝1}
[0164]
|s|≥3
[0165]
計算狀態序列中相鄰兩個狀態的公共接觸點數量s是否大於等於3，若是，則兩狀態可達，執行步驟四四，否則，該條狀態序列不可達；其中，j
foot
為機器人的第j
foo
t條機械腿的索引，d為相鄰兩狀態足端位置的差值，d(j
foot
)為d的第j
foot
列，ε2優選為0.4，σ
f,0
(j
foot
)和σ
f,1
(i
foot
)為j
foot
號和i
foot
號機械腿足端與地面的物理接觸情況，σ
f,1
(if
oot
)表示σ
f,s
(i
foot
)在集合s中索引的值為1，σ
f,0
(j
foot
)表示在集合s中索引σ
f,s
(j
foot
)的值為0；
[0166]
步驟四四、判斷機器人相鄰的兩狀態是否均滿足靜穩態約束，若是則兩狀態單步可達否則，單步不可達。
[0167]
本實施方式中，進行多約束條件下狀態單步可達性判斷：
[0168]
a)無碰撞約束及其判別：六足機器人的運動規划過程需要接收上遊算法模塊給出的環境信息，包括無法跨越需要繞過的障礙物和用於支撐六足機器人運動的地面離散可落足點。記環境中障礙物點組成的集合為o
obs
，同時注意到在實際情況中障礙物往往是分塊的獨立障礙物，記第k個連通的障礙物點組成的集合為o
obsk
，有成立，k的取值範圍由實際分塊獨立連通障礙物的個數k決定；離散可落足點組成的集合為o
fhold
。集合中各元素的值為該點在世界坐標系下的位置。
[0169]
基於上述符號定義，六足機器人的各腿在運動過程中不能發生碰撞，具體可建模為式：
[0170][0171]
式中，分別表示第i
foot
和第j
foot
條腿在角度配置為和情況下空間上的碰撞體積，除此之外，六足機器人各部分不能與環境發生碰撞。對六足機器人
整體的任意關節配置qa，表示空集，與環境不發生碰撞的無碰撞約束定義如式：
[0172][0173]
表示狀態φ的無碰撞約束邏輯狀態為真；
[0174]
當某一狀態φ滿足無碰撞約束可記作如式：
[0175]
collifree(φ)＝true
[0176]
在進行碰撞判別時，主要的思想是儘可能減少不必要檢測的次數，能夠自適應的根據實際機器人與環境障礙物的相對位置關係進行高效的判別。能夠支撐這種判別特性的數據結構是自頂向下由大範圍到局部細節的層次包圍樹。基於兩個構建好的層次包圍樹進行碰撞判別時，有兩個重點步驟需要關注，一是對兩個層次包圍樹碰撞判別的遍歷順序，需要在判別不可能發生碰撞時提前剪枝退出，儘可能提高效率；二是對於在判別過程中樹與樹的節點間的碰撞判別應該儘可能快速、魯棒。
[0177]
b)準靜態穩定與運動學約束：給定一六足機器人狀態φ，在該狀態下六足機器人應該滿足準靜態穩定約束和運動學約束。對六足機器人狀態φ而言，六條腿中表示與地面是否接觸，能否受到地面作用力的足端組合為σf，該向量中腿編號索引值為1的，並且該編號索引的腿部出錯狀態ff對應值為0的腿編號構成集合k0，集合中元素個數為k0。則由牛頓-歐拉公式，穩定態有下式所示關係。
[0178][0179][0180]
式中，為第i
foot
號機械足在世界坐標系下的位置，底面作用到i
foot
號機械足端在世界坐標系下的力矢量，ob質心在世界坐標系下的位置，近似為六足機器人機體的幾何中心，g為重力加速度，m為質量。
[0181]
考慮到六足機器人在行走過程中不應發生打滑，則地面作用到足端的力應始終保持在摩擦錐內，則有無碰撞約束的邏輯表達式成立：
[0182][0183]
式中：γ
1ifoot
為i
foot
號腿足端與地面接觸的一個切向方向，γ
2ifoot
為i
foot
號腿足端與地面接觸的另一個切向方向，n
ifoot
為i
foot
號腿足端與地面接觸的法向方向，μ
ifoot
為i
foot
號腿足端與地面接觸的摩擦係數，對六足機器人狀態φ而言，需要滿足運動學約束，表示狀態φ的準靜態穩定與運動學約束邏輯狀態為真，即為表達式：
[0184]
ki(φ)＝true
[0185]
就本質而言，運動學約束是需要讓任意腿i的關節配置qi滿足如下式約束。
[0186]ql
≤qi≤qu[0187]
對上下界ql和qu的確定，首先為了儘可能避免發生腿與腿之間的碰撞，防止出現不穩定因素，可以適當的在不影響整體的運動性能的前提下限定1號關節的關節角度上下界。
[0188]
對於2、3號關節的關節角度上下界的確定，可以對機器人單腿的雅克比矩陣進行計算，進而由得到的雅克比矩陣計算單腿的奇異點位置，在六足機器人運動的過程中各腿
配置應儘量避免靠近奇異點。基於這樣的考慮，可以得到2、3號關節的關節角度的上下界。在給定各關節角的上下界約束後，對機器人狀態φ，如果逆運動學求解無實數解或得到的各腿關節配置超出了上下界，則表明該狀態不符合運動學約束；否則該狀態符合運動學約束。
[0189]
狀態單步可達性判斷算法
[0190]
時間ti到tf期間，機器人整體配置qa(ti)對應狀態φ0、qa(tf)對應φ1，是否存在一單步狀態轉移算子使得進而能夠生成一高維連續過渡軌跡使得機器人能在滿足各約束的條件下從狀態φ0過渡到狀態φ1。狀態φ0和狀態φ1各數據分別記為
wrb,0
,
wrb,0
,wp
f,0
,f
f,0
,σ
f,0
,和
wrb,1
,
wrb,1
,wp
f,1
,f
f,1
,σ
f,1
,
[0191]
step1：對如下式所示的準靜態穩定與運動學約束的邏輯表示判斷是否為真：如果為假，則兩狀態不可達，退出；否則轉入step2。
[0192]
[ki(φ0)＝true]∧[equil(φ0)＝true]∧[ki(φ1)＝true]∧[equil(φ1)＝true]
[0193]
step2：對無碰撞約束的邏輯表達式所示的無碰撞約束的邏輯表示判斷是否為真：如果為假，則兩狀態不可達，退出；否則轉入step3。
[0194]
[collifree(φ0)＝true]∧[collifree(φ1)＝true]
[0195]
step3：考慮到穩定性約束以及留出一定的穩定餘量，兩狀態的公共接觸點數量應大於等於3，即對邏輯判斷是否為真：如果為假，則兩狀態不可達，退出；否則轉入step4。
[0196]
s＝{i
foot
∈z:1≤i
foot
≤6,||d(i
foot
)||≤ε,σ
f,0
(i
foot
)＝σ
f,1
(i
foot
)＝1}
[0197]
|s|≥3
[0198]
step4：構建新的機器人狀態φ
test0
和φ
test1
，六足機器人足端接觸情況σ
f,s
∈{0,1}，是根據集合s確定的，在集合s中的編號所索引的σ
f,s
的值為1，不在該集合中的編號所索引的σ
f,s
的值為0，判斷機器人狀態φ
test0
和狀態φ
test1
的無碰撞約束的邏輯表達是否為真：如果為假，則表示兩狀態不可達，退出；否則表示兩狀態可達，退出。
[0199][0200]
[equil(φ
test0
)＝true]∧[equil(φ
test1
)＝true]
[0201]
根據上述的4個步驟，即可判斷給定的兩狀態的單步可達性了。值得指出的是，如果根據上述算法判別兩狀態單步不可達，並不意味著在實際情況中就一定不能從前一個狀態轉移到後一個狀態——這可以分為兩種情況，一種是不滿足各種約束的，這種情況一定不能發生轉移；另一種情況是雖然單步不可達，但是前一個狀態可以通過多次單步狀態轉移算子的作用到達後一個狀態，是多步可達的。在離散接觸狀態規劃和路徑評分應用中，利用狀態單步可達性判斷即可完成所有工作，且多步可達的判斷可以很容易從單步可達的累積迭代拓展過去，因此不對多步可達作過多闡述。
[0202]
進一步地，本發明中，步驟五中，採用深度優先剪枝搜索算法在可達的狀態序列中搜索獲取存在冗餘的狀態的主序列的具體方法為：
[0203]
步驟五一、根據優化後的行走路徑和當前狀態φ
t
確定前進方向vb；
[0204][0205]
其中，r
track
為路徑的跟蹤係數，δrb表示一段路徑的微元，xi代表路徑x上第i個離散點(對應離散路徑點序列中第i個元素)，r
b,t
表示t時刻機器人坐標系相對於世界坐標系的位移；
[0206]
步驟五二、根據當前狀態φ
t
，確定狀態轉移算子中支撐擺動狀態向量cf的所有取值，並通過條件1和條件2對支撐擺動狀態向量cf的所有取值進行篩選，獲取支撐擺動狀態向量cf取值的集合sc；
[0207]
條件1：支撐擺動狀態向量cf的某一位索引為1時，必須滿足當前狀態下各機械足與地面的物理接觸狀態σ
f,t
對應位索引為1，且各腿的容錯狀態f
f,t
對應位索引為0，
[0208]
表示當某一機械腿要作為支撐腿時必須滿足其與地面物理接觸且不為容錯腿；
[0209]
條件2：支撐擺動狀態向量cf中索引為1的數量大於等於3；表示任一狀態時都至少有3條機械足同時與地面接觸；
[0210]
步驟五三、基於vb和sc初步確定與每一個狀態φ
t
具有狀態單步可達性的下一個狀態φ
t+1
的子節點集合s
φ,t
；
[0211]
步驟五四、計算集合s
φ,t
中最大前進步長l
max
；
[0212]
l
max
＝argmax(||φ:r
b-φ
t
:rb||),φ∈s
φ,t
[0213]
其中，rb為當前狀態機器人世界坐標系的位移，並判斷所述最大前進步長l
max
是否小於給定步長閾值ε0，若是，則停止對下一個狀態的搜索，完成存在冗餘的狀態序列的搜索；否則，執行步驟五五；ε0的範圍可以為0.2-0.5，根據不同機器人其取值範圍不同；
[0214]
步驟五五、判斷當前狀態的所有子節點的位置與優化後的行走路徑的末端位置的距離是否小於極小閾值ε1；若是，完成存在冗餘的狀態序列的搜索；否則，執行步驟五六；ε1的範圍可以為0.1-0.2，根據不同機器人其取值範圍不同；
[0215]
步驟五六、判斷搜索擴展的次數是否超出極限閾值次數tmax，若是，則完成存在冗餘的狀態序列的搜索，否則，返回執行步驟五三，其中，tmax實際是根據電腦gpu的計算能力確定的。
[0216]
本實施方式中，確定集合s
φ,t
的流程包括以下順序步驟，考慮到六足機器人能夠全向移動，且為了儘可能突出評分用途，不考慮在路徑跟蹤過程中對姿態的約束，令δθb為0，δθb為地形環境特徵的映射表達；
[0217]
step1：根據路徑x和當前狀態φ
t
確定前進方向vb[0218]
路徑的跟蹤係數記作rtrack，rtrack越大對路徑的跟蹤越粗糙，反之越精細；
[0219][0220]
step2：根據當前狀態φ
t
確定狀態轉移算子中支撐擺動狀態向量cf的可能取值，構成集合sc[0221]
當前狀態下各腿與地面的物理接觸狀態為σf,t，各腿的容錯狀態為f
f,t
，則對於cf的確定有如下必須滿足的條件：
[0222]
1、cf的某一位索引為1時，必須滿足σ
f,t
對應位索引為1且f
f,t
對應位索引為0，表示
當某一腿要作為支撐腿時必須滿足其與地面物理接觸且不為容錯腿。
[0223]
2、cf中索引為1的數量大於等於3，這是進行穩定轉移的必要條件。基於上述條件可取的cf值可構成一集合sc。
[0224]
step3：基於vb和sc確定所有可能的與φ
t
具有狀態單步可達性的下一個狀態φ
t+1
，構成集合s
φ,t
對集合sc內的任一支撐擺動狀態向量，以vb為六足機器人機體的前進方向，在該方向上機體的前進距離記作l；擺動腿的落足點位置為在機體前進l距離後各擺動腿滿足運動學約束的任意可落足位置，對應擺動腿沒有落足點則記為容錯腿(對於本實施方式所提出的路徑的評分需求，不考慮腿部出現驅動異常，假設所有腿都是正常可動的)，放置容錯腿足端到相對於機身固定不與地面接觸的位置，支撐腿的足端位置不變；基於上述描述即可得到狀態轉移算子進而能得到作用算子後所有可能的下一狀態φ
t+1
。對所有可能的下一狀態按照上一節所述的保證狀態單步可達性的判斷條件進行篩選，去除掉不能單步可達的狀態，最終構成集合s
φ,t
。
[0225]
step4：至此，每一個狀態φ
t
都能夠在當前狀態與給定路徑的相對關係下生成一組新的狀態φ
t+1
組成的集合s
φ,t
構成該狀態的兒子節點，形成一個不斷擴展的樹形結構，這個樹形結構構成了生成序列的指數增長的搜索空間。在集合s
φ,t
中應有一最大前進步長，記作lmax，其計算如下式：
[0226]
l
max
＝argmax(||φ:r
b-φ
t
:rb||),φ∈s
φ,t
[0227]
可以看到，在主序列生成算法中，當到達當前狀態單步可達的所有的下一狀態的步長均小於某一給定較小閾值ε0時，則認為當前狀態的後續狀態空間不需要繼續搜索了，直接進行剪枝，回溯到父節點。
[0228]
當出現搜索的當前狀態顯示的位置r
b,t
與路徑的末位置xn在一定極小閾值ε1下接近，或者搜索擴展的次數超出計算資源所決定的計算極限次數t
max
，則表示完成了整個主序列生成過程，進而退出搜索。
[0229]
在整個搜索過程中應該不斷更新最遠能夠到達的序列κm，並在算法運行退出時作為最終結果。從算法的偽代碼可以看到，在搜索的過程中傾向於走更大的步長並在搜索完畢較大的步長後會回退到更小的步長進行搜索。最終通過基於深度優先的剪枝搜索算法生成了一條初始粗略的由離散接觸狀態組成的主序列，各相鄰狀態滿足單步可達性條件。
[0230]
進一步地，結合圖5說明本實施方式，本實施方式中，步驟六中通過分治搜索算法對所述主狀態序列進行優化的方法為：
[0231]
步驟六一：通過一個隨機狀態轉移算子，考慮無碰撞和運動學等約束，生成趨向於終點的一系列單步可達的離散接觸狀態，形成一條隨機序列；
[0232]
步驟六二：重複步驟六一，直至達到序列生成終止條件，獲取a條隨機序列；執行步驟六三；
[0233]
序列生成終止條件為：1、重複次數達到預設值；所述重複次數是根據計算資源確定的；2、當隨機序列的狀態點聚集在一個路徑狀態點周圍，不再繼續前進；3、當隨機序列的所有狀態點與目標狀態點的距離小於距離閾值；即，生成的狀態序列在一定閾值內接近目標狀態序列；
[0234]
步驟六三：對所有狀態節點進行狀態間單步可達性判斷，對單步可達的兩個狀態節點進行連接並標記，獲取多個單步可達的狀態節點；
[0235]
步驟六四：結合所述多個單步可達的狀態節點，使用採用經典的貝爾曼-福特算法求解從起點狀態到終點狀態的最短狀態序列，完成對所述主狀態序列進行優化。
[0236]
基於分治搜索的主序列優化算法：
[0237]
接下來基於上述初步生成的主序列，在有限時間片內採用分治搜索的方法，進而對主序列進行進一步優化，儘可能減少狀態冗餘。
[0238]
先以當前狀態的機體位姿為基礎隨機確定一傾向於向終點狀態收斂的滿足拓展約束的機體位姿；然後根據當前環境的可落足點情況，隨機確定不與當前狀態的容錯向量及物理接觸向量發生衝突且能夠保證轉移過程準靜態穩定的支撐擺動狀態向量；最後隨機確定轉移後的機體位姿下滿足無碰撞約束和運動學約束的擺動腿落足點，並判斷在轉移後的機體位姿下支撐腿是否滿足無碰撞約束和運動學約束。分治搜索算法的示意圖如圖三所示。
[0239]
值得指出的是，在圖三中，序列連接的實線箭頭也是具有狀態間單步可達性的，只是為了標示單個序列的行進方向賦予其箭頭。分治搜索並基於圖的最短路徑優化能夠有效降低時間複雜度，將一個較大規模的優化問題轉化為多個小規模的子問題，線性化的降低了整個問題的時間複雜度，非常有利於並行化執行，這對於快速路徑評分應用來說具有非常大的優勢。
[0240]
快速路徑評分算法：
[0241]
基於上述兩個步驟，構成整個面向交互式應用的快速路徑評分算法。記運行前兩節算法後得到的離散接觸狀態序列為κ
result
，沿用上文中提到的對於路徑的表示為x。則對於路徑評分的第一項可表述如式：
[0242][0243]
式中：r
bg
為序列κ
result
的最終狀態所對應的機體位置。xi為待跟蹤路徑的第i個離散路徑點。
[0244]
對於路徑評分的第二項可表述如下式：
[0245][0246]
式中：κ
result
(i)—為序列κ
result
的第i個離散接觸狀態，n為離散點的個數；
[0247]
則對於路徑評分可表示為如下式：
[0248]
ξ＝ω1ξ1+ω2ξ2[0249]
式中：ω1為路徑評分第一項的權重；ω2為路徑評分第二項的權重。
[0250]
以蒙特卡洛樹搜索算法作為實際跟蹤路徑的離散接觸狀態規划算法，本章提出的快速評分算法對實際的跟蹤效果能夠作出有效預測。路徑評分的第一項表示對實際跟蹤路徑的可通過性或完成程度進行預測評估，該項分值越大，則表明該路徑將越容易被跟蹤，路徑的完成程度將越高。路徑評分的第二項對實際跟蹤路徑所生成的離散接觸狀態序列的平均步長進行預測評估，該項分值越大，表明實際跟蹤該路徑將生成具有較大平均步長的高
質量狀態序列。
[0251]
綜上所述，可以發現在快速路徑評分算法中，充斥著大量狀態單步可達性判斷算法的應用，同時分治搜索和隨機序列的生成也對算法的並行化增加了更多可能，得益於計算機技術的飛速發展以及gpu架構的獨特性，對gpu的充分利用可以極大地加速上述評分過程。
[0252]
人機協同系統框架構建
[0253]
整個軟硬體架構包括人機互動系統、機載計算系統、機載運動控制系統、六足機器人硬體平臺這幾個部分，綜合形成仿真和實物系統，如圖6。各部分通過有線通信或無線通信進行連接。無線通信採用tcp/ip協議，能夠保證數據的可靠傳輸；有線通信中，各傳感器信息和電機驅動控制信息通過有線的相關協議進行有效通信。
[0254]
1)人機互動系統
[0255]
人機互動系統負責對機器人端反饋的信息進行可視化顯示，以供人對機器人系統進行狀態監測和行為評估，進而人通過六自由度觸覺力反饋設備進行協同的運動規劃以及採用各種交互方式在出現異常時快速介入、在關節層精細的對六足機器人進行調整、基於簡單的基本步態對六足機器人進行直接控制等。
[0256]
六自由度觸覺力反饋設備由3d systems公司研發，其具有六個輸入自由度，並具有三個以力矩形式輸出的三個自由度。基於此設備，如前文所述，人得以與六足機器人系統進行協同的運動規劃。當機器人出現異常時，人可以通過基於mfc編寫的ui界面對六足機器人進行關節層的精細控制，也可以發送急停指令保證機器人的安全，還可以發送基本的步態指令使機器人按照完全給定的周期步態序列進行無感知信息的簡單移動。有時考慮到ui界面調整不方便、不自然，可以輔以語音交互的方式，定義一些基本的動作模組和元指令。多模態的信息輸入方式讓人更好的參與到六足機器人的運動規划過程，也促進了人機智能的融合。
[0257]
對於反饋信息來說，有多種層次和維度的反饋。具體包括：對於路徑規劃，會直觀的將路徑可視化到回傳圖像上，以輔助人進行路徑選擇；感知層的建圖信息以grid_map的形式]可視化在rviz中；讀取機器人定位信息和關節角信息，基於urdf將六足機器人的狀態可視化在rviz中；在機器人實際執行下一步行走之前，會將規劃的關節角度序列可視化，只有人經由可視化判斷沒有出現問題才實際執行，這樣提升了冗餘安全性，可以有效防止定位、感知建圖、規劃的某一模塊出現問題時下發到執行層的命令無效導致安全風險的出現。直觀、自然、信息量充分的反饋界面也更好的促進了人機智能的融合。本系統與機載計算系統由區域網連接，受益於ros的多機分布式網絡通信架構，以機載計算系統作為ros的主核心，人機互動系統啟動的節點能夠在進行適當的配置後通過無線區域網與主核心進行通信，進而能夠像在同一臺計算機上一樣，訂閱在機載計算系統中的話題，進而對感興趣的信息進行可視化。
[0258]
2)機載計算系統
[0259]
機載計算系統主要負責進行環境感知、機器人狀態估計、路徑規劃、離散接觸狀態規劃、過渡軌跡規劃、人類輸入信息處理等功能的計算。該系統在上下遊關係中起到承上啟下的作用，是整個軟硬體架構中最核心的部分。該系統的硬體基礎可以是常規的個人pc主機，也可以是英偉達的嵌入式主機。為了保證速度和資源利用的充分性，核心算法由c++實
現，並部署在ros(robot operating system)作業系統上。
[0260]
ros系統是一個部署在linux系統上的具有單主核心、多節點通過tcp/udp協議進行異步通信、能夠進行基於服務的同步遠程過程調用機制通信的機器人分布式框架。每一個節點可以看做是一個進程，每個節點內又可以開啟多個線程，通過合理的軟體架構設計，計算資源得以充分有效的利用。以機載計算系統為主核心，受益於ros的多機分布式網絡通信架構，人機互動系統通過區域網與機載計算系統進行通信，但是在實踐中發現，建立的通信是不穩定的，偶爾會出現預期功能沒有順利進行，重啟所有節點才恢復的問題；這主要是由於ros的運行機制是所有節點信息交互完全通過一個主核心進行，一旦主核心由於某種原因掛掉，整個系統就會崩潰，無法按照預期功能進行。ros2從本質上解決了這個問題，即便一個節點掛掉也可以通過其他節點進行通信，提高了系統的魯棒性，但是，考慮到ros2的開源社區相對不太完善，且對於本文研究來說並沒有涉及到過多獨立系統的交互，因此ros2架構的應用留作後續研究。
[0261]
接下來介紹機載計算系統中基於ros的軟體架構設計。如圖五是各處理進程節點與緩存數據話題的架構關係。各節點從話題中訂閱數據，並基於數據運行核心程序，將處理結果發布到圖中指向的話題中。規劃的關節角度序列會發送到人機互動界面中進行顯示，如經由人判斷運行正常，則下發到實際機器人進行執行或者在gazebo中進行仿真。人機協同的路徑規劃模塊讀取人類輸入信息和環境地圖，不斷的生成融合人類意圖的待評估路徑，路徑評估模塊動態的對待評估路徑進行評估並反饋給人機互動界面供人決策，最終選定路徑下發到離散接觸狀態規劃模塊。其餘模塊在圖中可以很容易知道對應的意思，在此就不明確闡述。
[0262]
在整個軟體架構的實現過程中，需要儘量提升軟體系統的可擴展性、可移植性、魯棒性以及用戶友好性。可擴展性保證了在之後的系統軟體開發過程中能夠在兼容舊有功能的基礎上快速開發新功能，在各個核心環節快速部署新的算法。這就需要充分利用c++面向對象的編程特性，充分利用封裝、繼承、多態的編程思想，以及創建型模式、結構型模式、行為模式等設計模式，仔細設計數據結構和各類之間的相互關係，設計出具有高可擴展性的機器人軟體系統。以本系統為例，路徑規劃和離散接觸狀態規劃在今後的算法研究過程中要進行經常性的版本迭代和替換，需要利用c++的多態特性，提取出所有規划算法的公有接口，以抽象基類的形式存在，在實現新的算法時只需要繼承該抽象基類，實現對應的接口函數即可便捷的在整個系統應用所實現算法。同時，整個系統的架構也應該儘量模塊解耦，避免出現過度耦合，導致修改擴展功能時牽一髮而動全身。可移植性保證了軟體系統能夠在不同參數同類構型的機器人上不出錯的快速部署，並對於上層智能算法來說能夠將底層實物機器人的控制和仿真環境機器人的控制進行封裝，進而編寫出兼容異構化底層的軟體系統。以本系統為例，仿真機器人和實物機器人在整個系統中的核心區別在於傳感器數據的讀取方法不同以及發送的角度序列控制機器人的機制不同。前者的解決方案仍舊利
[0263]
用c++的多態機制，抽象出相同接口，派生出因仿真、實物而不同的傳感器數據讀取類；後者的解決方案受益於ros的分布式節點架構，通過運行不同機制的控制節點即可便捷切換實物機器人及仿真機器人。在實際的算法開發過程中，考慮到實物場景的危險性以及實驗場景搭建繁瑣，在算法研究階段主要在仿真環境中進行迭代，算法驗證階段在實物場景中進行。
[0264]
魯棒性保證了程序在面對各種異常場景能夠健壯運行，這一方面可以由具有高度自適應性的自主智能算法來保證，另一方面在算法的工程實現過程中也應該充分考慮到可能出現的各種邊界條件，並採用高級程序語言的異常處理機制進行處理；同時也應該充分考慮到內存洩漏的問題，防止因內存洩漏造成長時間運行時上層應用程式的奔潰。以本系統為例，在工程實現過程中，大量應用了c++的trycatch機制，出現的異常得以有效及時的處理；同時，採用智能指針和引用傳值，避免出現內存洩漏和大量的無用內存拷貝，提升了整個系統的運行速度和魯棒性。
[0265]
用戶友好性保證了在基於已有程序進行功能擴展和二次開發時，能夠極大限度降低用戶錯誤使用基礎的工具函數的概率，以及能夠使得用戶在符合現有架構的基礎上進行工程開發而不對現有架構造成破壞。以本系統為例，充分利用c++的封裝特性，仔細設計各類的公有成員，儘可能做到對細節的有效封裝和清晰明確的成員定義。
[0266]
3)機載運動控制系統
[0267]
機載運動控制系統接收上層的命令以及實際的傳感信息，直接控制關節電機驅動系統，並在過程中把一些關鍵信息傳輸到機載計算系統。由於機載運動控制系統直接與底層硬體平臺交互，因此無論是其硬體層面還是軟體層面，可靠性和穩定性都至關重要。因此，選用倍福的工業pc作為機載運動控制系統的主機，並使用twincat軟體系統，將工業pc搭載的windows系統轉換為一個帶有機器人實時作業系統的實時控制系統。
[0268]
實時作業系統能夠在外界事件或者數據產生時快速做出響應，並在規定時間內調度一切可用計算資源完成實時任務。這非常適合於具有固定控制間隔的周期性控制任務，能夠保證控制的穩定性，不至於出現間隔不定而導致跟蹤發散的問題。基於實時作業系統，機載運動控制系統能夠控制機器人穩定可靠的跟蹤上層算法規劃的關節角度序列。
[0269]
機載運動控制系統還能夠直接接收從人機互動系統中輸出的關節層和急停指令，能夠可靠穩定的對人類的輸入信息進行響應。本系統還實現了基本的周期性步態控制、機體位姿控制，在實際應用中能夠幫助人進行基本的實驗調試和異常處理。
[0270]
首先考慮到自主智能對複雜環境下場景的泛化性和通用性處理能力還不夠強的特點，本發明通過結合人的智能，提出了一種閉環人機協同路徑規劃方法，該方法能夠在考慮不同環境代價和兼容人類啟發函數的同時生成安全、平滑的路徑。針對人不擅長於進行高維空間計算的特點、本發明考慮六足機器人的準靜態穩定約束、運動學約束構建了狀態單步可達性判斷算法，提出了一種基於深度優先剪枝搜索的主序列生成算法對路徑的可跟蹤性進行評估，對備選路徑實際的易跟蹤性及跟蹤質量以路徑分值的形式作出了有效預測，進而作為在線交互式的有效信息幫助人進行路徑選擇。本發明還通過將步態、步長、機體位姿和立足點的規劃建立為一個序列優化問題，提出一種滑動蒙特卡洛樹搜索得方法，該方法通過引入滑動根節點有效平衡了算法的搜索時間和採樣充分性，大大提高機器人在稀疏落足點環境下的通行能力。本發明基於ros系統，設計了一個考慮人機互動的六足機器人智能規劃軟體框架，該系統充分利用封裝、繼承、多態的編程思想，以及創建型模式、結構型模式、行為模式等設計模式，仔細設計數據結構和各類之間的調用關係，設計出具有高可拓展性的規劃軟體框架。
[0271]
雖然在本文中參照了特定的實施方式來描述本發明，但是應該理解的是，這些實施例僅僅是本發明的原理和應用的示例。因此應該理解的是，可以對示例性的實施例進行
許多修改，並且可以設計出其他的布置，只要不偏離所附權利要求所限定的本發明的精神和範圍。應該理解的是，可以通過不同於原始權利要求所描述的方式來結合不同的從屬權利要求和本文中所述的特徵。還可以理解的是，結合單獨實施例所描述的特徵可以使用在其他所述實施例中。