当前位置:   article > 正文

Delta机器人鲁棒控制仿真_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˙dq˙)+ KP(qdq)

选取合适的 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  M1M  )y + M1(nn ))
图 4鲁棒计算力矩控制框图

由李亚普诺第二方法可以确定使系统具有鲁棒稳定性的 ω \omega ω,首先构造状态变量 ξ \xi ξ,并将控制系统写成状态方程的形式。

ξ = [ q d − q q ˙ d − q ˙ ] T \xi =

[qdqq˙dq˙]
^{T} ξ=[qdqq˙dq˙]T

ξ ˙ = [ O E − K P − K D ] ξ +   [ O E ]   ( η   − ω ) (1) \dot{\xi} =

[OEKPKD]
\xi + \
[OE]
\ \left( \eta\ - \omega \right) \tag{1} ξ˙=[OKPEKD]ξ+ [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(

[OEKPKD]
^{T}Q + Q
[OEKPKD]
\right)\xi + 2\xi^{T}Q
[OE]
\ \left( \eta\ - \omega \right) V˙=ξT([OKPEKD]TQ+Q[OKPEKD])ξ+2ξTQ[OE] (η ω)

上式中,前一项的非负性是易于保证的,只要选取合适的 K D K_{D} KD K P K_{P} KP即可,事实上前一项也正是系统的线性部分。后一项的非负性依靠选取合适的 ω \omega ω来保证,将向量 [ O E ] Qξ

[OE]
\text{Qξ} [OE]记作 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 ^ ∣ ∣

||η||=||(E  M1M^)y + M1(nn^)|| ||E  M1M^|| (||q¨d||+||KD(q˙d q˙)|| + ||KP (qd q)||+||ω||) + ||M1|| ||nn^||
η=(E  M1M )y + M1(nn ) E  M1M  (q¨d+KD(q˙d q˙) + KP (qd q)+ω) + M1 nn

另外,由于我们机器人系统的路径是确定的,估计动力学模型时的误差也是在一定范围内的,即期望轨迹 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 M1M <α<1, q

   ∣ ∣ n − n ^ ∣ ∣ < Φ < ∞ ,   ∀ q ,   q ˙ \text{\ \ }\left| \left| n - \widehat{n} \right| \right| < \Phi < \infty,\ \forall q,\ \dot{q}   nn <Φ<, 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)) + MM1Φ)

在该项目中选取 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 MM1=1.00×103,  α= 7.25×101, QM=1.00,设置鲁棒项,跟踪与之前相同的五次多项式信号,得到的响应如图5所示。
在这里插入图片描述
图 5系统响应-鲁棒计算力矩

可以看到相比之前的系统,稳态误差下降了超过 80 % 80\% 80%,尝试对 M M − 1 , α , Q M M_{M}^{- 1},\alpha,Q_{M} MM1,α,QM进行调整,发现改动 α \alpha α带来的稳态误差的变化最大,在 0 < α < 7.25 × 1 0 − 1 0 < \alpha < 7.25 \times 10^{- 1} 0<α<7.25×101时,增大 α \alpha α的值可以减小稳态误差,在 7.25 × 1 0 − 1 < α < 1 7.25 \times 10^{- 1} < \alpha < 1 7.25×101<α<1时,增大 α \alpha α会带来稳态时的等幅振荡(临界稳定)。

注意这里并没有消除稳态误差,原因是选取 ω \omega ω [ O E ] Qξ

[OE]
\text{Qξ} [OE]同向会使得 ξ = 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=14 ancosT2πnt+n=14 bnsinT2πnt

在这里插入图片描述
图 10周期轨迹-关节空间

在联合仿真中,用上述计算力矩和PID前馈复合控制系统跟踪该轨迹,有误差曲线如图11所示,可以看到即便是跟踪周期信号,该控制系统的误差也可稳定在 0.01 rad 0.01\text{rad} 0.01rad 0.06 ∘ {0.06}^{{^\circ}} 0.06)以内,控制效果非常好。
在这里插入图片描述图 11跟踪周期轨迹的误差

下面是Adams中的仿真动画。
在这里插入图片描述

代码以及模型数据

代码和模型的数据已经上传到CSDN,如有需要请点击此处下载。没有写注释,还请多多包涵,如有问题欢迎讨论,谢谢!

参考文献


  1. 毛洪国. 基于动力学模型的DELTA机器人运动控制研究[D]. 哈尔滨工业大学. ↩︎

  2. Siciliano B , Sciavicco L , Villani L , et al. Robotics[M]. Springer London, 2009. ↩︎

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/菜鸟追梦旅行/article/detail/619273
推荐阅读
相关标签
  

闽ICP备14008679号