移動機器人的運動控制

目的

　　輪式移動機器人鎮定、軌跡跟蹤控制理論和方法入門。

　　　　　　　

1　前言

　　如果你常看歷史劇可能注意到了，古代的車幾乎都是兩輪結構的。無論東方還是西方，不管是戰車還是貨車，古代超過兩輪的車極少。究其原因當然很多，但是最主要的原因是古人一直沒搞明白兩個車輪以上的車怎麼拐彎。別小看這個問題喲，車輛轉彎不是那麼容易的，在結構設計上不簡單，在運動控制上更是極具挑戰性。人類發明車輪是個突破，怎麼把多個車輪用好也是個不小難題。

　　和其它的控制系統一樣，輪式移動機器人的控制任務也可以分成以下兩種：
　　1　鎮定：控制機器人到達並穩定在某個狀態，實際的例子就是把車停到一個指定的車位裏
　　2　跟蹤：控制機器人跟蹤某個運動着的軌跡，實際的例子就是讓汽車沿着車道中心線行駛
　　$\bigstar$　這兩個任務哪個更難呢？
　　我們都知道對於機械臂來說，控制它到某個狀態比控制它跟蹤一個空間軌跡更簡單，所以很多人理所當然地認爲移動機器人的“鎮定”比“跟蹤”更簡單。但是實際情況是鎮定更難，這是因爲運動約束的存在$^{[1]}$。也就是說移動機器人和機械臂的控制難度剛好反過來了。哈哈！有意思吧，下面會具體分析到底爲什麼是這樣。

2　汽車模型

　　一般的汽車都是四輪形式，爲了採用艾克曼轉向機構和差速器，這是最常見的一種轉向形式。在論文中，一般把四輪車簡化爲兩輪的自行車，這樣的簡化不改變問題的本質，而且處理更方便。這樣得到的模型可以用式$(1)$所示的方程來描述，稱爲運動學方程。
$\begin{aligned}\tag{1} &\dot{x}=v cos(\theta)\\ &\dot{y}=v sin(\theta)\\ &\dot{\theta}=\frac{v}{L}tan(\phi) \end{aligned}$

　　式$(1)$中，$(x,y,\theta)$是位置和姿態，或者叫狀態量，是我們想通過控制改變的；而$(\phi,v)$是控制量，是我們能直接改變的，如下圖所示。$\phi$代表自行車前輪的轉角（單位是度或者弧度），$v$代表後輪的速度（單位是m/s）。控制量一般總是受約束的，例如$-30\degree\le\phi\le30\degree$，$-1m/s\le v \le1m/s$。所以簡單來說，移動機器人的控制任務就是通過改變$(\phi,v)$來改變$(x,y,\theta)$。

　　可以看到這個模型並不是很複雜，我們分析一下它有什麼特點：
　　1　從數學的角度看，方程中包含狀態$\theta$的三角函數，所以它是個非線性系統。這是個壞消息，因爲非線性系統的控制一般比線性系統更困難。
　　2　從驅動數量的角度看，模型中控制量的個數少於狀態量的個數（$2<3$），因此它是個欠驅動系統（underactuated），這意味着我們要用少數控制量去影響干預多數狀態量。
　　3　從運動或者約束的角度看，這個模型還包含非完整約束，什麼是非完整約束呢？我們給方程組$(1)$中的第一個等式兩邊乘以$cos(\theta)$，給第二個等式兩邊乘以$sin(\theta)$，然後將兩式相加就會得到式$(2)$，這個式子表示的就是一個非完整約束。區分“完整約束”和“非完整約束”的標準就是能不能積分，如果能將狀態的導數項積分得到代數方程，那就是完整約束。例如$x_1\dot{x}_1+x_2\dot{x}_2=0$可以積分成$\frac{1}{2}x_1^2+\frac{1}{2}x_2^2=c$，$c$是個與初始條件有關的常數。再看式$(2)$則不能積分。對於完整約束，由於可以積分得到代數方程（速度約束變成了位置約束，階數降低），因此可以通過變量替換消去一些狀態變量（例如用$x_1$表示$x_2$從而消去$x_2$），讓問題變得更簡單。但是對於非完整約束就不行了。非完整約束系統總是欠驅動系統。飛機、汽車、船舶都是非完整約束系統，受到轉向半徑的約束。
$\begin{aligned}\tag{2} &cos(\theta)\dot{x}+sin(\theta)\dot{y}=v\\ \end{aligned}$

