曲率可控雙輪自平衡機器人路徑規劃

牛雪娟，陳國振，孫宏圖

（天津工業大學機械工程學院，天津 300387）

1 引言

雙輪自平衡機器人屬于輪式移動機器人中的一種，其路徑規劃是一個復雜的問題。常用的路徑規劃算法只關注了路徑的有無碰撞，而對路徑的可行性沒有給予過多的關注。這是因為路勁規劃對輪式移動機器人的運動學或其他幾何約束不予考慮。

雙輪自平衡機器人的特點是零半徑回轉、占地面積小和重心高[1]。正是由于其重心高的特點，在高曲率軌跡曲線快速轉向時有側向傾覆的危險。因此期望雙輪自平衡機器人的運動軌跡是一條低曲率軌跡曲線。常用的平滑軌跡曲線有回旋曲線，貝賽爾曲線，立方螺旋線和B 樣條曲線[2-5]。文獻[3]運用分段貝塞爾曲線克服了高連續性需要高階數曲線的問題，使得軌跡曲線具有低階數、高連續性的特點，但是由于貝賽爾曲線的固有特性使得軌跡曲線靈活性差，不利于調節曲線曲率。文獻[4]運用立方螺旋線作為過渡曲線連接直線與弧線，保證軌跡的曲率連續性，但其弧線曲率為定值，在轉向時需要進行速度規劃。文獻[5]提出了一種速度曲率函數，使得機器人以變速度轉向，減小了離心力對其平穩性的影響，但是其主要使用于車輪在三個及以上的機器人。對于雙輪自平衡機器人而言，其在加減速的運動過程中為了保持車身的運動平衡會出現前后擺動的現象。

因此，采用三次B 樣條曲線作為軌跡曲線，在保證軌跡曲線曲率連續性[6]的基礎上，根據其基函數的局部支撐性，提出一種控制點的移動方法，使得其滿足最大曲率約束條件，減小因轉向速度快和重心高引起的車身傾斜，使得雙輪自平衡機器人在無傾斜狀態下以較快速度勻速平穩運動。

2 雙輪自平衡機器人模型建立及分析

2.1 雙輪自平衡機器人運動學建模

由于雙輪自平衡機器人是基于兩輪的速度差來完成曲線運動，故參照差速驅動移動機器人建立運動學模型[7]，如圖1 所示。

圖1 運動學模型Fig.1 Kinematic Model

定義左輪的線速度為vl，右輪的線速度為vr，兩輪間的距離為l。前進速度v 和轉向角速度ω 可以用vl和vr來描述：

顯然，依式（1）可以計算出曲率k，曲率k 描述如下：

從式（2）可以注意到，如果兩車輪以相反的方向轉動，隨著兩車輪線速度在數值上的接近，軌跡曲率值將會一直增大。然而，若期望雙輪自平衡機器人以高線速度運動，那么在實際運動中應該避免兩車輪以異號的線速度來實現急轉彎，因為短時間內的大范圍調速容易造成電機的過載。如果兩車輪線速度均為正值并以某一給定的線速度 v 為上界（即 vl，vr∈［0，v］），則依據公式（2）計算得出的曲率 k 有界，其取值區間為［-2/l，2/l］。

2.2 曲率對雙輪自平衡機器人側向穩定性影響

雙輪自平衡機器人在前進與后退方向上具有動態穩定性，但是在側向上是靜態穩定的。由于這個原因，其在高速運動時轉向受到限制，因為離心力會導致車身不穩定和傾斜，受力分析，如圖2 所示。

圖2 雙輪自平衡機器人受力分析Fig.2 Force Analysis of Two-Wheel Self-Balancing Robot

離心力可以通過下述公式描述：

式中：Fc—離心力；M—雙輪平衡車整體重量；v—前進速度；ω—旋轉角速度；r—旋轉半徑。

