学习用，翻译自
YAMAMOTO Y, YUN X, 1992. Coordinating locomotion and manipulation of a mobile manipulator[C/OL]//[1992] Proceedings of the 31st IEEE Conference on Decision and Control. 2643-2648 卷3. DOI:10.1109/CDC.1992.371337.的部分
考虑m个双边约束下具有n个广义坐标q的机械系统，其动力学方程可以通过欧拉拉格朗日方程
M
(
q
)
q
¨
+
V
(
q
,
q
˙
)
=
E
(
q
)
τ
−
A
T
(
q
)
λ
(1)
M(q)\ddot{q}+V(q,\dot{q})=E(q)\tau-A^T(q)\lambda \tag{1}
M(q)q¨+V(q,q˙)=E(q)τ−AT(q)λ(1)
确立，
其中
M
(
q
)
M(q)
M(q)是
n
×
n
n\times n
n×n的惯性矩阵，
V
(
q
,
q
˙
)
V(q,\dot{q})
V(q,q˙)是关于位置和速度的阻力向量，
E
(
q
)
E(q)
E(q)是
n
×
r
n\times r
n×r的输入转换矩阵，
τ
\tau
τ是
r
r
r维输入向量，
A
(
q
)
A(q)
A(q)是
m
×
n
m\times n
m×n的雅克比矩阵，
λ
\lambda
λ是约束力向量。
该机械系统的
m
m
m个约束方程可以写成
C
(
q
,
q
˙
)
=
0.
C(q,\dot{q})=0.
C(q,q˙)=0.
如果其中的一个约束方程可以写成，
或者通过积分可以转化为
C
i
(
q
)
=
0
C_i(q)=0
Ci(q)=0的形式，那么该约束是完整约束。
否则，该约束为动力学约束（非几何约束），一般称为非完整(nonholonomic)约束。
假设有
k
k
k个完整的以及
m
−
k
m-k
m−k个非完整的独立约束，他们都可以写成
A
(
q
)
q
˙
=
0
(3)
A(q)\dot{q}=0 \tag{3}
A(q)q˙=0(3)
的形式。
令
s
q
(
q
)
,
⋯
,
s
n
−
m
(
q
)
s_q(q),\cdots,s_{n-m}(q)
sq(q),⋯,sn−m(q)为
A
(
q
)
A(q)
A(q)的零空间中的光滑并且线性不相关的向量场，i.e.
A
(
q
)
s
i
(
q
)
=
0
,
i
=
1
,
…
,
n
−
m
A(q)s_i(q)=0, \quad i=1,\dots, n-m
A(q)si(q)=0,i=1,…,n−m
令
S
(
q
)
S(q)
S(q)为由这些向量组成的满秩矩阵
S
(
q
)
=
[
s
1
(
q
)
⋯
s
n
−
m
(
q
)
]
S(q)=[s_1(q)\ \cdots\ s_{n-m}(q)]
S(q)=[s1(q) ⋯ sn−m(q)]
令
Δ
\Delta
Δ为这些向量场张成的分布
Δ
=
s
p
a
n
{
s
1
(
q
)
,
⋯
,
s
n
−
m
(
q
)
}
\Delta=span \{s_1(q), \cdots, s_{n-m}(q)\}
Δ=span{s1(q),⋯,sn−m(q)}
因此，
q
˙
∈
Δ
\dot{q}\in\Delta
q˙∈Δ。
我们无法确定
Δ
\Delta
Δ是否对合(involutive)。
可以令
Δ
∗
\Delta^*
Δ∗为包含
Δ
\Delta
Δ的最小的对合分布。
可知
d
i
m
(
Δ
)
≤
d
i
m
(
Δ
∗
)
=
k
dim(\Delta)\leq dim(\Delta^*)=k
dim(Δ)≤dim(Δ∗)=k。
G. Campion 1991发现共由三种情况
考虑由(1)(3)定义与约束的系统，
由于受约束的速度总是在
A
(
q
)
A(q)
A(q)的零空间中，
可以定义
n
−
m
n-m
n−m个速度
v
(
t
)
=
[
v
1
v
2
⋯
v
n
−
m
]
v(t)=[v_1\ v_2\ \cdots \ v_{n-m}]
v(t)=[v1 v2 ⋯ vn−m]使得
q
˙
=
S
(
q
)
v
(
t
)
(5)
\dot{q}=S(q)v(t) \tag{5}
q˙=S(q)v(t)(5)
这些速度需要是不可积的。
对(5)微分，并代入(1)中的
q
¨
\ddot{q}
q¨，同时前乘
S
T
S^T
ST
S
T
(
M
S
v
˙
(
t
)
+
M
S
˙
v
(
t
)
+
V
)
=
S
T
E
τ
S^T (MS \dot{v}(t)+M\dot{S}v(t)+V)=S^T E \tau
ST(MSv˙(t)+MS˙v(t)+V)=STEτ
选取状态量
x
=
[
q
T
v
T
]
T
x=[q^T\ v^T]^T
x=[qT vT]T，可得
x
˙
=
[
S
v
f
2
]
+
[
0
(
S
T
M
S
)
−
1
S
T
E
]
τ
\dot{x}=[Svf2]
其中，
f
2
=
(
S
T
M
S
)
−
1
(
−
S
T
M
S
˙
v
−
S
T
V
)
f_2=(S^T M S)^{-1} (-S^T M \dot{S} v-S^T V)
f2=(STMS)−1(−STMS˙v−STV)。
假设执行器的输入数量大于或等于机械系统的自由度(
t
≥
n
−
m
t\geq n-m
t≥n−m)
并且
(
S
T
M
S
)
−
1
S
T
E
(S^T M S)^{-1} S^T E
(STMS)−1STE的秩为
n
−
m
n-m
n−m，
我们可能可以应用非线性反馈
τ
=
(
(
S
T
M
S
)
−
1
S
T
E
)
+
(
u
−
f
2
)
\tau=\left((S^T M S)^{-1} S^T E\right)^+(u-f_2)
τ=((STMS)−1STE)+(u−f2)
其中上标
+
+
+表示矩阵的广义逆。
于是状态方程可以简化为
x
˙
=
f
(
x
)
+
g
(
x
)
u
,
f
(
x
)
=
[
S
(
q
)
v
0
]
,
g
(
x
)
=
[
0
I
]
,
\dot{x}=f(x)+g(x)u,f(x) =[S(q)v0]
定理1: 非完整系统(10)是可控的
定理2：非完整系统的平衡点
x
=
0
x=0
x=0，通过一个光滑的状态反馈控制，可以使其拉格朗日稳定，
但是无法使其渐近稳定。
余下的部分，我们考虑公式(3)同时包含完整约束以及非完整约束的情况。
定理3：如果系统(10)存在至少一个约束是非完整的，那么它不是可以通过状态反馈可以输入状态可线性化的
证明：系统如果输入状态可线性化，需要满足两个条件，可以发现对合条件并不满足。
定义一组分布
D
j
=
s
p
a
n
{
L
f
i
g
∣
i
=
0
,
1
,
…
,
j
−
1
}
,
j
=
1
,
2
,
…
D_j = span \{ L^i_f g | i=0,1,\dots,j-1\}, j=1,2,\dots
Dj=span{Lfig∣i=0,1,…,j−1},j=1,2,…
对合性要求
D
1
,
D
2
,
…
,
D
2
n
−
m
D_1,D_2,\dots,D_{2n-m}
D1,D2,…,D2n−m都是对合的。
注意系统的状态数为
2
n
−
m
2n-m
2n−m。
由于
g
g
g是常数，所以
D
1
=
s
p
a
n
{
g
}
D_1=span\{g\}
D1=span{g}是对合的。
下面计算
L
f
g
=
[
f
,
g
]
=
∂
g
∂
x
f
−
∂
f
∂
x
g
=
−
[
S
(
q
)
0
]
L_fg=[f,g]= \frac{\partial g}{\partial x}f -\frac{\partial f}{\partial x}g =-[S(q)0]
由于由
S
(
q
)
S(q)
S(q)的列张成的分布
Δ
\Delta
Δ不是对合的，
分布
D
2
=
s
p
a
n
{
g
,
L
f
g
}
D_2=span\{g,L_fg\}
D2=span{g,Lfg}不是对合的。
因此，系统不是输入-状态可线性化的。
■
\blacksquare
■
尽管带有非完整约束的系统不是输入-状态可线性化的，
如果选取合适的输出方程，他是可以输入-输出线性化的。
考察系统的位置控制，也就是说输入方程只关于位置量
q
q
q。
既然系统的自由度数为
n
−
m
n-m
n−m，我们可能至多有
n
−
m
n-m
n−m个独立的位置输出方程。
y
=
h
(
q
)
=
[
h
1
(
q
)
⋯
h
n
−
m
(
q
)
]
(11)
y=h(q)=[h_1(q)\ \cdots \ h_{n-m}(q)] \tag{11}
y=h(q)=[h1(q) ⋯ hn−m(q)](11)
可以输入输出线性化的充要条件为解耦矩阵满秩 ref20。
使用(11)的输出方程，系统的解耦矩阵
Φ
(
x
)
\Phi(x)
Φ(x)是
(
n
−
m
)
×
(
n
−
m
)
(n-m)\times (n-m)
(n−m)×(n−m)的矩阵
Φ
(
q
)
=
J
h
(
q
)
S
(
q
)
\Phi(q)=J_h(q)S(q)
Φ(q)=Jh(q)S(q)
其中
J
h
=
∂
h
∂
q
J_h=\frac{\partial h}{\partial q}
Jh=∂q∂h是一个
(
n
−
m
)
×
n
(n-m)\times n
(n−m)×n的Jacobian矩阵。
当
J
h
J_h
Jh的行与
A
(
q
)
A(q)
A(q)独立的时候，\Phi(x)$ 是非奇异的
为了刻画零动态并且实现输入-输出线性化，引入新的状态变量 z z z，于是
z
=
T
(
x
)
=
[
z
1
z
2
z
3
]
=
[
h
(
q
)
L
f
h
(
q
)
h
~
(
q
)
]
=
[
h
(
q
)
Φ
(
q
)
v
h
~
(
q
)
]
z=T(x)=[z1z2z3]
其中
h
~
(
q
)
\tilde{h}(q)
h~(q)是可以使
[
J
h
T
,
J
h
~
T
]
[J_h^T,J_{\tilde{h}}^T]
[JhT,Jh~T]满秩的m维函数。
可以验证
T
(
x
)
T(x)
T(x)是微分同胚并且是一个合理的状态变换。
新的状态
z
z
z下的系统为：
z ˙ 1 = ∂ h ∂ q q ˙ = z 2 \dot{z}_1=\frac{\partial h}{\partial q} \dot{q}=z_2 z˙1=∂q∂hq˙=z2
z ˙ 2 = Φ ˙ ( q ) v + Φ ( q ) u \dot{z}_2=\dot{\Phi}(q) v + \Phi(q) u z˙2=Φ˙(q)v+Φ(q)u
z ˙ 3 = J h ~ S v = J h ~ S ( J h S ) − 1 z 2 \dot{z}_3=J_{\tilde{h}}S v = J_{\tilde{h}}S(J_h S)^{-1} z_2 z˙3=Jh~Sv=Jh~S(JhS)−1z2
使用如下的状态反馈
u = Φ − 1 ( q ) ( v − Φ ˙ ( q ) v ) u=\Phi^{-1}(q)(v-\dot{\Phi}(q)v) u=Φ−1(q)(v−Φ˙(q)v)
我们实现了输入-输出线性化，同时实现了输入-输出解耦。
系统的可观测部分为
z ˙ 1 = z 2 , z ˙ 2 = v y = z 1 \dot{z}_1=z_2,\quad \dot{z}_2=v \quad y=z_1 z˙1=z2,z˙2=vy=z1
系统的不可观测部分为：（代入 z 1 = 0 z_1=0 z1=0和 z 2 = 0 z_2=0 z2=0）
z ˙ 3 = 0 \dot{z}_3=0 z˙3=0
这个系统显然是Lagrange稳定的但是不是渐近稳定。