xb

yb

y P 
r

O x

- Oxy hệ tọa độ cố định

- Pxbyb hệ tọa độ IMU
Vị trí P trong Oxy

 cos  
OP  r  r  

sin  
Vận tốc điểm P trong Oxy

 sin  
v  r  r   

cos  
Gia tốc điểm P trong Oxy

cos    sin  
a  v  r   
 r 
2    (1)
 
sin   cos  
b T
Kí hiệu a  [ax , ay ] là tọa độ của a trong Pxbyb , ta có
  cos   sin    cos   sin   a  a cos   a sin  

a  b
a    x   x y  (2)
     
 sin  cos    sin  cos   ay  ax sin   ay cos  
Kết hợp (1) và (2), ta được
a cos   a sin      
 x y   r  cos   2  r  sin   
a sin   a cos    sin    cos  
 x y     
Giải ra ta có

ay
 
r (3)
ax
2 
r
Thí nghiệm:

- Ước lượng ax , ay bằng IMU + lọc Kalman

- Đo r
- Tính  theo công thức (3) và vẽ các đồ thị để so sánh với tính theo encoder.
- Tính  theo công thức (3) và vẽ các đồ thị để so sánh:
o Tính  theo encoder
o Tính  theo gyroscope của IMU + lọc Kalman