　　4　它還是個無漂移系統（Driftless），這裏的“漂移”跟賽車沒關係。無漂移的意思是當控制量都爲零時，狀態量不會改變（即導數項也爲零），不給控制輸入系統就靜止不動，這個很容易檢驗。
　　5　這個模型是一組常微分方程，如果控制量只依賴狀態不依賴時間，那麼它還是個自治系統（Autonomous system）。自治系統是針對閉環系統的。
　　6　從輸入控制量的角度看，經過簡單的替換（令$\gamma=\frac{v}{L}tan(\phi)$），它還可以變成仿射非線性系統（Control-affine systems），即模型雖然關於關於狀態量仍然是非線性的，但是關於控制量$(v,\gamma)$是線性的。
　　7　它也是個微分平坦（differentially flat）系統，控制量可以用狀態及其導數來表示，如下式。其中，$dir=sign(\left\langle (\dot{x},\dot{y}),(cos(\theta),sin(\theta))\right\rangle)$表示運動方向。
$\begin{aligned}\tag{3} &v=dir\sqrt{\dot{x}^2+\dot{y}^2}\\ &\phi=arctan(L\dot{\theta}/v)\\ \end{aligned}$

3　可控性

　　瞭解了模型的特點，我們下面最關心的是它的可控性。可控性就是能不能通過施加控制使系統到達狀態空間中的任意一個狀態。可控性自然是很重要的，因爲如果一個系統是不可控的，也就意味着不管你施加怎樣控制量都到不了想要的狀態，那我們就不費勁去尋找一個控制律了。注意可控性研究的是模型自身的性質，與控制量無關，它不關心控制量是多大。
　　$\bigstar$　那麼模型$(1)$是不是可控的呢？
　　我們最熟悉、也是最簡單的是線性系統，翻出你大學的《自動控制原理》課本，可以找到線性系統可控性的充要條件是可控性矩陣是滿秩的。但是很遺憾，我們面對的模型$(1)$是個非線性系統，得尋找更一般的方法。根據Chow的定理$^{[1]}$，模型$(1)$是可控的。

　　　　　

　　可控性回答的是“能不能”的問題，但是它沒有回答“怎麼做”。我們可以設計一個開環的控制（不依賴狀態）到達給定的狀態嗎？完全可以，但是實際系統會有各種誤差和擾動，即使理論計算的很好，實際一用就會出現偏差，所以最好找一個閉環的反饋控制，也就是依賴系統狀態的控制，記成$u=u(x)$。如果還有什麼要求的話，我們希望這個控制是狀態的光滑函數，畢竟常見的初等函數都是光滑的嘛。那麼這樣的要求算不算太高呢？
　　$\bigstar$　模型$(1)$是否存在光滑的反饋控制呢？
　　一個叫Brockett的哥們回答了這個問題：不存在。Brockett在論文《Asymptotic stability and feedback stabilization》中給出了系統存在光滑的反饋控制的必要條件。很遺憾，我們的機器人模型不滿足這個條件。論文使用了微分幾何和Lyapunov穩定理論等工具，非常高深。我看這個結論很少被提到，只有賈欣樂解釋過，所以下面我翻譯一下論文的基本思想。

Brockett

　　Brockett的證明

