모바일 로봇 PD, DOB 시뮬링크 구현 후 비교. [참고] 정슬, 로봇시스템, GS인터비전

1. Mobile Robot Simulation
강한솔
2015.06.11

2. IINDEX
2015-06-11
2
01 Introduction
02 Dynamics & Kinematics of Mobile Robot
03 Simulation
04 Conclusion

3. 2015-06-11
3
Introduction01
Pioneer 3-DX
Mapping &
Vision
Gripping &
Manipulation
Audio &
Speech

4. 2015-06-11
4
Introduction01
MSRDS

5. 2015-06-11
5
Introduction01
Pioneer 3-DX Spec
Operation
Robot Weight : 9kg
Operating Payload : 17kg
Dimensions (mm)

6. 2015-06-11
6
Introduction01
Goal
1 2 3
1
2
3
∅

7. 2015-06-11
7
Dynamics & Kinematics of Mobile Robot02
Kinematics
yx
xzx
xDG
dabyybx
daabyybx
dawvv







)(
Gx
bydybdx
bbdbyybx
x
xyx
)cos()sin(
)sincos(






O x
y
r2
L
yb
xbya
xa G
Gx
Gy
d
Gy















 











w
v
d
d
y
x
G
G
10
cossin
sincos





 sincos xyy bba 
D


cossincos
sincossin
dwvdyy
dwvdxx
G
G





8. 2015-06-11
8
Dynamics & Kinematics of Mobile Robot02
Kinematics
rrwvrrwv LLLRRR    ,























L
R
L
r
L
r
rr
w
v




22
)(
2
1
)(
2
1
rrvvv LRLR   
L
r
L
r
L
v
L
v
w
LRLR
LR









































L
R
G
G
L
r
L
r
L
r
d
r
L
r
d
r
L
r
d
r
L
r
d
r
y
x










cossin
2
cossin
2
sincos
2
sincos
2
RvLv
v
w
L

9. 2015-06-11
9
Dynamics & Kinematics of Mobile Robot02
Dynamics




















10
01
)cossin()cossin(
)sincos()sincos(
)( cc
dbcdbc
dbcdbc
qS


L
r
c
L
b  ,
2
 T
AqPqqCqqM  )(),()( 
0sincos    dxyyD
)(
2
sincos LRD
r
yxx   
No slipping
Pure rolling
0)(
22
0sincos
00cossin



























qqA
y
x
rr
d
L
R











 TTTTT
ASPSCSSSMS  )( 
  SSqqSq  ,)(

10. 2015-06-11
10
Dynamics & Kinematics of Mobile Robot02
Dynamics
L
r
c
L
b  ,
2




















L
R
L
R
w
w
IIccmbImbc
ImbcIIccmb






22222
22222
)(
)(
2
2
2
22 






L
mIdmII wbzcc
2
2
1
rmI ww 
  ),()()()()()()( qqVStqSqMStqSqMS TTT


11. 2015-06-11
11
Simulation03
Simulink Block Diagram – PD Control
][][
][][
e
j
de
j
dpLdLdpe
j
pe
j
ppLdLppL
e
j
de
j
dpRdRdpe
j
pe
j
ppRdRppR
kpkkkpkk
kpkkkpkk







   deeee yxp ,
22

12. 2015-06-11
12
Simulation03
Simulink Block Diagram – PD Control
][][
][][
e
j
de
j
dpLdLdpe
j
pe
j
ppLdLppL
e
j
de
j
dpRdRdpe
j
pe
j
ppRdRppR
kpkkkpkk
kpkkkpkk







   deeee yxp ,
22

13. 2015-06-11
13
Simulation03
Simulink Block Diagram – PD Control
][][
][][
e
j
de
j
dpLdLdpe
j
pe
j
ppLdLppL
e
j
de
j
dpRdRdpe
j
pe
j
ppRdRppR
kpkkkpkk
kpkkkpkk







   deeee yxp ,
22

14. 2015-06-11
14
Simulation03
Simulink Block Diagram – PD Control
][][
][][
e
j
de
j
dpLdLdpe
j
pe
j
ppLdLppL
e
j
de
j
dpRdRdpe
j
pe
j
ppRdRppR
kpkkkpkk
kpkkkpkk







   deeee yxp ,
22
function [qddotr,qddotl]= Dinv(qr,ql)
%L : distance of between wheels
%r : raius of wheel
%mc : mass of cart part
%mw : mass of wheel
%Rl : length of robot
%Rw : width of robot
L=0.381; r=0.0975; mc=8; mw=0.5;
Rl=0.455; Rw=0.393;
%m : mass of robot(mc+2mw) 9kg
m=mc+2*mw;
b=L/2; c=r/L;
%Ibz: moment of inertia (bz)
%Iw : moment of inertia (by)
%Ic : moment of inertia (cart)
%I : moment of inertia (entire)
Ibz=mw*r^2/4;
Iw=mw*r^2/2;
Ic=mc*(Rl^2+Rw^2)/12;
I=Ic+2*Ibz+2*mw*(L/2)^2;
d11=m*b^2*c^2+I*c^2+Iw;
d12=c^2*(m*b^2-I);
d21=c^2*(m*b^2-I);
d22=m*b^2*c^2+I*c^2+Iw;
D=[d11 d12;d21 d22];
D_inv=inv(D);
T_h=[qr; ql];
out=(D_inv*T_h);
qddotr=out(1);
qddotl=out(2);

15. 2015-06-11
15
Simulation03
Simulink Block Diagram – PD Control
][][
][][
e
j
de
j
dpLdLdpe
j
pe
j
ppLdLppL
e
j
de
j
dpRdRdpe
j
pe
j
ppRdRppR
kpkkkpkk
kpkkkpkk







   deeee yxp ,
22
function [xdot,ydot,phidot]= Jacobi(qdotr,qdotl,phi)
L=0.381; r=0.0975;
j11=r/2*cos(phi);
j12=r/2*cos(phi);
j21=r/2*sin(phi);
j22=r/2*sin(phi);
j31=r/L;
j32=-r/L;
J=[j11 j12;j21 j22;j31 j32];
U=[qdotr;qdotl];
out=J*U;
xdot=out(1);
ydot=out(2);
phidot=out(3);

16. 2015-06-11
16
Results
03 Simulation

17. 2015-06-11
17
Simulation03
Results

18. 2015-06-11
18
Simulation03
Simulink Block Diagram – DOB Control

19. 2015-06-11
19
Simulation03
Simulink Block Diagram – DOB Control
function [r,l]= Dhat(u1,u2)
%L : distance of between wheels
%r : raius of wheel
%mc : mass of cart part
%mw : mass of wheel
%Rl : length of robot
%Rw : width of robot
alpha = 0.8; %모델의 정확도
L=0.381; r=0.0975; mc=8; mw=0.5;
Rl=0.455; Rw=0.393;
%m : mass of robot(mc+2mw) 9kg
m=mc+2*mw;
b=L/2; c=r/L;
%Ibz: moment of inertia (bz)
%Iw : moment of inertia (by)
%Ic : moment of inertia (cart)
%I : moment of inertia (entire)
Ibz=mw*r^2/4;
Iw=mw*r^2/2;
Ic=mc*(Rl^2+Rw^2)/12;
I=Ic+2*Ibz+2*mw*(L/2)^2;
d11=m*b^2*c^2+I*c^2+Iw;
d12=c^2*(m*b^2-I);
d21=c^2*(m*b^2-I);
d22=m*b^2*c^2+I*c^2+Iw;
D=alpha*[d11 d12;d21
d22];
T_h=[u1;u2];
out=(D*T_h);
r=out(1);
l=out(2);

20. 2015-06-11
20
Simulation03
Simulink Block Diagram – DOB Control

21. 2015-06-11
21
Simulation03
Results

22. 2015-06-11
22
Simulation03
Results

23. 2015-06-11
23
Simulation03
Compare PD to DOB
PD Control
DOB Control

24. 2015-06-11
24
Simulation03
Compare PD to DOB
PD Control
DOB Control

25. 2015-06-11
25
Conclusion04
 엔코더만 사용시 heading angle의 부재로 정확한 위치제어가 어렵다.
 추가적인 센서를 이용해 좀 더 정밀한 위치제어가 가능하다.
Mobile Robot
 PD Control은 구성하기 쉬운 반면에, Disturbance에 취약하다는 것을 알 수 있었다.
 DOB에서 미분을 두 번하여 사용할 경우 굉장한 노이즈가 생기는 것을 확인할 수 있었다.
 DOB를 잘 활용하면 효과적인 Disturbance Cancellation을 얻을 수 있다.
PD Control & DOB Control

26. Q & A