#机器人 #控制 #误差动力学 #设定点控制 #计算力矩控制器 #PID控制
详细可参考书本:《现代机器人学:机构规划与控制》第十一章
11.2 误差动力学(定义)
误差动力学(Error dynamic):决定受控系统关节误差 θ e ( t ) \theta_e(t) θe(t)的演化的微分方程称为误差动力学。其中, θ e ( t ) = θ d ( t ) − θ ( t ) \theta_e(t)=\theta_d(t)-\theta(t) θe(t)=θd(t)−θ(t)
误差响应(Error response):定义为受控系统在初始条件为 θ e ( 0 ) = 1 \theta_e(0)=1 θe(0)=1,且 θ ˙ e ( 0 ) = θ ¨ e ( 0 ) = ⋯ = 0 \dot\theta_e(0)=\ddot\theta_e(0)=\cdots=0 θ˙e(0)=θ¨e(0)=⋯=0。 θ e ( t ) ( t > 0 ) \theta_e(t)(t>0) θe(t)(t>0)的响应。
典型的误差响应可以通过瞬态响应和稳态响应进行描述。
- 稳态响应 e s s e_{ss} ess:表示在 θ e ( t ) \theta_e(t) θe(t)在 t → ∞ t\rightarrow\infty t→∞时的渐进误差;
- 瞬态响应:通过超调和(2%)调节时间进行表征。
超调 = ∣ θ e , min − e s s θ e ( 0 ) − e s s ∣ × 100 % \text{超调}=\left| \frac{\theta _{\mathrm{e},\min}-e_{ss}}{\theta _e\left( 0 \right) -e_{ss}} \right|\times 100\% 超调= θe(0)−essθe,min−ess ×100%
误差响应示例
力或力矩输入的设定点控制
单个连杆动力学模型:
τ
=
M
θ
¨
+
b
θ
˙
+
m
g
r
cos
θ
\tau =M\ddot{\theta}+b\dot{\theta}+mgr\cos \theta
τ=Mθ¨+bθ˙+mgrcosθ
PID控制器为:
τ
=
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
+
K
d
θ
˙
e
\tau =K_p\theta _e+K_i\int{\theta _e\left( t \right) \mathrm{d}t+K_d\dot{\theta}_e}
τ=Kpθe+Ki∫θe(t)dt+Kdθ˙e
水平面内的设定点控制(PD)
考虑
K
i
=
0
K_i=0
Ki=0,假设机器人在平面内运动(无重力影响),将PD控制律带入运动学模型:
M
θ
¨
+
b
θ
˙
=
K
p
θ
e
+
K
d
θ
˙
e
(1)
M\ddot{\theta}+b\dot{\theta}=K_p\theta _e+K_d\dot{\theta}_e \tag{1}
Mθ¨+bθ˙=Kpθe+Kdθ˙e(1)
若控制目标为设定点控制,即
θ
d
\theta_d
θd恒定,则有
θ
˙
d
=
θ
¨
d
=
0
\dot{\theta}_d=\ddot{\theta}_d=0
θ˙d=θ¨d=0。
那么
θ
e
=
θ
d
−
θ
,
θ
˙
e
=
−
θ
˙
,
θ
¨
e
=
−
θ
¨
\theta _e=\theta _d-\theta ,\dot{\theta}_e=-\dot{\theta},\ddot{\theta}_e=-\ddot{\theta}
θe=θd−θ,θ˙e=−θ˙,θ¨e=−θ¨. 上式可改写为:
−
M
θ
¨
e
−
b
θ
˙
e
=
K
p
θ
e
+
K
d
θ
˙
e
-M\ddot{\theta}_e-b\dot{\theta}_e=K_p\theta _e+K_d\dot{\theta}_e
−Mθ¨e−bθ˙e=Kpθe+Kdθ˙e
⇒
M
θ
¨
e
+
(
K
d
+
b
)
θ
˙
e
+
K
p
θ
e
=
0
(2)
\Rightarrow M\ddot{\theta}_e+\left( K_d+b \right) \dot{\theta}_e+K_p\theta _e=0 \tag{2}
⇒Mθ¨e+(Kd+b)θ˙e+Kpθe=0(2)
将上式转换为标准二阶形式:
θ
¨
e
+
K
d
+
b
M
θ
˙
e
+
K
p
M
θ
e
=
0
\ddot{\theta}_e+\frac{K_d+b}{M}\dot{\theta}_e+\frac{K_p}{M}\theta _e=0
θ¨e+MKd+bθ˙e+MKpθe=0
⇒
θ
¨
e
+
2
ζ
ω
n
θ
˙
e
+
ω
n
2
θ
e
=
0
\Rightarrow \ddot{\theta}_e+2\zeta \omega _n\dot{\theta}_e+\omega _{n}^{2}\theta _e=0
⇒θ¨e+2ζωnθ˙e+ωn2θe=0
式中,
ζ
=
b
+
K
d
2
K
p
M
,
ω
n
=
K
p
M
\zeta =\frac{b+K_d}{2\sqrt{K_pM}},\omega _n=\sqrt{\frac{K_p}{M}}
ζ=2KpMb+Kd,ωn=MKp.
垂直面内的设定点控制(PID)
当考虑在垂直平面内的设定点控制
(
g
>
0
)
\left( g>0 \right)
(g>0)时,误差动力学为:
M
θ
¨
e
+
(
K
d
+
b
)
θ
˙
e
+
K
p
θ
e
=
m
g
r
cos
θ
M\ddot{\theta}_e+\left( K_d+b \right) \dot{\theta}_e+K_p\theta _e=mgr\cos \theta
Mθ¨e+(Kd+b)θ˙e+Kpθe=mgrcosθ
这意味着关节停留在满足
K
p
θ
e
=
m
g
r
cos
θ
K_p\theta_e=mgr\cos\theta
Kpθe=mgrcosθ这一条件的位形
θ
\theta
θ处,即当
θ
d
≠
±
π
/
2
\theta _d\ne \pm {{\pi}/{2}}
θd=±π/2时,最终误差
θ
e
\theta_e
θe非零。其原因是:为了让连杆在
θ
d
≠
±
π
/
2
\theta _d\ne \pm {{\pi}/{2}}
θd=±π/2处保持静止,机器人必须提供非零扭矩,但是只有当
θ
e
≠
0
\theta_e\ne0
θe=0时,PD控制律才能在静止时产生非零扭矩。一般而言,课题公国增加增益
K
p
K_p
Kp来减小这个稳态误差。
为了消除稳态误差,设置
K
i
>
0
K_i>0
Ki>0,即使在零位置误差情况下,它也允许非零的稳态扭矩,此时只有积分误差必须为零。此时有,
{
τ
=
M
θ
¨
+
b
θ
˙
+
m
g
r
cos
θ
τ
=
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
+
K
d
θ
˙
e
\begin{cases} \tau =M\ddot{\theta}+b\dot{\theta}+mgr\cos \theta\\ \tau =K_p\theta _e+K_i\int{\theta _e\left( t \right) \mathrm{d}t}+K_d\dot{\theta}_e\\ \end{cases}
{τ=Mθ¨+bθ˙+mgrcosθτ=Kpθe+Ki∫θe(t)dt+Kdθ˙e
若控制目标为设定点控制:
θ
˙
d
=
θ
¨
d
=
0
⇒
θ
e
=
θ
d
−
θ
,
θ
˙
e
=
−
θ
˙
,
θ
¨
e
=
−
θ
¨
\dot{\theta}_d=\ddot{\theta}_d=0\Rightarrow \theta _e=\theta _d-\theta ,\dot{\theta}_e=-\dot{\theta},\ddot{\theta}_e=-\ddot{\theta}
θ˙d=θ¨d=0⇒θe=θd−θ,θ˙e=−θ˙,θ¨e=−θ¨.则设定点的误差动力学为:
M
θ
¨
e
+
(
K
d
+
b
)
θ
˙
e
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
=
τ
d
i
s
t
M\ddot{\theta}_e+\left( K_d+b \right) \dot{\theta}_e+K_p\theta _e+K_i\int{\theta _e\left( t \right) \mathrm{d}t}=\tau _{\mathrm{dist}}
Mθ¨e+(Kd+b)θ˙e+Kpθe+Ki∫θe(t)dt=τdist
式中,
τ
d
i
s
t
=
m
g
r
cos
θ
\tau _{\mathrm{dist}}=mgr\cos \theta
τdist=mgrcosθ,代表代替重力项的扰动力矩。对上式两边求导,可得三阶误差动力学:
M
θ
¨
e
+
(
K
d
+
b
)
θ
¨
e
+
K
p
θ
˙
e
+
K
i
θ
e
=
τ
˙
d
i
s
t
M\ddot{\theta}_e+\left( K_d+b \right) \ddot{\theta}_e+K_p\dot{\theta}_e+K_i\theta _e=\dot{\tau}_{dist}
Mθ¨e+(Kd+b)θ¨e+Kpθ˙e+Kiθe=τ˙dist
为确保上式所有根都具有负实部,其控制增益必须满足以下条件以确保稳定性,
{
K
d
>
−
b
,
K
p
>
0
,
(
b
+
K
d
)
K
p
M
>
K
i
>
0
\begin{cases} K_d>-b,\\ K_p>0,\\ \frac{\left( b+K_d \right) K_p}{M}>K_i>0\\ \end{cases}
⎩
⎨
⎧Kd>−b,Kp>0,M(b+Kd)Kp>Ki>0
前馈控制
轨迹跟踪的另一个策略时使用机器人的动力学模型来主动产生力矩,而不是等待误差。 令控制器动力学模型为:
τ
=
M
~
(
θ
(
t
)
)
θ
¨
(
t
)
+
h
~
(
θ
(
t
)
,
θ
˙
(
t
)
)
\tau =\widetilde{M}\left( \theta \left( t \right) \right) \ddot{\theta}\left( t \right) +\widetilde{h}\left( \theta \left( t \right) ,\dot{\theta}\left( t \right) \right)
τ=M
(θ(t))θ¨(t)+h
(θ(t),θ˙(t))
式中,如果
M
~
(
θ
)
=
M
(
θ
)
,
h
~
(
θ
,
θ
˙
)
=
h
(
θ
,
θ
˙
)
\widetilde{M}\left( \theta \right) =M\left( \theta \right) , \widetilde{h}\left( \theta ,\dot{\theta} \right) =h\left( \theta ,\dot{\theta} \right)
M
(θ)=M(θ),h
(θ,θ˙)=h(θ,θ˙),那么模型是完美的。
给定轨迹发生器的
θ
d
,
θ
˙
d
,
θ
¨
d
\theta _d, \dot{\theta}_d, \ddot{\theta}_d
θd,θ˙d,θ¨d,则通过控制器得到的前馈力矩可表示为:
τ
=
M
~
(
θ
d
(
t
)
)
θ
¨
d
(
t
)
+
h
~
(
θ
d
(
t
)
,
θ
˙
d
(
t
)
)
\tau =\widetilde{M}\left( \theta _d\left( t \right) \right) \ddot{\theta}_d\left( t \right) +\widetilde{h}\left( \theta _d\left( t \right) ,\dot{\theta}_d\left( t \right) \right)
τ=M
(θd(t))θ¨d(t)+h
(θd(t),θ˙d(t))
显然,对于任意一个模型,都无法准确实现
M
~
(
θ
)
=
M
(
θ
)
,
h
~
(
θ
,
θ
˙
)
=
h
(
θ
,
θ
˙
)
\widetilde{M}\left( \theta \right) =M\left( \theta \right) , \widetilde{h}\left( \theta ,\dot{\theta} \right) =h\left( \theta ,\dot{\theta} \right)
M
(θ)=M(θ),h
(θ,θ˙)=h(θ,θ˙),因此单独的前馈控制意义不大,一般总是将前馈控制与反馈控制一起使用。
前馈加反馈线性化
考虑如下误差动力学:
θ
e
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
+
K
d
θ
˙
e
=
0
\theta _e+K_p\theta _e+K_i\int{\theta _e\left( t \right) \mathrm{d}t}+K_d\dot{\theta}_e=0
θe+Kpθe+Ki∫θe(t)dt+Kdθ˙e=0
注意,上述误差动力学与前面的并不相同,可以理解为采用PID控制器对误差
θ
e
\theta_e
θe进行控制,由于
θ
¨
e
=
θ
¨
d
−
θ
¨
\ddot{\theta}_e=\ddot{\theta}_d-\ddot{\theta}
θ¨e=θ¨d−θ¨,选取指令加速度为
θ
¨
=
θ
¨
d
−
θ
¨
e
\ddot{\theta}=\ddot{\theta}_d-\ddot{\theta}_e
θ¨=θ¨d−θ¨e,可以得到:
θ
¨
=
θ
¨
d
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
+
K
d
θ
˙
e
⏟
−
θ
¨
e
(3)
\ddot{\theta}=\ddot{\theta}_d+\underset{-\ddot{\theta}_e}{\underbrace{K_p\theta _e+K_i\int{\theta _e\left( t \right) \mathrm{d}t}+K_d\dot{\theta}_e}} \tag{3}
θ¨=θ¨d+−θ¨e
Kpθe+Ki∫θe(t)dt+Kdθ˙e(3)
将公式(3)带入机器人的动力学模型
{
M
~
,
h
~
}
\left\{ \widetilde{M},\widetilde{h} \right\}
{M
,h
}中,即可得到前馈加反馈线性化控制器,也称为逆动力学控制器,或计算力矩控制器:
τ
=
M
~
(
θ
)
[
θ
¨
d
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
+
K
d
θ
˙
e
]
+
h
~
(
θ
,
θ
˙
)
(11.33)
\tau =\widetilde{M}\left( \theta \right) \left[ \ddot{\theta}_d+K_p\theta _e+K_i\int{\theta _e\left( t \right) \mathrm{d}t}+K_d\dot{\theta}_e \right] +\widetilde{h}\left( \theta ,\dot{\theta} \right) \tag{11.33}
τ=M
(θ)[θ¨d+Kpθe+Ki∫θe(t)dt+Kdθ˙e]+h
(θ,θ˙)(11.33)