轮式移动机器人的运动控制

Hazel ·

更新时间:2024-09-21

· 547 次阅读

目的

　　轮式移动机器人（主要是汽车）的镇定和跟踪控制理论和方法入门。

1　前言

　　如果你常看历史剧可能注意到了，古代的车几乎都是两轮结构的。无论东方还是西方，不管是战车还是货车，古代超过两轮的车极少。究其原因当然有很多，但是最主要的是古人一直没弄明白两个车轮以上的车怎么拐弯。别小看这个问题哟，车辆转弯不是那么容易的，在结构设计上不简单，在运动控制上更是极具挑战性。人类发明车轮是个突破，怎么把多个车轮用好也是个不小难题。

　　和其它的控制系统一样，轮式移动机器人的控制任务也可以分成以下两种：
　　1　镇定：控制机器人到达并稳定在某个状态，实际的例子就是把车停到一个指定的车位里
　　2　跟踪：控制机器人跟踪某个运动着的轨迹，实际的例子就是让汽车沿着车道中心线行驶
　　★\bigstar★　这两个任务哪个更难呢？
　　我们都知道对于机械臂来说，控制它到某个状态比控制它跟踪一个空间轨迹更简单，所以很多人理所当然地认为移动机器人的“镇定”比“跟踪”更简单。但是实际情况是镇定更难，这是由于运动约束的存在[1]^{[1]}[1]。也就是说移动机器人和机械臂的控制难度刚好反过来了。哈哈！有意思吧，下面会具体分析到底为什么是这样。

2　汽车模型

　　一般的汽车都是四轮形式，为了采用艾克曼转向机构和差速器，这是最常见的一种转向形式。在论文中，一般把四轮车简化为两轮的自行车，这样的简化不改变问题的本质，而且处理更方便。这样得到的模型可以用式(1)(1)(1)所示的方程来描述，称为运动学方程。
x˙=vcos(θ)y˙=vsin(θ)θ˙=vLtan(ϕ)(1) \begin{aligned}\tag{1} &\dot{x}=v cos(\theta)\\ &\dot{y}=v sin(\theta)\\ &\dot{\theta}=\frac{v}{L}tan(\phi) \end{aligned} x˙=vcos(θ)y˙=vsin(θ)θ˙=Lvtan(ϕ)(1)

　　其中，(x,y,θ)(x,y,\theta)(x,y,θ)是车后轴中心点的位置和汽车的姿态，可以称为状态量，是我们想改变的；而(ϕ,v)(\phi,v)(ϕ,v)是控制量，是我们能直接改变的，如下图所示。ϕ\phiϕ代表自行车前轮的转角（单位是度或者弧度），vvv代表后轮的速度（单位是m/s）。控制量一般总是受约束的，例如−30°≤ϕ≤30°-30\degree\le\phi\le30\degree−30°≤ϕ≤30°，−1m/s≤v≤1m/s-1m/s\le v \le1m/s−1m/s≤v≤1m/s。简单来说，移动机器人的控制任务就是通过改变(ϕ,v)(\phi,v)(ϕ,v)来改变(x,y,θ)(x,y,\theta)(x,y,θ)。