在轉向時，小的離心力對雙輪自平衡機器人的側向穩定性不會產生較大的影響，但當其增大到某一值時，其會處于傾斜的一種臨界穩定狀態。當離心力產生的力矩大于重力產生的力矩時，這種穩定狀態將被破壞，使雙輪平衡車發生傾斜。

從圖2 可以得出發生傾斜時離心力臨界值的計算公式：

式中：g—重力加速度；l—兩輪間距離；h—重心的高。

將式（2）和式（3）帶入式（4）中可得出不同前進速度下的臨界曲率，如下：

綜合2.1 和2.2 節所述，假設雙輪平衡車僅在一個方向上運動（如0

通常情況下，雙輪自平衡機器人可以通過限制前進速度和旋轉角速度或增加車輪的寬度來避免這種形式的傾斜。但是這兩種解決方案都不理想，因為第一種解決方案限制了運動速度，第二種解決方案增加了機器人的占地面積。故我們希望通過利用軌跡曲線的靈活性降低曲率值，來保證雙輪平衡車的運動速度和其體積小巧的特點。

3 運動軌跡平滑

3.1 三次B 樣條軌跡曲線生成

三次B 樣條曲線可以表示為：

對于三次B 樣條曲線的一、二階導數可用如下公式計算：

圖3 軌跡曲線對比Fig.3 Trajectory Curve Comparison

圖4 軌跡曲線碰撞Fig.4 Trajectory Curve Collision

根據雙輪自平衡機器人的大小，對地圖上的障礙物進行擴展，將地圖進行柵格化處理。使用A*算法[8]搜索最短路徑，并將搜索到的路徑點作為控制點，生成三次B 樣條曲線來平滑運動軌跡。相比較于軌跡曲線經過路徑點的方法，該方法可以得到更逼近于搜索的路徑且距離更短，如圖3（a）、圖3（b）所示。其中，紅色線為A*算法搜索出的線段路徑，藍色線為平滑的軌跡曲線。但是以路徑點為控制點生成的三次B 樣條軌跡曲線在轉向處會與障礙物發生碰撞，如圖4（a）、圖4（b）所示。為此，我們通過在轉向處兩個連續控制點的中點處插入一個新的控制點，來迫使三次B 樣條曲線[9]逼近控制多邊形，從而達到避障的目的。而由A*算法搜索處的路徑，在轉向處多為斜對角線，則添加的中間控制點就會落于障礙物的直角點上，軌跡曲線仍會發生碰撞，故我們提出一種碰撞檢測方法和解決辦法。

3.2 基于車身邊界條件的軌跡碰撞檢測

使用了高效樣條碰撞檢測算法[10]的思想。三次B 樣條軌跡曲線為 C（x（u），y（u）），障礙物表示為 B，對于?u∈［0，1］無碰撞條件為 C（x（u），y（u））∩B=φ。若檢測到軌跡有碰撞，則對于?u∈［0，1］，有 C（x（u），y（u））∩B≠φ。令 d（C（u），B）為 C（u）和 B 之間的距離。t 是雙輪自平衡機器人車身的邊界條件。如果檢測到碰撞，則ρ≤1。碰撞檢測公式如下：

在式（8）中，雙輪自平衡機器人的車身邊界t 應小于C（u）和B 之間的距離。為使軌跡曲線無碰撞，將轉向分為兩種：分別為同號曲率轉向，異號曲率連續轉向，如圖4（c）、圖4（d）所示。對于同號曲率轉向，將轉向處的中間控制點沿45°線遠離障礙物直角點的方向移動3/2t 的距離，而共線的相鄰控制點分別沿其所在直線向中間控制點移動的距離。對于異號曲率連續轉向，將轉向處共線的三個控制點沿遠離障礙物直角點的方向平移3。

3.3 基于曲率約束的軌跡優化

在軌跡曲線無碰撞的基礎上，判斷軌跡曲線是否滿足雙輪自平衡機器人最大曲率約束，因此需要計算軌跡曲線的曲率。三次B 樣條曲線的曲率可以按照如下公式計算：

