Matrix Pilot Presentation by Paul Bizard
advertisement
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