　　可以看到这个模型并不是很复杂，我们分析一下它有什么特点：
　　1　从数学的角度看，方程中包含状态θ\thetaθ的三角函数，所以它是个非线性系统。这是个坏消息，因为非线性系统的分析和控制一般比线性系统更困难。
　　2　从驱动数量的角度看，模型中控制量的个数少于状态量的个数（2<32<32<3），因此它是个欠驱动系统（underactuated），这意味着我们要用少数控制量去影响干预多数状态量。
　　3　从运动或者约束的角度看，这个模型还包含非完整约束，什么是非完整约束呢？我们给方程组(1)(1)(1)中的第一个等式两边乘以cos(θ)cos(\theta)cos(θ)，给第二个等式两边乘以sin(θ)sin(\theta)sin(θ)，然后将两式相加就会得到式(2)(2)(2)，这个式子表示的就是一个非完整约束。区分“完整约束”和“非完整约束”的标准就是能不能积分，如果能将状态的导数项积分得到代数方程，那就是完整约束。例如x1x˙1+x2x˙2=0x_1\dot{x}_1+x_2\dot{x}_2=0x1x˙1+x2x˙2=0可以积分成12x12+12x22=c\frac{1}{2}x_1^2+\frac{1}{2}x_2^2=c21x12+21x22=c，ccc是个与初始条件有关的常数。再看式(2)(2)(2)则不能积分。对于完整约束，由于可以积分得到代数方程（速度约束变成了位置约束，阶数降低），因此可以通过变量替换消去一些状态变量（例如用x1x_1x1表示x2x_2x2从而消去x2x_2x2），让问题变得更简单。但是对于非完整约束就不行了。非完整约束系统总是欠驱动系统。飞机、汽车、船舶都是非完整约束系统，即便不考虑干扰，要想把它们控制好也不是那么容易的。
cos(θ)x˙+sin(θ)y˙=v(2) \begin{aligned}\tag{2} &cos(\theta)\dot{x}+sin(\theta)\dot{y}=v\\ \end{aligned} cos(θ)x˙+sin(θ)y˙=v(2)

　　4　它还是个无漂移系统（Driftless），这里的“漂移”跟赛车没关系。无漂移的意思是当控制量都为零时，状态量不会改变（即导数项也为零）。不给控制输入系统就静止不动相当于没有惯性，这个很容易检验。
　　5　这个模型是一组常微分方程，如果控制量只依赖状态不依赖时间，那么它还是个自治系统（Autonomous system）。自治系统是针对闭环系统的。
　　6　从输入控制量的角度看，经过简单的替换（令γ=vLtan(ϕ)\gamma=\frac{v}{L}tan(\phi)γ=Lvtan(ϕ)），它还可以变成仿射非线性系统（Control-affine systems），即模型虽然关于关于状态量仍然是非线性的，但是关于控制量(v,γ)(v,\gamma)(v,γ)是线性的。
　　7　它也是个微分平坦（differentially flat）系统，控制量可以用状态量及其导数来表示，如下式。其中，dir=sign(⟨(x˙,y˙),(cos(θ),sin(θ))⟩)dir=sign(\left\langle (\dot{x},\dot{y}),(cos(\theta),sin(\theta))\right\rangle)dir=sign(⟨(x˙,y˙),(cos(θ),sin(θ))⟩)表示运动方向。
v=dirx˙2+y˙2ϕ=arctan(Lθ˙/v)(3) \begin{aligned}\tag{3} &v=dir\sqrt{\dot{x}^2+\dot{y}^2}\\ &\phi=arctan(L\dot{\theta}/v)\\ \end{aligned} v=dirx˙2+y˙2ϕ=arctan(Lθ˙/v)(3)

3　可控性

　　了解了模型的特点，我们下面最关心的是它的可控性。可控就是能通过施加控制使系统到达状态空间中的任意一个状态，反之就是不可控。可控性自然是很重要的，因为如果一个系统是不可控的，也就意味着不管你施加怎样的控制量都到不了想要的状态，那我们就不再费劲去寻找一个控制律了。注意可控性研究的是模型本身的性质，与控制量无关，它不关心控制量是多大。
　　★\bigstar★　那么模型(1)(1)(1)是不是可控的呢？
　　我们最熟悉、也是最简单的是线性系统，翻出你大学的《自动控制原理》课本，可以找到线性系统可控性的充要条件是可控性矩阵是满秩的。但是很遗憾，我们面对的模型(1)(1)(1)是个非线性系统，得寻找更一般的方法。根据Chow的定理[1]^{[1]}[1]，模型(1)(1)(1)是可控的。

