- 1【计算机网络实验】实验三 IP网络规划与路由设计（头歌）_头歌 静态路由配置 ping 命令测试
- 2Mysql 数据库APi 编程（c/c++）-1.0_mysql数据表 api
- 3RuntimeError: Failed to allocate graph: MYRIAD device is not opened
- 4vue跳转，v-model 双向绑定，-vuex的使用cookie：，视频第三方播放
- 5微软官宣：Windows11将成为第一个全局AI辅助的操作系统_win11有自带的ai工具吗
- 6git知识点以及git常见错误_hint: 'git pull ...') before pushing again. hint:
- 7微信小程序页面传递参数方法_微信小程序navigateto传参
- 8键盘上各按键对应的英文名(转载)_numplus是哪个键
- 9python输出自己的名字_使用Python实现自我介绍
- 10【PHP】PHP代码审计基础知识
Delta机器人鲁棒控制仿真_delta机器人怎么求关节的扭矩
赞
踩
由于在构建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 =
ξ
˙
=
[
O
E
−
K
P
−
K
D
]
ξ
+
[
O
E
]
(
η
−
ω
)
(1)
\dot{\xi} =
对于一个正定二次型 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(
上式中，前一项的非负性是易于保证的，只要选取合适的
K
D
K_{D}
KD和
K
P
K_{P}
KP即可，事实上前一项也正是系统的线性部分。后一项的非负性依靠选取合适的
ω
\omega
ω来保证，将向量
[
O
E
]
Qξ
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
^
∣
∣
另外，由于我们机器人系统的路径是确定的，估计动力学模型时的误差也是在一定范围内的，即期望轨迹 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ξ
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，如有需要请点击此处下载。没有写注释，还请多多包涵，如有问题欢迎讨论，谢谢！
