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

牛雪娟，陳國振，孫宏圖

（天津工業(yè)大學(xué)機械工程學(xué)院，天津 300387）

1 引言

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

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

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

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

2.1 雙輪自平衡機器人運動學(xué)建模

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

圖1 運動學(xué)模型Fig.1 Kinematic Model

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

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

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

2.2 曲率對雙輪自平衡機器人側(cè)向穩(wěn)定性影響

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

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

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

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

在轉(zhuǎn)向時，小的離心力對雙輪自平衡機器人的側(cè)向穩(wěn)定性不會產(chǎn)生較大的影響，但當(dāng)其增大到某一值時，其會處于傾斜的一種臨界穩(wěn)定狀態(tài)。當(dāng)離心力產(chǎn)生的力矩大于重力產(chǎn)生的力矩時，這種穩(wěn)定狀態(tài)將被破壞，使雙輪平衡車發(fā)生傾斜。

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

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

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

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

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

3 運動軌跡平滑

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

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

對于三次B 樣條曲線的一、二階導(dǎo)數(shù)可用如下公式計算：

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

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

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

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 應(yīng)小于C（u）和B 之間的距離。為使軌跡曲線無碰撞，將轉(zhuǎn)向分為兩種：分別為同號曲率轉(zhuǎn)向，異號曲率連續(xù)轉(zhuǎn)向，如圖4（c）、圖4（d）所示。對于同號曲率轉(zhuǎn)向，將轉(zhuǎn)向處的中間控制點沿45°線遠離障礙物直角點的方向移動3/2t 的距離，而共線的相鄰控制點分別沿其所在直線向中間控制點移動的距離。對于異號曲率連續(xù)轉(zhuǎn)向，將轉(zhuǎn)向處共線的三個控制點沿遠離障礙物直角點的方向平移3。

3.3 基于曲率約束的軌跡優(yōu)化

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

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

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

圖5 軌跡曲線調(diào)整Fig.5 Trajectory Curve Adjustment

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

同理可得：

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

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

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

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

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

4 案例分析

圖7 軌跡曲線Fig.7 Trace Curve

圖8 曲率曲線Fig.8 Curvature Curve

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

5 結(jié)論

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