　　一開始，Brockett考慮的是最一般的自治非線性系統：
$\begin{aligned}\tag{3} \dot{x}=f(x,u)\\ \end{aligned}$　　這個系統的平衡點是$x_0$。啥是平衡點？平衡點就是速度爲零的點，速度爲零意味着系統到達這個點就不動了，也就是到達平衡狀態了，顧名思義。一般我們把想到達的目標狀態設爲平衡點。當然，都到目標點了，控制量也就等於零了。
$\begin{aligned} f(x_0,0)=0\\ \end{aligned}$　　然後作者描述了這篇論文要解決的唯一的一個問題：能不能找到一個光滑的函數$u(x)$使系統$(3)$在平衡點$x_0$處漸進穩定。注意$u(x)$的寫法表示控制量$u$只依賴於狀態$x$，不依賴其它的東西，比如不依賴時間。所以即使你找不到一個光滑$u(x)$，那不意味着你找不到一個光滑的$u(x,t)$，很多後來的學者就是這麼幹的，把時間引進來，或者犧牲光滑性，找一個不連續的解。
　　論文第二部分，Brockett開始給出一些基本定義給後文做鋪墊，但是是用微分幾何的語言描述的。對於沒學過微分幾何的同學，看到這裏估計腿一軟就給跪下了，筆者也是不太懂。不過即使這部分看不懂好像也不太妨礙理解後面的證明部分。當然能掌握微分幾何是最好的，因爲好多論文都使用了微分幾何的工具和術語，如果不懂看起來論文來會很吃力。懂微分幾何的另一個好處是讓你的論文更上檔次，畢竟很多搞機器人研究的數學都停留在初等微積分和線性代數的水平。
　　有一個可微的流形$X$（對於咱們來說，$X$就是移動機器人的所有位姿$(x,y,\theta)$組成的構型空間，所以$X=\mathbb{R}^2\times S$）。在$X$上再定義一個向量叢$\pi:E\rightarrow X$。這裏解釋一下向量叢，它是纖維叢的一個特例，也就是纖維是向量空間的纖維叢。最常見的向量叢就是切叢。好吧，這下又得解釋纖維叢。直觀的理解纖維叢，可以把它想象成一把刷子。其中，$E$是整個向量叢空間（被稱爲總空間，整個刷子），而$X$是$E$的基礎（所以又叫基空間，刷子的柄），刷子毛就是纖維。纖維叢是微分幾何中一個比較重要的概念，雖然比較抽象，但是如果瞭解了來龍去脈還是不難理解的。假如我們只考慮歐式空間，完全不需要使用這些匪夷所思的東西，生活簡單又愜意。但是真實世界很多空間是非歐的，這就像幾千年來人們一直認爲地球是平的，突然有人發現地球是圓的，那原來的一些數學原理就得改造了，包括纖維叢、共變導數等等很多抽象的東西都是爲了改造而出現的。
　　$\rm{TX}$是$X$上的切叢，切叢就是把$X$上所有點（$\forall x\in X$）處的切空間（$\rm{T}\it{_x}\rm{X}$）合起來看成一個整體，這樣構成的空間。每個點$x$處的切空間就是切向量——也就是速度向量$(\dot{x},\dot{y},\dot{\theta})$組成的線性空間。$\pi^*\rm{TX}$是$\rm{TX}$的拉回（pullback）。這句我也看不太懂，猜測是：$\pi^*\rm{TX}$是$\rm{TX}\rightarrow E$。$\pi^*\rm{TX}$的一個截面（section）就是給$E$中的每個點分配一個$\rm{TX}$空間中的速度向量。截面是纖維叢裏的一個重要概念，這裏它就是映射：$\pi^*\rm{TX}\rightarrow E$。如果給$E$選個平凡的局部座標系，那麼這個截面就表示爲$\dot{x}=f(x,u)$，稱這樣的截面爲一個控制系統。費了半天勁纔剛給出了控制系統的正式定義。

