由于在构建Delta机构动力学模型的时候采取了一些假设（主动臂从动臂都认为是均质杆1）本次仿真采用关节空间鲁棒计算力矩控制的方法控制机器人。
1.1 计算力矩控制
常规的计算力矩控制策略如图
1所示，使用估计的机器人动力学模型控制实际的机器人，在理想的情况下，即估计所用的模型与实际模型完全一致：
M
^
=
M
,
n
^
=
n
\widehat{M} = M,\ \widehat{n} = n
M
=M, n
=n.
图 1计算力矩控制框图
此时被控系统就被线性化，直接写出其常微分方程：
q ¨ = q ¨ d + K D ( q ˙ d − q ˙ ) + K P ( q d − q ) \ddot{q} = \ {\ddot{q}}_{d} + \ K_{D}\left( {\dot{q}}_{d} - \dot{q} \right) + \ K_{P}\left( q_{d} - q \right) q¨= q¨d+ KD(q˙d−q˙)+ KP(qd−q)
选取合适的 K D K_{D} KD和 K P K_{P} KP就能保证系统的稳定。
根据先前所计算的逆动力学模块，我们首先按照图
1所示的结构构建了计算力矩系统，并对其进行跟踪五次多项式信号的控制测试，得到的结果如图2所示。可以看到控制所用的数学模型与实际模型（Adams模型）有一定差别，导致系统具有较大的稳态误差。
图 2系统跟踪五次多项式轨迹的响应-常规策略
1.2 鲁棒计算力矩
对于这种控制策略所使用的数学模型可能与实际模型不一致的问题，在机器人学专著《Robotics: Modelling, Planning and Control》 (Siciliano, 2010)2 中，有该控制问题详细讨论，其原理就是引入一个鲁棒项来（图4中的 ω \omega ω）辅助控制作为补偿，尽可能减小或抵消因估计不准确带来的影响（记作 η \eta η）。
M
q
¨
+
n
=
M
^
y
+
n
^
{\text{\ M\ }\ddot{q}\ + \ n = \ \ \widehat{M}y\ + \ \widehat{n} }
M q¨ + n= M
y + n
q
¨
=
y
−
(
(
E
−
M
−
1
M
^
)
y
+
M
−
1
(
n
−
n
^
)
)
{\ddot{q}\ = \ y\ - \left( \left( E\ - \ M^{- 1}\widehat{M}\ \right)y\ + \ M^{- 1}\left( n - \widehat{n} \right) \right)}
q¨ = y −((E − M−1M
)y + M−1(n−n
))
图 4鲁棒计算力矩控制框图
由李亚普诺第二方法可以确定使系统具有鲁棒稳定性的 ω \omega ω，首先构造状态变量 ξ \xi ξ，并将控制系统写成状态方程的形式。
ξ = [ q d − q q ˙ d − q ˙ ] T \xi = \begin{bmatrix} q_{d} - q \\ {\dot{q}}_{d} - \dot{q} \end{bmatrix}^{T} ξ=[qd−qq˙d−q˙]T
ξ ˙ = [ O E − K P − K D ] ξ + [ O E ] ( η − ω ) (1) \dot{\xi} = \begin{bmatrix} O & E \\ - K_{P} & - K_{D} \end{bmatrix}\xi + \ \begin{bmatrix} O \\ E \end{bmatrix}\ \left( \eta\ - \omega \right) \tag{1} ξ˙=[O−KPE−KD]ξ+ [OE] (η −ω)(1)
对于一个正定二次型 V ( ξ ) = ξ T Q ξ > 0 , ∀ ξ ≠ 0 \text{\ V}\left( \xi \right) = \xi^{T}Q\xi > 0,\ \forall\ \xi \neq \ \mathbf{0} V(ξ)=ξTQξ>0, ∀ ξ= 0，要保证系统渐进稳定，就要保证该二次型对时间的导数小于零，由上述状态方程形式控制系统，可求得该二次型的导数为：
V ˙ = ξ T ( [ O E − K P − K D ] T Q + Q [ O E − K P − K D ] ) ξ + 2 ξ T Q [ O E ] ( η − ω ) \dot{V} = \xi^{T}\left( \begin{bmatrix} O & E \\ - K_{P} & - K_{D} \\ \end{bmatrix}^{T}Q + Q\begin{bmatrix} O & E \\ - K_{P} & - K_{D} \\ \end{bmatrix} \right)\xi + 2\xi^{T}Q\begin{bmatrix} O \\ E \\ \end{bmatrix}\ \left( \eta\ - \omega \right) V˙=ξT([O−KPE−KD]TQ+Q[O−KPE−KD])ξ+2ξTQ[OE] (η −ω)
上式中，前一项的非负性是易于保证的，只要选取合适的 K D K_{D} KD和 K P K_{P} KP即可，事实上前一项也正是系统的线性部分。后一项的非负性依靠选取合适的 ω \omega ω来保证，将向量 [ O E ] Qξ \begin{bmatrix} O & E \\ \end{bmatrix}\text{Qξ} [OE]Qξ记作 z z z，可以求解矩阵不等式来求解该鲁棒控制问题。
z T ( η − ω ) = z T η − ∣ ∣ z ∣ ∣ ρ ≤ ∣ ∣ z ∣ ∣ ( ∣ ∣ η ∣ ∣ − ρ ) \ z^{T}\left( \eta\ - \omega \right) = z^{T}\eta - \left| \left| z \right| \right|\rho \leq \left| \left| z \right| \right|\left( \left| \left| \eta \right| \right|\ - \rho \right) zT(η −ω)=zTη−∣∣z∣∣ρ≤∣∣z∣∣(∣∣η∣∣ −ρ)
依据 z z z是否是非零向量分情况讨论，若z是零向量，则系统又回到线性情况，稳定性容易保证，若 z z z不是零向量，不妨选取 ω \omega ω与 z z z同向，将其幅值记作 ρ \rho ρ，由柯西不等式的向量形式可以得到上式，显然只要保证 ρ \rho ρ大于 η \eta η的幅值，就能保证系统的稳定。
而对于 η \eta η的幅值，有：
∣ ∣ η ∣ ∣ = ∣ ∣ ( E − M − 1 M ^ ) y + M − 1 ( n − n ^ ) ∣ ∣ ≤ ∣ ∣ E − M − 1 M ^ ∣ ∣ ( ∣ ∣ q ¨ d ∣ ∣ + ∣ ∣ K D ( q ˙ d − q ˙ ) ∣ ∣ + ∣ ∣ K P ( q d − q ) ∣ ∣ + ∣ ∣ ω ∣ ∣ ) + ∣ ∣ M − 1 ∣ ∣ ∣ ∣ n − n ^ ∣ ∣ \begin{aligned} \left| \left| \eta \right| \right| &= \left| \left| \left( E\ - \ M^{- 1}\widehat{M} \right)y\ + \ M^{- 1}\left( n - \widehat{n} \right) \right| \right| \\ &\leq \ \left| \left| E\ - \ M^{- 1}\widehat{M} \right| \right|\ \left( \left| \left| {\ddot{q}}_{d} \right| \right| + \left| \left| K_{D}\left( {\dot{q}}_{d} - \ \dot{q} \right) \right| \right|\ + \ \left| \left| K_{P}\ \left( q_{d}\ - q \right) \right| \right| + \left| \left| \omega \right| \right| \right)\ + \ \left| \left| M^{- 1} \right| \right|\ \left| \left| n - \widehat{n} \right| \right|\end{aligned} ∣∣η∣∣=∣∣∣∣∣∣(E − M−1M )y + M−1(n−n )∣∣∣∣∣∣≤ ∣∣∣∣∣∣E − M−1M ∣∣∣∣∣∣ (∣∣q¨d∣∣+∣∣KD(q˙d− q˙)∣∣ + ∣∣KP (qd −q)∣∣+∣∣ω∣∣) + ∣∣∣∣M−1∣∣∣∣ ∣∣n−n ∣∣
另外，由于我们机器人系统的路径是确定的，估计动力学模型时的误差也是在一定范围内的，即期望轨迹 Q d Q_{d} Qd和 M ^ , n ^ \widehat{M},\ \widehat{n} M , n 满足下式：
sup t ≥ 0 ∣ ∣ q ¨ d ∣ ∣ < Q M < ∞ \sup_{t \geq \ 0}\text{\ \ }\left| \left| {\ddot{q}}_{d} \right| \right| < Q_{M} < \infty\ t≥ 0sup ∣∣q¨d∣∣<QM<∞
∣ ∣ E − M − 1 M ^ ∣ ∣ < α < 1 , ∀ q \left| \left| E - \ M^{- 1}\widehat{M} \right| \right| < \alpha < 1,\ \forall q ∣∣∣∣∣∣E− M−1M ∣∣∣∣∣∣<α<1, ∀q
∣ ∣ n − n ^ ∣ ∣ < Φ < ∞ , ∀ q , q ˙ \text{\ \ }\left| \left| n - \widehat{n} \right| \right| < \Phi < \infty,\ \forall q,\ \dot{q} ∣∣n−n ∣∣<Φ<∞, ∀q, q˙
所以，只要 ρ \rho ρ满足下式，就能保证系统渐进稳定。
ρ ≥ 1 1 − α ( α Q M + α ( ∣ ∣ K D ( q ˙ d − q ˙ ) ∣ ∣ + ∣ ∣ K P ( q d − q ) ∣ ∣ ) + M M − 1 Φ ) \rho \geq \frac{1}{1 - \alpha}\ \left( \text{α\ }Q_{M} + \alpha\left( \ \left| \left| K_{D}\ \left( {\dot{q}}_{d} - \ \dot{q} \right) \right| \right| + \left| \left| K_{P}\left( q_{d}\ - \ q \right) \right| \right| \right)\ + \ M_{M}^{- 1}\Phi \right) ρ≥1−α1 (α QM+α( ∣∣KD (q˙d− q˙)∣∣+∣∣KP(qd − q)∣∣) + MM−1Φ)
在该项目中选取
M
M
−
1
=
1.00
×
1
0
−
3
,
α
=
7.25
×
1
0
−
1
,
Q
M
=
1.00
M_{M}^{- 1} = 1.00 \times 10^{- 3},\ \ \alpha = \ 7.25 \times 10^{- 1},\ Q_{M} = 1.00
MM−1=1.00×10−3, α= 7.25×10−1, QM=1.00，设置鲁棒项，跟踪与之前相同的五次多项式信号，得到的响应如图5所示。
图 5系统响应-鲁棒计算力矩
可以看到相比之前的系统，稳态误差下降了超过 80 % 80\% 80%，尝试对 M M − 1 , α , Q M M_{M}^{- 1},\alpha,Q_{M} MM−1,α,QM进行调整，发现改动 α \alpha α带来的稳态误差的变化最大，在 0 < α < 7.25 × 1 0 − 1 0 < \alpha < 7.25 \times 10^{- 1} 0<α<7.25×10−1时，增大 α \alpha α的值可以减小稳态误差，在 7.25 × 1 0 − 1 < α < 1 7.25 \times 10^{- 1} < \alpha < 1 7.25×10−1<α<1时，增大 α \alpha α会带来稳态时的等幅振荡（临界稳定）。
注意这里并没有消除稳态误差，原因是选取 ω \omega ω与 [ O E ] Qξ \begin{bmatrix} O & E \\ \end{bmatrix}\text{Qξ} [OE]Qξ同向会使得 ξ = 0 \xi=\bm{0} ξ=0不是式1的平衡点，从而导致系统仅仅满足李雅普诺夫稳定而不是渐进稳定（asymptotically stable）。
1.3 其他复合控制策略
为了消除稳态误差，引入积分项
K
I
K_{I}
KI，并配置假设线性系统的极点为
−
8
±
6
i
,
−
40
- 8 \pm 6i, - 40
−8±6i,−40，即
K
D
=
50
,
K
P
=
740
,
K
I
=
4000
K_{D} = 50,\ K_{P} = 740,\ K_{I} = 4000
KD=50, KP=740, KI=4000，新的控制系统如图
6所示。还是跟踪同样的五次多项式信号，得到的误差如图7所示。
由于极点配置的距离虚轴比较远，可以看到系统在很短的时间内就完成了调整，误差也十分小，对五次多项式轨迹的跟随效果非常好。
图 6（鲁棒）计算力矩 和PID前馈控制系统
图 7 跟踪五次多项式轨迹的误差-引入积分项
如果将图6中的鲁棒项取消注释，使用鲁棒计算力矩和PID前馈复合控制，跟踪同样的五次多项式轨迹，得到的系统响应和误差如图8、图9所示。可以看到系统在瞬态时的误差有所下降，但在稳态时出现等幅振荡。
图 8系统响应-鲁棒计算力矩和PID前馈复合控制
图 9误差-鲁棒计算力矩和PID前馈复合控制
2 机器人轨迹规划
对机器人进行轨迹规划也有很多种方法，本项目选取了在关节空间的轨迹规划，并使用三角函数插值机器人的轨迹（具有周期性）进行规划。在给定一个周期内的7组
(
t
,
θ
)
\left( t,\theta \right)
(t,θ)和初始时刻
(
θ
(
0
)
,
θ
˙
(
0
)
)
\left( \theta\left( 0 \right),\dot{\theta}\left( 0 \right) \right)
(θ(0),θ˙(0))时，可以规划四阶三角函数。采取周期
T
=
3
s
，
T = 3s，
T=3s，空间跨度
0.15
×
0.15
×
0.30
m
3
0.15 \times 0.15 \times 0.30\ m^{3}
0.15×0.15×0.30 m3的控制点，规划得到的周期轨迹如图
10所示，图中的x代表控制点。
θ ( t ) = a 0 + ∑ n = 1 4 a n cos 2 π T nt + ∑ n = 1 4 b n sin 2 π T nt \theta\left( t \right) = a_{0} + \ \sum_{n = 1}^{4}{\ a_{n}\cos{\frac{2\pi}{T}\text{nt}}} + \sum_{n = 1}^{4}{\ b_{n}\sin{\frac{2\pi}{T}\text{nt}}} θ(t)=a0+ n=1∑4 ancosT2πnt+n=1∑4 bnsinT2πnt
图 10周期轨迹-关节空间
在联合仿真中，用上述计算力矩和PID前馈复合控制系统跟踪该轨迹，有误差曲线如图11所示，可以看到即便是跟踪周期信号，该控制系统的误差也可稳定在
0.01
rad
0.01\text{rad}
0.01rad（
0.06
∘
{0.06}^{{^\circ}}
0.06∘）以内，控制效果非常好。
图 11跟踪周期轨迹的误差
下面是Adams中的仿真动画。
代码以及模型数据
代码和模型的数据已经上传到CSDN，如有需要请点击此处下载。没有写注释，还请多多包涵，如有问题欢迎讨论，谢谢！
3718
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