　　可控性回答的是“能不能”的问题，但是它没有回答“怎么做”。我们还是不知道怎么控制。
　　我们可以设计一个开环的控制（不依赖状态）到达给定的状态吗？可以，但是实际系统会有各种误差和扰动，即使理论计算的很好，实际一用就会出现偏差，所以最好找一个闭环的反馈控制，也就是依赖状态的控制，记成u=u(x)u=u(x)u=u(x)。如果还有什么要求的话，我们希望这个控制最好是状态的光滑函数，毕竟常见的初等函数都是光滑的嘛，方便我们构造。那么这样的要求算不算太高呢？
　　★\bigstar★　模型(1)(1)(1)是否存在光滑的反馈控制呢？
　　一个叫Brockett的哥们回答了这个问题：不存在。Brockett在论文《Asymptotic stability and feedback stabilization》中给出了系统存在光滑的反馈控制的必要条件。很遗憾，我们的机器人模型不满足这个条件。论文使用了微分几何和Lyapunov稳定理论等工具。我看这个结论很少被提到，只有贾欣乐解释过，所以下面我翻译一下论文的基本思想。

Brockett

　　Brockett的证明

　　一开始，Brockett考虑的是最一般的自治非线性系统：
x˙=f(x,u)(3) \begin{aligned}\tag{3} \dot{x}=f(x,u)\\ \end{aligned} x˙=f(x,u)(3)　　假设这个系统的平衡点是x0x_0x0。啥是平衡点？平衡点就是速度为零的点，速度为零意味着系统到达这个点就不动了，也就是到达平衡状态了，顾名思义。一般我们把想到达的目标状态设为平衡点。当然，都到目标点了，控制量也就等于零了。
f(x0,0)=0 \begin{aligned} f(x_0,0)=0\\ \end{aligned} f(x0,0)=0　　然后作者描述了这篇论文要解决的唯一的一个问题：能不能找到一个光滑的函数u(x)u(x)u(x)使系统(3)(3)(3)在平衡点x0x_0x0处渐进稳定。注意u(x)u(x)u(x)的写法表示控制量uuu只依赖于状态xxx，不依赖其它的东西，比如不依赖时间。所以即使你找不到一个光滑u(x)u(x)u(x)，那不意味着你找不到一个光滑的u(x,t)u(x,t)u(x,t)，很多后来的学者就是这么干的，把时间引进来，或者牺牲光滑性，找一个不连续的解。
　　论文第二部分，Brockett开始给出一些基本定义给后文做铺垫，但是是用微分几何的语言描述的。对于没学过微分几何的同学，看到这里估计腿一软就给跪下了，笔者也是不太懂。不过即使这部分看不懂好像也不太妨碍理解后面的证明部分。当然能掌握微分几何是最好的，因为好多论文都使用了微分几何的工具和术语，如果不懂看起来论文来会很吃力。懂微分几何的另一个好处是让你的论文更上档次，毕竟很多搞机器人研究的数学都停留在初等微积分和线性代数的水平。
　　有一个可微的流形XXX（对于咱们来说，XXX就是移动机器人的所有位姿(x,y,θ)(x,y,\theta)(x,y,θ)组成的构型空间，所以X=R2×SX=\mathbb{R}^2\times SX=R2×S）。在XXX上再定义一个向量丛π:E→X\pi:E\rightarrow Xπ:E→X。这里解释一下向量丛，它是纤维丛的一个特例，也就是纤维是向量空间的纤维丛。最常见的向量丛就是切丛。好吧，这下又得解释纤维丛。直观的理解纤维丛，可以把它想象成一把刷子。其中，EEE是整个向量丛空间（被称为总空间，整个刷子），而XXX是EEE的基础（所以又叫基空间，刷子的柄），刷子毛就是纤维。纤维丛是微分几何中一个比较重要的概念，虽然比较抽象，但是如果了解了来龙去脉还是不难理解的。假如我们只考虑欧式空间，完全不需要使用这些匪夷所思的东西，生活简单又惬意。但是真实世界很多空间是非欧的，这就像几千年来人们一直认为地球是平的，突然有人发现地球是圆的，那原来的一些数学原理就得改造了，包括纤维丛、共变导数等等很多抽象的东西都是为了改造而出现的。
　　TX\rm{TX}TX是XXX上的切丛，切丛就是把XXX上所有点（∀x∈X\forall x\in X∀x∈X）处的切空间（TxX\rm{T}\it{_x}\rm{X}TxX）合起来看成一个整体，这样构成的空间。每个点xxx处的切空间就是切向量——也就是速度向量(x˙,y˙,θ˙)(\dot{x},\dot{y},\dot{\theta})(x˙,y˙,θ˙)组成的线性空间。π∗TX\pi^*\rm{TX}π∗TX是TX\rm{TX}TX的拉回（pullback）。这句我也看不太懂，猜测是：π∗TX\pi^*\rm{TX}π∗TX是TX→E\rm{TX}\rightarrow ETX→E。π∗TX\pi^*\rm{TX}π∗TX的一个截面（section）就是给EEE中的每个点分配一个TX\rm{TX}TX空间中的速度向量。截面是纤维丛里的一个重要概念，这里它就是映射：π∗TX→E\pi^*\rm{TX}\rightarrow Eπ∗TX→E。如果给EEE选个平凡的局部坐标系，那么这个截面就表示为x˙=f(x,u)\dot{x}=f(x,u)x˙=f(x,u)，称这样的截面为一个控制系统。费了半天劲才刚给出了控制系统的正式定义。

