Matrix Pilot Presentation by Paul Bizard

Report
UAVDevBoard – MatrixPilot
Block diagrams 01-02-2012
MatrixPilot – Glossary
cCL
cSP
ySP
y
q
f
qSP
z
zSP
Th
Thm
Em
Am
Rm
Va
Vg
Vw
VIMU
Sc
course leg bearing
bearing set point
yaw set point
yaw
pitch
roll
pitch setpoint
altitude
altitude setpoint
throttle command
manual throttle command
manual elevator command
manual aileron command
manual rudder command
airspeed
ground speed
wind velocity
speed from IMU
steering command
de
dr
da
elevator deflection (>0 up)
rudder deflection (>0 right)
aileron deflection (>0 right)
MatrixPilot – Parameters
Tmax
Tmin
qmax
qmin
q0
qRTL
DH
VSP
PKP
PKD
KRUE
KROE
KEBO
YKPA
RKPA
RKDA
YKDA
KABO
YKPR
YKDR
RKPR
KARU
KRBO
ALT_HOLD_THROTTLE_MAX
ALT_HOLD_THROTTLE_MIN
ALT_HOLD_PITCH_MAX
ALT_HOLD_PITCH_MIN
ALT_HOLD_PITCH_HIGH
RTL_PITCH_DOWN
HEIGHT_MARGIN
DESIRED_SPEED
PITCHGAIN
PITCHKD
RUDDER_ELEV_MIX
ROLL_ELEV_MIX
ELEVATOR_BOOST
YAWKP_AILERON
ROLLKP
ROLLKD
YAWKD_AILERON
AILERON_BOOST
YAWKR_RUDDER
YAWKD_RUDDER
ROLLKP_RUDDER
MANUAL_AILERON_RUDDER_MIX
RUDDER_BOOST
MatrixPilot – Coordinate Systems – Industry Standard Convention
xb
q
y
ye
y
xe
f
yb
zb
f
q
ze
Earth fixed reference frame: (xe, ye, ze)
Body-fixed reference frame: (xb, yb, zb)
Euler angles yaw, pitch & roll: (y, q, f)
MatrixPilot – Rotation rates – Industry Standard Convention
xb
p
(p, q, r) are the coordinates of the rotational vector W
expressed in the body-fixed reference frame (xb, yb, zb)
q
yb
r
zb

=  + tan   sin  +  cos 


=  cos  −  sin 

  sin  +  cos 
=

cos 
MatrixPilot – Coordinate Systems – Warning
This presentation uses the industry standard convention for aerospace coordinate systems.
The plane coordinate system coincide with the Earth-fixed reference frame when the plane
is located at the origin of the Earth-fixed reference frame, pointing North, with the plane
level with respect to both pitch q and roll f.
For historical reasons the plane and the earth coordinate systems used by the
UAVDevBoard software differ slightly from the industry standard convention.
Please refer to:
http://code.google.com/p/gentlenav/wiki/UDBCoordinateSystems
The relation between the Direct Cosine Matrix and the Euler angles is (standard
convention):
cos  cos 
 = cos  sin 
− sin 
sin  sin  cos  − cos  sin 
sin  sin  sin  + cos  cos 
sin  cos 
cos  sin  cos  + sin  sin 
cos  sin  sin  − sin  cos 
cos  cos 
MatrixPilot – Altitude Control (full altitude hold or pitch only)
speed control
VSP
Va
VIMU
2
V SP - V
2
0
DzV
Thm
2g
Min()
T
Tmax
  70ms
1
Th
1  s
Tmin
zSP
Dz
full altitude
hold
-DH
0
DH Dz
q
z
-DH
qmin
q0
qmax
0 DH
Dz
qSP
MatrixPilot – Normal Pitch Control
pitch stabilization
0
qSP
sinq
dq
dt
qRTL
cosq
dr
PKP
radio off
& GPS steering
0
PKD
0
KRUE
cosq sinf
(cosq
sinf)2
rudder input used
& rudder output used
& pitch feedback
pitch feedback
0
KROE
radio ON & pitch feedback
Em
0
KEBO
de
MatrixPilot – Normal Pitch Control
The Direct Cosine Matrix is used as much as possible:
Pitch
θ ≃ sin  = −31
Pitch rate


=  ≃ cos 
= cos   cos  −  sin  =  ⋅ 33 +  ⋅ 32


Roll
 ≃ cos  sin  = 32
MatrixPilot – Waypoints Normal Navigation
waypoint (n+1)
Nc
SP
waypoint
radius
Vg
finish line
The bearing set point cSP is equal to the
bearing between the plane and the next
waypoint
waypoint (n)
course leg
The finish line is perpendicular to the
course leg
MatrixPilot – Waypoints Cross Track Navigation
waypoint (n+1)
If the cross track error is greater than
CTMARGIN the plane bearing set
point cSP is limited to the course leg
bearing cCL plus or minus 45°
Nc
SP
Vg
waypoint
radius
finish line
cross track error
CTMARGIN
Nc
If the cross track error is lower than CTMARGIN
the deviation of the plane bearing set point cSP
relatively to the course leg bearing cCL is
proportional to the cross track error
CL
waypoint (n)
course leg
The finish line is perpendicular to the
course leg
MatrixPilot – Navigation – Yaw Set Point
N
The yaw set point ySP is not strictly
equal to the bearing set point cSP in
order to take into account the crabbing
of the airplane due to the wind
x
Va
ySP
cSP
Vw ⊥
Vw
Vg
y
 ⊥ =  sin  −  cos 

 ⊥
=  + arcsin

MatrixPilot – Navigation – Yaw Angle Error
The error between the yaw set point and the actual yaw is computed using the Direct
Cosine Matrix
cos   =
x
N
sin y
cos   ×  =

1
sin
ySP
cos  cos 
11
=
21
cos  sin 
ySP
y
Dy
cos 
11
×
21
sin 
= cos  cos  sin  −   sin 

= cos  sin  − 
= cos  sin 
y
1
cos y
cos ySP
≃ 
MatrixPilot – Navigation – Steering Command
The steering command is set to a constant value if the yaw error is greater than 90° or
lower than -90°
Sc
cosq
cosq sinDy
ySP
Dy
-180° -90°
0
90°
Sc
180° Dy
y
-cosq
wind_gain
The steering command is homogeneous to a bank angle
MatrixPilot – Normal Roll Control
aileron navigation
& GPS steering
0
Sc
da
YKPA
roll stabilization aileron
& pitch feedback
0
cosq sinf
RKPA
roll stabilization aileron
& pitch feedback
p
0
RKDA
yaw stabilization aileron
& pitch feedback
0
r
YKDA
radio ON
& pitch feedback
0
Am
KABO
MatrixPilot – Normal Yaw Control
rudder navigation
& GPS steering
0
Sc
dr
YKPR
roll stabilization rudder
& pitch feedback
0
cosq sinf
RKPR
yaw stabilization rudder
& pitch feedback
r
0
YKDR
pitch feedback
0
Am
KARU
radio ON
& pitch feedback
0
Rm
KRBO

similar documents