---

一、独轮移动机器人

独轮图

1. Configuration

$$q=\left(\begin{array}{c}x\\ y\\ \theta \end{array}\right)$$

x, y 表示轮子与地面接触点坐标， $\theta$ 是轮子方向转角。

2. 约束 Constraints

有几个轮子就有几个 pure rolling (纯滚动) 的约束

$$\sin \theta \dot{x}-\cos \theta \dot{y}=0$$

写成矩阵（Pfaffian Form）形式 $A^T(q)\dot{q}=0$，即：

$$\left(\begin{array}{ccc}\sin \theta & \cos \theta & 0\end{array}\right)\left(\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right)=0$$

3. 运动学模型

独轮只有一个轮子，所以只有一个 pure rolling，即只有一个运动学约束，k = 1，即 矩阵 $A^T$ 有 k = 1 行，其 configuration q 有几个量，则 矩阵 $A^T$ 有 n = 3 列。

然后
$$\dot{q}\in N(A^T(q))$$

其中，零和空间 N 的维度为 n - k = 3 - 1 = 2，即，$\dot{q}$ 应有两个解 $g_1,g_2$ (也即输入)，使得 Pfaffian Form 成立，计算后得：

$$\dot{q}=g_1(q)u_1+g_2(q)u_2=\left(\begin{array}{c}\cos \theta \\ \sin \theta \\ 0\end{array}\right)u_1+\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)u_2$$

即
$$\dot{x}=\cos \theta u_1,\dot{y}=\sin \theta u_1,\dot{\theta }=u_2$$

其中，$$u_1=\omega r,u_2=\dot{\Theta }$$

4. Lie Bracket 与判断是否可控 Controllable

Lie Bracket的定义

$g_i(x)$ 是一个 n 维向量域，当给定 $g_1(x),g_2(x)$时，然后

$$g_3=[g_1(x),g_2(x)]=\frac{\partial g_2}{\partial x}{g}_1-\frac{\partial g_1}{\partial x}{g}_2$$

$g_3$ 就是一个新的向量域，称为 $g_1(x),g_2(x)$ 的 Lie Bracket.

判断是否可控 Controllable

对于所有得输入向量域 (Input vector filed)， 把他们收集起来写成

Δ={g1(x),g2(x),...,gm(q),...,[gi,gj],...,[gi,[gi,gk]],...,andsoon...}\Delta = \{ g_{1}(x), g_{2}(x), ..., g_{m}(q), ..., [g_{i}, g_{j}], ..., [g_{i}, [g_{i}, g_{k}]], ..., and \space so \space on... \} Δ={g1​(x),g2​(x),...,gm​(q),...,[gi​,gj​],...,[gi​,[gi​,gk​]],...,and so on...}

如果 维度 dim $\Delta$ = n，则运动学模型是可控的。或者可以看 rank $\Delta$ = n（我不知道为什么，但这是很常用的方法）

对于 独轮来说，

$$g_3=[g_1,g_2]=\left(\begin{array}{c}\sin \theta \\ -\cos \theta \\ 0\end{array}\right)$$

$$rank(g_1,g_2)=\left(\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ \sin \theta & 0& -\cos \theta \\ 0& 1& 0\end{array}\right)=n=3$$

二、两轮自行车 Bicycle

1. Configuration

自行车俯瞰图

$$q=\left(\begin{array}{c}x\\ y\\ \theta \\ \varphi \end{array}\right)$$

n = 4

2. 约束 Constraints

有几个轮子就有几个 pure rolling (纯滚动) 的约束，两个轮子就有俩约束，k = 2

对于后轮 Pure Rolling Constraint：

$$\sin \theta \dot{x}-\cos \theta \dot{y}=0$$

对前轮：

$$\sin (\theta +\varphi )\dot{x_f}-\cos (\theta +\varphi )\dot{y_f}=0$$

其中$x_f,y_f$ 要替换成 x，y， 即：

$$x_f=x+l\cos \theta$$

$$y_f=y+l\sin \theta$$

对时间求导，得：

$$\dot{x_f}=\dot{x}-l\sin \theta \dot{\theta }$$

$$\dot{y_f}=\dot{y}+l\cos \theta \dot{\theta }$$

最后全替换了，前轮约束公式就变成了下式：

$$\sin (\theta +\varphi )\dot{x}-\cos (\theta +\varphi )\dot{y}-l\sin \varphi \dot{\theta }=0$$

3. 运动学模型

$$A^T(q)\dot{q}=0\Rightarrow \left(\begin{array}{cccc}\sin \theta & -\cos \theta & 0& 0\\ \sin (\theta +\varphi )& -\cos (\theta +\varphi )& -l\cos \varphi & 0\end{array}\right)\left(\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\\ \dot{\varphi }\end{array}\right)=0$$

有 $n-k=2$，计算得：
$$\dot{q}=\left(\begin{array}{c}\cos \theta \\ \sin \theta \\ \frac{\tan \theta }{l}\\ 0\end{array}\right)u_1+\left(\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right)u_2$$

其中，$u_1=\pm \sqrt{\dot{x^2}+\dot{y^2}},u_2=\dot{\varphi }$.

4. 判断是否可控

已知 $g_1,g_2$，计算：

$$g_3=[g_1,g_2]=\left(\begin{array}{c}0\\ 0\\ -\frac{1}{lco{s}^2\varphi }\\ 0\end{array}\right)$$

$$g_4=[g_1,g_3]=\left(\begin{array}{c}-\frac{\sin \theta }{l^2\cos \varphi }\\ \frac{\cos \theta }{l^2\cos \varphi }\\ 0\\ 0\end{array}\right)$$

则：

$$rank\left(\begin{array}{cccc}|& |& |& |\\ g_1& g_2& g_3& g_4\\ |& |& |& |\end{array}\right)=4$$

则自行车约束可控！

一键三连！！！！