　　论文第三部分就开始证明了，在正式证明之前，Brockett先举了一个众所周知的例子，就是线性系统 x˙=Ax+Bu\dot{x}=Ax+Bux˙=Ax+Bu 可控的充要条件。存在连续可微控制律的充分条件有三个：
　　(1)　非线性控制系统线性化后不存在不可控的模式，即不存在实部为正的特征值；
　　(2)　平衡点x0x_0x0附近存在一个邻域N，使得对于N中每个点ξ\xiξ总存在一个控制uξu_\xiuξ能够将控制系统x˙=f(x,uξ)\dot{x}=f(x,u_\xi)x˙=f(x,uξ)转移到平衡点x0x_0x0；
　　(3)　映射γ:(x,u)→f(x,u)\gamma:(x,u)\rightarrow f(x,u)γ:(x,u)→f(x,u)能够覆盖一个包含原点0的开集；
　　最重要的是条件(3)，Brockett是这样证明的：
　　如果x0x_0x0是控制系统x˙=a(x)\dot{x}=a(x)x˙=a(x)的渐进稳定平衡点，那么一定存在一个Lyapunov函数VVV满足这样的性质：
　　●　V(x)>0V(x)>0V(x)>0（对于所有的x≠x0x\neq x_0x=x0都成立）
　　●　VVV是连续可微函数
　　●　VVV的水平集V−1V^{-1}V−1就是一系列的同伦球。
　　为了直观的展示Lyapunov函数和它的水平集，先看一个简单的例子。假设Lyapunov函数是这样的：V(x,y)=(x2+y2)/2V(x,y)=(x^2+y^2)/2V(x,y)=(x2+y2)/2，就是个抛物面，它的图像如下图所示。VVV的水平集就是VVV等于常数的点组成的集合：{(x,y)∣V(x,y)=c}\{(x,y)|V(x,y)=c\}{(x,y)∣V(x,y)=c}，也就是用水平面截抛物面得到的一个个圆圈，就是图中的红圈（只画了有限的几个）。这个Lyapunov函数是二维的V(x,y)V(x,y)V(x,y)。对于三维的Lyapunov函数V(x,y,z)=(x2+y2+z2)/2V(x,y,z)=(x^2+y^2+z^2)/2V(x,y,z)=(x2+y2+z2)/2，它的水平集就是个球了，再高维度的都统一叫球。