其中三次B 樣條曲線的一階導、二階導可由式（7）計算。取dK/du=0，可以求得曲率的極值點，判斷其是否滿足的約束，對于不滿足最大曲率約束條件的區間進行優化處理。因A*算法搜索出的路徑轉向處為斜對角線的特點，所以軌跡曲線曲率極值點出現在控制點連線夾角γ≤90°處，故以γ=90°為界限對三次B 樣條曲線控制多邊形進行修改，來降低曲率。

為了分析控制點移動距離與曲率k 之間的關系，定義如下變量：β 為∠的角度，γ 為∠P1P3P5的角度，b 為的角度，h 為控制點P3到線段的距離，d 為線段的長度，h′為移動控制點P和P后控制點P到線段的距離，如圖5 所243示。其中P2和P4分別為線段的中點，且和分別垂直于線段的。

圖5 軌跡曲線調整Fig.5 Trajectory Curve Adjustment

對于B 樣條曲線已經有一曲率估計公式被廣泛認可[11]，其計算公式為：

同理可得：

將式（11）～式（13）代入式（10）中可得：

由于A*算法搜索到的路徑在進行180°轉向時具有對稱特性，故圖5 中，簡化式（14）得：

將雙輪自平衡機器人最大曲率約束kmax代入式（15）中，可以求得控制點在沿垂直于原直線方向上移動的距離。

算法主要流程為：首先通過A*算法搜索路徑生成線段性軌跡，將搜索到的路徑點作為三次B 樣條軌跡曲線的控制點，然后判斷線段性軌跡的轉向，對轉向段添加中間控制點，并根據轉向類型判斷有無碰撞。若發生碰撞，則依轉向類型移動控制點。其次判斷控制點夾角γ≤90°，若滿足條件，則添加kmax約束計算出控制點的移動距離，更新控制點。最后生成三次B 樣條軌跡曲線，流程圖，如圖6 所示。

圖6 算法流程圖Fig.6 Algorithm Flow Chart

4 案例分析

圖7 軌跡曲線Fig.7 Trace Curve

圖8 曲率曲線Fig.8 Curvature Curve

給出環境地圖，依A*算法搜索最短路徑，并以式（6）生成運動軌跡，如圖7（a）所示。在轉向處軌跡曲線存在碰撞點。根據式（9）計算出的軌跡曲線曲率，如圖8（a）所示。曲率變化范圍為［-1.45，0.5］。給定參數，雙輪自平衡機器人重心高h=0.7m，兩輪間距離 l=0.5m，重力加速度g=9.8m/s2，則曲率k∈［-4，4］。環境地圖以1：5 的比例建立，則t=0.05。期望的軌跡曲線滿足平滑、無碰撞和最大曲率約束三個條件。現以最大前進速度v=1.6m/s 為條件，根據式（5）計算得出的臨界曲率k=1.36。添加中間控制點后軌跡曲線，如圖7（b）所示。曲率曲線，如圖8（b）所示。可以發現增加控制點后軌跡曲線曲率有所增長。利用基于無碰撞和最大曲率約束的軌跡優化方法后，軌跡曲線，如圖7（c）所示。曲率曲線，如圖8（c）所示。優化后的軌跡曲線并未與障礙物發生碰撞，且曲率范圍調整到［-1.32，0.72］，小于臨界曲率k。

5 結論

通過建立雙輪自平衡機器人的運動學模型并分析受力情況，計算得出雙輪自平衡機器人在運動過程中的最大曲率值。以A*算法搜索出的路徑點作為三次B 樣條曲線的控制點，生成運動軌跡。在此基礎上，以給出的碰撞檢測方法，判斷軌跡曲線的碰撞情況，并推導出了控制點移動距離與軌跡曲線曲率k 之間的關系，并以此來控制軌跡曲線曲率，使其滿足雙輪自平衡機器人運動最大曲率約束條件。仿真結果表明，軌跡曲線平滑、無碰撞且曲率控制在最大曲率約束范圍內。