　　論文第三部分就開始證明了，在正式證明之前，Brockett先舉了一個衆所周知的例子，就是線性系統 $\dot{x}=Ax+Bu$ 可控的充要條件。存在連續可微控制律的充分條件有三個：
　　(1)　非線性控制系統線性化後不存在不可控的模式，即不存在實部爲正的特徵值；
　　(2)　平衡點$x_0$附近存在一個鄰域N，使得對於N中每個點$\xi$總存在一個控制$u_\xi$能夠將控制系統$\dot{x}=f(x,u_\xi)$轉移到平衡點$x_0$；
　　(3)　映射$\gamma:(x,u)\rightarrow f(x,u)$能夠覆蓋一個包含原點0的開集；
　　最重要的是條件(3)，Brockett是這樣證明的：
　　如果$x_0$是控制系統$\dot{x}=a(x)$的漸進穩定平衡點，那麼一定存在一個Lyapunov函數$V$滿足這樣的性質：
　　●　$V(x)>0$（對於所有的$x\neq x_0$都成立）
　　●　$V$是連續可微函數
　　●　$V$的水平集$V^{-1}$就是一系列的同倫球。
　　爲了直觀的展示Lyapunov函數和它的水平集，先看一個簡單的例子。假設Lyapunov函數是這樣的：$V(x,y)=(x^2+y^2)/2$，就是個拋物面，它的圖像如下圖所示。$V$的水平集就是$V$等於常數的點組成的集合：$\{(x,y)|V(x,y)=c\}$，也就是用水平面截拋物面得到的一個個圓圈，就是圖中的紅圈（只畫了有限的幾個）。這個Lyapunov函數是二維的$V(x,y)$。對於三維的Lyapunov函數$V(x,y,z)=(x^2+y^2+z^2)/2$，它的水平集就是個球了，再高維度的都統一叫球。

　　存在$\alpha$和$\epsilon>0$使得在$V^{-1}(\alpha)$上，內積$\left\langle \partial V/\partial x,a(x)\right\rangle<-\epsilon$。這意味着對於足夠小的$\|\xi\|$，向量場$\dot{x}=a(x)+\xi$仍然指向同倫球$V^{-1}(\alpha)$的裏面。通過求解$\dot{x}=a(x)+\xi$可以得到一個到自身的連續映射$\{x|V(x)\leq\alpha\}$。根據萊夫謝茨不動點定理，這個映射具有一個固定點，這個固定點一定是$\dot{x}=a(x)+\xi$的平衡點。這意味着我們可以求解$a(x)=\xi$，不管$\|\xi\|$多小。如果$a(x)=f(x,u(x))$，那麼必然有$\xi=f(x,u(x))$可解。到此就完成了證明。
　　對於我們關心的汽車模型$(1)$，可以用上面的證明結論檢驗一下它爲什麼不存在光滑的反饋控制律。考慮速度$\xi=(0,c,0)$的例子，這裏$c$是個不爲零的常數。當狀態$(x,y,\theta)$中的角度$\theta=0$時就不存在解。直觀的講就是，汽車不能有相對車身側方向的速度，這是符合我們的直覺的。
　　證明用到了萊夫謝茨不動點定理。這個定理更難理解，不再展開。不過這個人倒是值得一提，數學家萊夫謝茨（Lefschetz）年輕時因爲事故失去雙手，後來鑽研數學，照片中戴手套的爲假手。

萊夫謝茨 (Lefschetz)

　　因爲Brockett的結論，人們放棄了尋找連續反饋控制律的嘗試，轉而尋找不連續或者時變的控制。

4　幾種控制律

　　這一節我們的目標是檢驗幾種前人提出的控制律的控制效果。

　　1 首先我們試試這本著名機器人書籍《Robotics, Vision and Control》第4.2.4節使用的一種簡單控制律，它與論文$[3]$中提出的差不多。它的思路是首先將模型$(1)$從直角座標系轉換到極座標系，然後設計了一個比例控制：
$\begin{aligned}\tag{4} &v=k_{\rho}\rho\\ &\gamma=k_{\alpha}\alpha+k_{\beta}\beta\\ \end{aligned}$

　　等等，前面不是證明了不存在光滑反饋控制律嗎，這個控制律在$\rho=0$處不連續，所以不違背Brockett定理。仿真的效果如下圖，左圖是沒有最小轉向半徑約束的情況，右圖是存在的情況。可見這個算法不能處理控制量受約束的模型。

　

參考文獻

[1]　Feedback Control of a Nonholonomic Car-like Robot, 第7頁.
[2]　A Mathematical Introduction to Robotic Manipulation, 第329頁.
[3]　Closed loop steering of unicycle like vehicles via Lyapunov techniques, M. Aicardi, IEEE Robotics & Automation Magazine, 1995.