　　存在α\alphaα和ϵ>0\epsilon>0ϵ>0使得在V−1(α)V^{-1}(\alpha)V−1(α)上，内积⟨∂V/∂x,a(x)⟩<−ϵ\left\langle \partial V/\partial x,a(x)\right\rangle<-\epsilon⟨∂V/∂x,a(x)⟩<−ϵ。这意味着对于足够小的∥ξ∥\|\xi\|∥ξ∥，向量场x˙=a(x)+ξ\dot{x}=a(x)+\xix˙=a(x)+ξ仍然指向同伦球V−1(α)V^{-1}(\alpha)V−1(α)的里面。通过求解x˙=a(x)+ξ\dot{x}=a(x)+\xix˙=a(x)+ξ可以得到一个到自身的连续映射{x∣V(x)≤α}\{x|V(x)\leq\alpha\}{x∣V(x)≤α}。根据莱夫谢茨不动点定理，这个映射具有一个固定点，这个固定点一定是x˙=a(x)+ξ\dot{x}=a(x)+\xix˙=a(x)+ξ的平衡点。这意味着我们可以求解a(x)=ξa(x)=\xia(x)=ξ，不管∥ξ∥\|\xi\|∥ξ∥多小。如果a(x)=f(x,u(x))a(x)=f(x,u(x))a(x)=f(x,u(x))，那么必然有ξ=f(x,u(x))\xi=f(x,u(x))ξ=f(x,u(x))可解。到此就完成了证明。
　　对于我们关心的汽车模型(1)(1)(1)，可以用上面的证明结论检验一下它为什么不存在光滑的反馈控制律。考虑速度ξ=(0,c,0)\xi=(0,c,0)ξ=(0,c,0)的例子，这里ccc是个不为零的常数。当状态(x,y,θ)(x,y,\theta)(x,y,θ)中的角度θ=0\theta=0θ=0时就不存在解。直观的讲就是，汽车不能有相对车身侧方向的速度，这是符合我们的直觉的。
　　证明用到了莱夫谢茨不动点定理。这个定理更难理解，不再展开。不过这个人倒是值得一提，数学家莱夫谢茨（Lefschetz）年轻时因为事故失去双手，后来身残志坚钻研数学，照片中戴手套的为假手。

莱夫谢茨 (Lefschetz)

　　因为Brockett的结论，人们放弃了寻找连续反馈控制律的尝试，转而寻找不连续或者时变的控制。

4　几种控制律

　　这一节我们的目的是检验几种前人提出的控制律的控制效果。
　　1　首先我们试试这本著名机器人书籍《Robotics, Vision and Control》第4.2.4节使用的一种简单控制律，它与论文[3][3][3]中提出的差不多。它的思路是首先将模型(1)(1)(1)从直角坐标系转换到极坐标系，然后设计了一个比例控制：
v=kρργ=kαα+kββ(4) \begin{aligned}\tag{4} &v=k_{\rho}\rho\\ &\gamma=k_{\alpha}\alpha+k_{\beta}\beta\\ \end{aligned} v=kρργ=kαα+kββ(4)

　　等等，前面不是证明了不存在光滑反馈控制律吗，这个控制律在ρ=0\rho=0ρ=0处不连续，所以不违背Brockett定理。仿真的效果如下图，我们让目标状态位于原点，姿态是90°，初始状态处于一个圆周上，初始角度都是0°。左图是没有最小转向半径约束的情况，右图是存在的情况。可见这个算法不能处理控制量受约束的模型。这不奇怪，因为原文只是针对差速机器人模型提出来的。

　　2　有学者提出了不连续的反馈控制，。

　　3　有学者另辟蹊径，把时间引入了控制量，提出了时变的反馈控制，例如论文[4][4][4]。

　　4　还有学者考虑了几种特殊情况下的反馈控制。

参考文献

[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.
[4]　Time-varying Feedback Stabilization of Car-like Wheeled Mobile Robots, Claude Samson,
The International Journal of Robotics Research.