简单记录一下导纳控制。
对于一个质量块
m
m
m，其位移记为
x
x
x，我们期望它按照预期保持在
x
0
x_0
x0。质量块受到环境的外力，记为
F
e
x
t
F_{ext}
Fext。施加的控制的力记为
F
F
F。
导纳控制希望通过控制使得将外力转化为相对期望位移的偏差位移。
m
x
¨
=
F
+
F
e
x
t
m\ddot x=F+F_{ext}
mx¨=F+Fext
e
=
x
−
x
0
，
M
d
e
¨
+
D
d
e
˙
+
K
d
e
=
F
e
x
t
e=x-x_0，M_d\ddot e+D_d\dot e+K_de=F_{ext}
e=x−x0，Mde¨+Dde˙+Kde=Fext对于导纳控制，其后接一个
P
D
PD
PD位置控制器
F
=
k
p
(
x
d
−
x
)
−
k
d
x
˙
F=k_p(x_d-x)-k_d\dot x
F=kp(xd−x)−kdx˙
m
x
¨
+
k
d
x
˙
+
k
p
(
x
−
x
d
)
=
F
e
x
t
m\ddot x+k_d\dot x+k_p(x-x_d)=F_{ext}
mx¨+kdx˙+kp(x−xd)=Fext
M
d
(
x
¨
d
−
x
¨
0
)
+
D
d
(
x
˙
d
−
x
˙
0
)
+
K
d
(
x
d
−
x
0
)
=
F
e
x
t
M_d(\ddot x_d-\ddot x_0)+D_d(\dot x_d-\dot x_0)+K_d(x_d-x_0)=F_{ext}
Md(x¨d−x¨0)+Dd(x˙d−x˙0)+Kd(xd−x0)=Fext
为更好地理解，我们进行
M
a
t
l
a
b
Matlab
Matlab仿真。
先将上式转化成状态空间模型。
取
x
1
=
x
x_1=x
x1=x，
x
2
=
x
˙
x_2=\dot x
x2=x˙，
x
3
=
x
d
x_3=x_d
x3=xd，
x
4
=
x
˙
d
x_4=\dot x_d
x4=x˙d。
取期望轨迹
x
0
=
t
x_0=t
x0=t，则
x
˙
1
=
x
2
\dot x_1=x_2
x˙1=x2
x
˙
2
=
−
k
p
m
x
1
−
k
d
m
x
2
+
k
p
m
x
3
+
1
m
F
e
x
t
\dot x_2=-\frac{k_p}{m}x_1-\frac{k_d}{m}x_2+\frac{k_p}{m}x_3+\frac{1}{m}F_{ext}
x˙2=−mkpx1−mkdx2+mkpx3+m1Fext
x
˙
3
=
x
4
\dot x_3=x_4
x˙3=x4
x
˙
4
=
−
K
d
M
d
x
3
−
D
d
M
d
x
4
+
D
d
+
K
d
t
M
d
+
1
M
d
F
e
x
t
\dot x_4=-\frac{K_d}{M_d}x_3-\frac{D_d}{M_d}x_4+\frac{D_d+K_dt}{M_d}+\frac{1}{M_d}F_{ext}
x˙4=−MdKdx3−MdDdx4+MdDd+Kdt+Md1Fext
取
m
=
1
m=1
m=1，
F
e
x
t
=
0.1
s
i
n
(
t
)
F_{ext}=0.1sin(t)
Fext=0.1sin(t)
我们先将Mass-Damper-Spring与PD的参数设为1
function xdot = mass_env(t, x);
xdot(1)=x(2);
xdot(2)=[-1 -1 1 0]*x+0.1*sin(t);
xdot(3)=x(4);
xdot(4)=[0 0 -1 -1]*x+1+t+0.1*sin(t);
xdot=xdot';
t0=0;tf=100;step=0.001;
x0(1:4) = [0 0 0 0]';
[t x] = ode45('mass_env',[t0:step:tf], x0);
plot(t, x(:,1));
下图分别为质量块的位置，和与期望位置的差值。
下面，我们考虑用导纳控制进行力的控制。
记期望力
F
d
F_d
Fd
F
=
k
p
(
x
d
−
x
)
−
k
d
x
˙
F=k_p(x_d-x)-k_d\dot x
F=kp(xd−x)−kdx˙
m
x
¨
+
k
d
x
˙
+
k
p
(
x
−
x
d
)
=
F
e
x
t
m\ddot x+k_d\dot x+k_p(x-x_d)=F_{ext}
mx¨+kdx˙+kp(x−xd)=Fext
M
d
x
¨
d
+
D
d
x
˙
d
+
K
d
x
d
=
F
d
−
F
−
F
e
x
t
M_d\ddot x_d+D_d\dot x_d+K_dx_d=F_d-F-F_{ext}
Mdx¨d+Ddx˙d+Kdxd=Fd−F−Fext
为更好地理解，我们进行
M
a
t
l
a
b
Matlab
Matlab仿真。
先将上式转化成状态空间模型。
取
x
1
=
x
x_1=x
x1=x，
x
2
=
x
˙
x_2=\dot x
x2=x˙，
x
3
=
x
d
x_3=x_d
x3=xd，
x
4
=
x
˙
d
x_4=\dot x_d
x4=x˙d，则
x
˙
1
=
x
2
\dot x_1=x_2
x˙1=x2
x
˙
2
=
−
k
p
m
x
1
−
k
d
m
x
2
+
k
p
m
x
3
+
1
m
F
e
x
t
\dot x_2=-\frac{k_p}{m}x_1-\frac{k_d}{m}x_2+\frac{k_p}{m}x_3+\frac{1}{m}F_{ext}
x˙2=−mkpx1−mkdx2+mkpx3+m1Fext
x
˙
3
=
x
4
\dot x_3=x_4
x˙3=x4
x
˙
4
=
k
p
M
d
x
1
+
k
d
M
d
x
2
−
K
d
+
k
p
M
d
x
3
−
D
d
M
d
x
4
+
1
M
d
(
F
d
−
F
e
x
t
)
\dot x_4=\frac{k_p}{M_d}x_1+\frac{k_d}{M_d}x_2-\frac{K_d+k_p}{M_d}x_3-\frac{D_d}{M_d}x_4+\frac{1}{M_d}(F_d-F_{ext})
x˙4=Mdkpx1+Mdkdx2−MdKd+kpx3−MdDdx4+Md1(Fd−Fext)
取
m
=
1
m=1
m=1，
F
e
x
t
=
0.1
s
i
n
(
t
+
0.1
)
F_{ext}=0.1sin(t+0.1)
Fext=0.1sin(t+0.1)，
F
d
=
5
s
i
n
(
t
)
F_d=5sin(t)
Fd=5sin(t)
我们先将Mass-Damper-Spring与PD的参数设为1
function xdot = mass_env1(t, x);
xdot(1)=x(2);
xdot(2)=[-1 -1 1 0]*x+0.1*sin(t+0.1);
xdot(3)=x(4);
xdot(4)=[1 1 -2 -1]*x+1+5*sin(t)-sin(t+0.1);
xdot=xdot';
t0=0;tf=100;step=0.001;
x0(1:4) = [0 0 0 0]';
[t x] = ode45('mass_env1',[t0:step:tf], x0);
plot(t, x(:,3)-x(:,1)-x(:,2));
此时，
F
d
F_d
Fd与
F
+
F
e
x
t
F+F_{ext}
F+Fext的差如下图
我们提高
K
d
K_d
Kd的值到
99
99
99，观察到误差缩小，如下图
