elective in roboticselective in robotics - state estimation (m. vendittelli) 7attitude estimation...
Post on 10-Aug-2020
2 Views
Preview:
TRANSCRIPT
Elective in Robotics
State Estimation (Marilena Vendittelli)
Elective in Robotics - State Estimation (M. Vendittelli) 2
• Sensors
•Key state estimates
• attitude
•velocity
•position
Outline
Elective in Robotics - State Estimation (M. Vendittelli) 3
basic instrumentation for state estimation
• IMU• barometer
common augmentations
• sonar, laser, infrared (for eight measurements)• monocular camera
less common equipment
• RGB-D sensors (like Kinect)• scanning laser range finders• GPS• VICON
Sensors (1/4)
Elective in Robotics - State Estimation (M. Vendittelli) 4
Sensors (2/4) IMU & barometer
camerasonar
Elective in Robotics - State Estimation (M. Vendittelli) 5
IMU
Humming Bird (IMU strap down configuration)
Sensors (3/4)
(2) accelerometer (x,y,z)
gyroscope
(3) yaw
(4) pitch
(5) roll
Elective in Robotics - State Estimation (M. Vendittelli) 6
IMU
Sensors (4/4)
3D-MAG: three-axis compass
Humming Bird
Elective in Robotics - State Estimation (M. Vendittelli) 7
Attitude estimation
rate gyro
• measures the angular velocity of SRB relative to SRI, expressed in SRB
SRISRB
⌦IMU
= ⌦+ b
⌦
+ ⌘ 2 SRB
a
IMU
= R
T (v � g
!zI
) + ba + ⌘a 2 SRB
u = [uThu
Te ]
T v = [vThv
Te ]
T
uh = 1p2b(Fh + bVh) ue =
1p2b(Fe � bVe)
vh = 1p2b(Fh � bVh) ve =
1p2b(Fe + bVe)
1
2
RvTvdt 1
2
RuTudt
v(t) = S(t)u(t) ) F (t)� bV (t) = S(t)[F (t) + bV (t)]
v(s) = S(s)u(s) ) F (s)� bV (s) = S(s)[F (s) + bV (s)]
||S(s)|| 1
||S(s)|| = sup! �
1/2max
{S⇤(j!)S(j!)} 1
⇢x = f(x) + g(x)uy = h(x) x 2 R
n, u 2 R
m, y 2 R
m
⇢x
1
= f
1
(x1
) + g
1
(x1
)u1
y
1
= h
1
(x1
)
⇢x
2
= f
2
(x2
) + g
2
(x2
)u2
y
2
= h
2
(x2
)
⇢u
1
= ± y
2
+ v
1
u
2
= ⌥ y
1
+ v
2
1
magnetometer
⌦IMU
= ⌦+ b
⌦
+ ⌘
⌦
2 SRB
a
IMU
= R
T (v � g
!zI
) + ba + ⌘a 2 SRB
' � T
m
!zI
� T
m
DR
Tv
m
IMU
= R
T I
m+Bm + ⌘m 2 SRB
a
IMU
' � T
m
R
T !zI
˙R = R(⌦
IMU
� b)⇥ � ↵
˙b = kb↵
↵ = (ka
g
2
((RT !zI
)⇥ a
IMU
) +km
|Im|2 ((RT I
m)⇥m
IMU
)⇥ + . . .
I
a
IMU
= Ra
IMU
' Ra
IMU
' �gR
!zI
�gRDR
Tv
+
v = �1
g
(RDR
T )�1(IaIMU
+ gR
!zI
)
˙v = �g(R
!zI
+RDR
Tv)� kw(v � v)
1
constant or slowly time-varying bias
measurement noise
local magnetic disturbance
• provides measurements of the ambient magnetic field
Elective in Robotics - State Estimation (M. Vendittelli) 8
Attitude estimation SRISRB
accelerometer
• measures the instantaneous linear acceleration of SRB due to exogenous forces⌦
IMU
= ⌦+ b
⌦
+ ⌘
⌦
2 SRB
a
IMU
= R
T (v � g
!zI
) + ba + ⌘a 2 SRB
m
IMU
= R
T I
m+ bm + ⌘m 2 SRB
a
IMU
⌘ � T
m
R
T !zI
˙R = R(⌦
IMU
� b)⇥ � ↵
˙b = kb↵
↵ = (ka
g
2
((RT !zI
)⇥ a
IMU
) +km
|Im|2 ((RT I
m)⇥m
IMU
)⇥ + . . .
u = [uThu
Te ]
T v = [vThv
Te ]
T
uh = 1p2b(Fh + bVh) ue =
1p2b(Fe � bVe)
vh = 1p2b(Fh � bVh) ve =
1p2b(Fe + bVe)
1
2
RvTvdt 1
2
RuTudt
v(t) = S(t)u(t) ) F (t)� bV (t) = S(t)[F (t) + bV (t)]
v(s) = S(s)u(s) ) F (s)� bV (s) = S(s)[F (s) + bV (s)]
||S(s)|| 1
||S(s)|| = sup! �
1/2max
{S⇤(j!)S(j!)} 1
1
near hovering, i.e.,
⌦IMU
= ⌦+ b
⌦
+ ⌘
⌦
2 SRB
a
IMU
= R
T (v � g
!zI
) + ba + ⌘a 2 SRB
' � T
m
!zI
� T
m
DR
Tv
m
IMU
= R
T I
m+ bm + ⌘m 2 SRB
a
IMU
' � T
m
R
T !zI
˙R = R(⌦
IMU
� b)⇥ � ↵
˙b = kb↵
↵ = (ka
g
2
((RT !zI
)⇥ a
IMU
) +km
|Im|2 ((RT I
m)⇥m
IMU
)⇥ + . . .
u = [uThu
Te ]
T v = [vThv
Te ]
T
uh = 1p2b(Fh + bVh) ue =
1p2b(Fe � bVe)
vh = 1p2b(Fh � bVh) ve =
1p2b(Fe + bVe)
1
2
RvTvdt 1
2
RuTudt
v(t) = S(t)u(t) ) F (t)� bV (t) = S(t)[F (t) + bV (t)]
v(s) = S(s)u(s) ) F (s)� bV (s) = S(s)[F (s) + bV (s)]
||S(s)|| 1
||S(s)|| = sup! �
1/2max
{S⇤(j!)S(j!)} 1
1
constant or slowly time-varying biases
measurement noise
blade flapping
and induced drag
B
v ⇡ 0
z = x2 � ↵(x1)
x1 = 0 x2 = ↵(x1) ↵(0) = 0 g1(x1) 6= 0 g2(x1, x2) 6= 0 8(x1, x2)
x1 = f1(x1) + g1(x1)x2
x2 = f2(x1, x2) + g2(x1, x2)x3
.
.
.
xn = fn(x1, . . . , xn) + gn(x1, . . . , xn)u
x1 = f1(x1) + g1(x1)x2
x2 = f2(x1, x2) + g2(x1, x2)u
m
˙V I = �TRe3 + F e(V I ,˙V I ,⌦,
˙⌦,d(t)) +R⌃R ⌧
˙R = R S(⌦)
J ˙⌦ = �S(⌦)J ⌦+ ⌧ + ⌧ e(V I ,˙V I ,⌦,
˙⌦,d(t)) + ⌧ g +⌃T Te3
⌧1 ⌧2 ⌧3 ⌧ �!T ⌦
!ib
!jb
!kb
C ˙q
mg|dB = �|F (j!⇡)|dB m' = ⇡ + F (j!c)
m'1
mg! = 0
�! = 0
+! = �1 ! = +1 ✏ ! 0
F (s) =
K
s(1 + ⌧s)
x
NF = n
+F
1
Elective in Robotics - State Estimation (M. Vendittelli) 9
state observer
SRISRB
⌦IMU
= ⌦+ b
⌦
+ ⌘
⌦
2 SRB
a
IMU
= R
T (v � g
!zI
) + ba + ⌘a 2 SRB
m
IMU
= R
T I
m+ bm + ⌘m 2 SRB
a
IMU
⌘ � T
m
R
T !zI
˙R = R(⌦
IMU
� b)⇥ � ↵
˙b = kb↵
↵ = (ka
g
2
((RT !zI
)⇥ a
IMU
) +km
|Im|2 ((RT I
m)⇥m
IMU
)⇥ + . . .
u = [uThu
Te ]
T v = [vThv
Te ]
T
uh = 1p2b(Fh + bVh) ue =
1p2b(Fe � bVe)
vh = 1p2b(Fh � bVh) ve =
1p2b(Fe + bVe)
1
2
RvTvdt 1
2
RuTudt
v(t) = S(t)u(t) ) F (t)� bV (t) = S(t)[F (t) + bV (t)]
v(s) = S(s)u(s) ) F (s)� bV (s) = S(s)[F (s) + bV (s)]
||S(s)|| 1
||S(s)|| = sup! �
1/2max
{S⇤(j!)S(j!)} 1
1
Attitude estimation
complementary filter: uses high frequency part of gyro and low-frequency of accelerometer and magnetometer
possible contributions from other sensors
Elective in Robotics - State Estimation (M. Vendittelli) 10
hyp.: horizontal flight
SRISRBVelocity estimation
⌦IMU
= ⌦+ b
⌦
+ ⌘
⌦
2 SRB
a
IMU
= R
T (v � g
!zI
) + ba + ⌘a 2 SRB
' � T
m
!zI
� T
m
DR
Tv
m
IMU
= R
T I
m+ bm + ⌘m 2 SRB
a
IMU
' � T
m
R
T !zI
˙R = R(⌦
IMU
� b)⇥ � ↵
˙b = kb↵
↵ = (ka
g
2
((RT !zI
)⇥ a
IMU
) +km
|Im|2 ((RT I
m)⇥m
IMU
)⇥ + . . .
I
a
IMU
= Ra
IMU
' Ra
IMU
' �gR
!zI
�gRDR
Tv
+
v = �1
g
(RDR
T )�1(IaIMU
+ gR
!zI
)
˙v = �g(R
!zI
+RDR
Tv)� kw(v � v)
1
Elective in Robotics - State Estimation (M. Vendittelli) 11
Position estimation
• height and position in the plane are often decoupled
• absolute: barometer, limited information from IMU, GPS, VICON,...
• relative: acoustic, laser-ranging or infrared, RGB-D cameras, SLAM
• measures provided by sensors are often fused in a Kalman filter
Elective in Robotics - Quadrotor Modeling (M. Vendittelli) 12
• R. Mahony, V. Kumar, P. Corke, "Multirotor Aerial Vehicles: Modeling, Estimation, and Control of Quadrotor," IEEE Robotics & Automation Magazine, vol.19, no.3, pp. 20-32, Sept. 2012.
• R. Mahony, T. Hamel and J. M. Pflimlin, "Nonlinear Complementary Filters on the Special Orthogonal Group," in IEEE Transactions on Automatic Control, vol. 53, no. 5, pp. 1203-1218, June 2008. doi: 10.1109/TAC.2008.923738
References
top related