CN108614573A - The automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicles - Google Patents

The automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicles Download PDF

Info

Publication number
CN108614573A
CN108614573A CN201810460050.6A CN201810460050A CN108614573A CN 108614573 A CN108614573 A CN 108614573A CN 201810460050 A CN201810460050 A CN 201810460050A CN 108614573 A CN108614573 A CN 108614573A
Authority
CN
China
Prior art keywords
motor
unmanned aerial
wing unmanned
rotor wing
axis
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201810460050.6A
Other languages
Chinese (zh)
Other versions
CN108614573B (en
Inventor
常亮
蔡星
孙培
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Expand Intelligent Technology Co Ltd
Original Assignee
Shanghai Expand Intelligent Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Expand Intelligent Technology Co Ltd filed Critical Shanghai Expand Intelligent Technology Co Ltd
Priority to CN201810460050.6A priority Critical patent/CN108614573B/en
Publication of CN108614573A publication Critical patent/CN108614573A/en
Application granted granted Critical
Publication of CN108614573B publication Critical patent/CN108614573B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10Simultaneous control of position or course in three dimensions
    • G05D1/101Simultaneous control of position or course in three dimensions specially adapted for aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The present invention provides a kind of automatic fault tolerant attitude control methods of six rotor wing unmanned aerial vehicles, include the following steps:The electric current and/or voltage for acquiring multiple motors on six rotor wing unmanned aerial vehicles judge that each motor whether there is failure according to the electric current and/or voltage;When it is faulty motor to judge a motor, another motor adjacent with the faulty motor and opposite rotation direction is closed;The serial number of the paddle i driven according to the faulty motor selects corresponding mapping matrix Ri, and according to the mapping matrix RiInput attitude angle is converted into attitude angle in order to control, i is the natural number for being less than or equal to 6 more than or equal to 0;The control attitude angle is inputted into the flight control modules of six rotor wing unmanned aerial vehicle and is flown with controlling six rotor wing unmanned aerial vehicle.Present invention when having inclined setting angle of each motor in six rotor wing unmanned aerial vehicles can optimize power distribution according to the fault condition of motor, realize the faults-tolerant control of six rotor wing unmanned aerial vehicles.

Description

The automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicles
Technical field
The present invention relates to multi-rotor unmanned aerial vehicles, and in particular, to a kind of automatic fault tolerant gesture stability of six rotor wing unmanned aerial vehicles Method.
Background technology
Six rotor wing unmanned aerial vehicles of standard are compared than four axis rotor wing unmanned aerial vehicles, there is better stability.It is equipped with more Motor is to obtain better control effect.Six rotor wing unmanned aerial vehicles in the case of a motor failure, still than four axis rotors without A man-machine more motor.And this is often still controllable by the aircraft of a motor of thinking to fail for mistake.
But it in fact can not strict guarantee hovering after losing a motor when six rotor wing unmanned aerial vehicles.And it is directed to and incites somebody to action Motor has the unmanned plane of mounted angle, in the case of 6 propeller normal works and 1 damage, general unmanned machine Flying control algorithm does not support motor to have the control of the unmanned plane and bad paddle of mounted angle.Therefore it needs to design a kind of control algolithm The faults-tolerant control of six rotor wing unmanned aerial vehicles is realized with framework.
Invention content
For the defects in the prior art, the object of the present invention is to provide a kind of automatic fault tolerant postures of six rotor wing unmanned aerial vehicles Control method.Needle of the present invention changes the setting angle of six rotor wing unmanned aerial vehicle motors, optimizes power according to the fault condition of motor The faults-tolerant control of six rotor wing unmanned aerial vehicles is realized in distribution.The wherein position of six rotor wing unmanned aerial vehicles and gesture stability can be described as follows: Remote controler sends out pitching, roll, yaw angle and throttle command, and six rotor wing unmanned aerial vehicles receive the order of remote controler, at six Automatic fault tolerant control may be implemented when motor is normal and an electrical fault.
According to the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle provided by the invention, six rotor wing unmanned aerial vehicle Each motor has with respect to the horizontal plane tilts established angle α, inclination established angle β in another direction along a side, including walks as follows Suddenly:
Step S1:The electric current and/or voltage for acquiring multiple motors on six rotor wing unmanned aerial vehicles, according to the electric current and/or electricity Pressure judges that each motor whether there is failure;
Step S2:When it is faulty motor to judge a motor, closes another adjacent with the faulty motor and turn The opposite motor in dynamic direction;
Step S3:The serial number of the paddle i driven according to the faulty motor selects corresponding mapping matrix Ri, and root According to the mapping matrix RiInput attitude angle is converted into attitude angle in order to control, i is the natural number for being less than or equal to 6 more than or equal to 0;
Step S4:The control attitude angle is inputted into the flight control modules of six rotor wing unmanned aerial vehicle to control described six Rotor wing unmanned aerial vehicle flies.
Preferably, further include following steps:
Step S5:The flight attitude angle of six rotor wing unmanned aerial vehicle is acquired by sensor;
Step S6:According to the deviation between the control attitude angle and the flight attitude angle, calculating need to be to described six The target propulsive force that rotor wing unmanned aerial vehicle appliesWith target torque τr, and then according to the target propulsive forceWith target torque τrMeter Calculate the rotating speed of each motor.
Preferably, when the electric current of a motor and/or voltage exist such as under type, judge that there are failures for the motor:
The electric current is more than upper current limit threshold value;
The electric current is less than lower current limit threshold value;
The current change rate is more than current change rate threshold value;
The voltage is more than upper voltage limit threshold value;
The voltage is less than lower voltage limit threshold value;
The voltage changing rate is more than voltage changing rate threshold value.
Preferably, further include following steps before the step S1:
Step M1:Establish body coordinate system FB, FB={ OB, (xB,yB,zB), wherein OBFor the center of body, zB、xB、yB The respectively Z axis of six rotor wing unmanned aerial vehicles, X-axis, Y-axis;zBSix rotor wing unmanned aerial vehicles are indicated in equilibrium state, the side perpendicular to ground To xBIt indicates across the center of the body along the direction that the head of the body extends, yBIt indicates perpendicular to zBAnd xBSide To;
When motor i is horizontally mounted, initial motor coordinate system is established, the z in the initial motor coordinate systemCFor Z axis, table Show the axis direction of motor i and the direction perpendicular to ground, xCFor X-axis, indicate across the center of the motor i along the horn The direction of extension, yCFor Y-axis, indicate perpendicular to zCAnd xCForm the direction of plane;
Step M2:For each motor i, when motor i driving slurries wing i is rotated clockwise, by motor i around xCAxis is inverse Hour hands rotation alphaiAngle and around yCAxis is counterclockwise rotation βiAngle;When motor i driving slurries wing i is rotated counterclockwise, by the electricity Machine i is around xCAxis rotates clockwise αiAngle and around yCAxis is counterclockwise rotation βiAngle;In the present embodiment, the rotation counterclockwise It is located at the outside of six rotor wing unmanned aerial vehicle with the observation position rotated clockwise.
Step M3:For motor i, motor coordinate system after rotation is established Its In, i indicates that the serial number of motor, i are the natural number for being less than or equal to 6 more than 0,Indicate the center of motor i, For The Z axis of motor i, X-axis, Y-axis;For Z axis, the axis direction of motor i is indicated,For X-axis, indicate across the motor i's Center is in β with the direction that the horn extendsiThe direction of angle,For Y-axis, indicate perpendicular toWithDirection;
Step M4:It will be eachAxis and zBAngle between axis is indicated by tilting established angle α, β, due to the i-th paddle ZpiAxis and the axial direction of the rotary shaft of the wing are equidirectional, the z of the i-th paddlepiAxis and body coordinate system zBRotation relationship between axis For,
zpi=Ryi)Rxi)zB
Wherein, Ryi) indicate yBThe spin matrix in direction, Rxi) indicate xBThe spin moment in direction indicates as follows respectively:
Work as zBWhen=[0 0 1], it can obtain:
The third element of matrix;
Step M5:I-th paddle will produce along zpiThe thrust f in directioniAnd torque taui,It can be expressed as in coordinate system,
Wherein, kfi、kτiFor with the relevant constant coefficient of paddle physical characteristic, kfiFor the Relation Parameters of thrust and rotating speed, kτiFor The Relation Parameters of torque and rotating speed, ωiFor the rotating speed of each paddle ,+indicate the direction of rotation of the paddle be it is clockwise ,-indicate The direction of rotation of the paddle is counterclockwise.
Gross thrust caused by all propellers that can be rotated of six rotor wing unmanned aerial vehiclesWith total torque τcIt is sat on ground Mark systemUnder can be expressed as
Wherein, piIndicate originIn body coordinate system FBIn position, u=[ω11|,ω22|…,ω66 |], F1U is inputted in order to control to the transition matrix for the thrust for being applied to unmanned plane, F2U is inputted in order to control to turn for being applied to unmanned plane The transition matrix of square, n are the sum for the motor that can be rotated, and n is less than or equal to i, and R is earth axes and motor coordinate after rotation SystemBetween transition matrix, Indicate the reasonable matrix number of 3 × n;
Kinetic model according to newton Euler's formula, six rotor wing unmanned aerial vehicles with inclination angle is as follows,
Wherein, the quality of six rotor wing unmanned aerial vehicles is m, and g is acceleration of gravity, and ω is the attitude angle of six rotor wing unmanned aerial vehicles, For the angular speed of six rotor wing unmanned aerial vehicles, J is rotary inertia,For acceleration, e3For earth axes Z axis (0,0,1).
Preferably, the RiIncluding R0、R1、R2、R3、R4、R5And R6
When there is no electrical fault, using mapping matrix R0,
Six rotor wing unmanned aerial vehicle includes number one motor, No. second motor, third motor, No. four motor, the 5th Number motor and No. six motor;
When number one motor, No. second electrical fault, using mapping matrix R1、R2,
When third motor, No. four electrical fault, using mapping matrix R3、R4,
When No. five motor, No. six electrical fault, using mapping matrix R5、R6,
Preferably, according to the mapping matrix RiInput attitude angle is converted into attitude angle in order to control, specially:
[Φ θ ψ]control=Ri[Φ θ ψ]pilot
[Φ θ ψ]pilotTo input attitude angle, [Φ θ ψ]controlAttitude angle in order to control, Φ indicate that roll angle, θ indicate Pitch angle, ψ indicate yaw angle.
Preferably, the step S5 includes the following steps:
Step S501:Steady state solution is solved, specifically, when six rotor wing unmanned aerial vehicles meet following formula when hovering posture:
By F2U=0 is it is found that steady state solution u belongs to transition matrix F2Nuclear space, can determine the solution of u be reduced into one it is straight Line, according to RF1U=mge3It can determine that u is uniquely solved;
Step S502:Make total torque τcAs possible close to target torque τr, gross thrustAs possible close to target propulsive force
Power distribution is solved by following optimization aim,
And then can be obtained by merging simplification,
Wherein, c is scalar factor.
Preferably, the motor is connected with pedestal with angle, and the pedestal is fixed on six rotor wing unmanned aerial vehicle motors On horizontal frames.
Compared with prior art, the present invention has following advantageous effect:
Present invention when having inclined setting angle of each motor in six rotor wing unmanned aerial vehicles, can be according to the event of motor Hinder situation optimization power distribution, realize the faults-tolerant control of six rotor wing unmanned aerial vehicles, i.e., six motors are normal and a motor therefore Automatic fault tolerant control may be implemented when barrier.
Description of the drawings
Upon reading the detailed description of non-limiting embodiments with reference to the following drawings, other feature of the invention, Objects and advantages will become more apparent upon:
Fig. 1 is the module diagram of six rotor wing unmanned aerial vehicles in the present invention;
Fig. 2 is the step flow chart of the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicles in the present invention;
Fig. 3 is the integrated flow figure of the mapping algorithm of input attitude angle in the present invention;
Fig. 4 is the integrated flow figure of power distribution module in the present invention.
Specific implementation mode
With reference to specific embodiment, the present invention is described in detail.Following embodiment will be helpful to the technology of this field Personnel further understand the present invention, but the invention is not limited in any way.It should be pointed out that the ordinary skill of this field For personnel, without departing from the inventive concept of the premise, various modifications and improvements can be made.These belong to the present invention Protection domain.
In the present embodiment, the present invention be directed to six rotor wing unmanned aerial vehicle safeties defect and deficiency, change six rotors without The setting angle of man-machine motor realizes the faults-tolerant control of six rotor wing unmanned aerial vehicles according to power distribution is optimized the characteristics of motor.Wherein The position of six rotor wing unmanned aerial vehicles and gesture stability can be described as follows:Remote controler sends out pitching, roll, yaw angle and throttle life It enables, six rotor wing unmanned aerial vehicles receive the order of remote controler, can be real when six motors are normal and an electrical fault Existing automatic fault tolerant control.
In the present embodiment, it when carrying out fault diagnosis, is determined according to the flying quality and priori that monitor Current flight device state, and output result is sent to gesture commands mapping block and power distribution module.Gesture commands reflect Module is penetrated according to current flight device state and the control command of winged hand, automatic mapping at gesture stability control attitude angle.Appearance State control module is determined by calculating the control attitude angle after mapping and deviation between the collected flight attitude angle of sensor Need the thrust applied and torque.Power distribution module makes the measurement of torque and thrust by solving nonlinear optimal problem The deviation of value and setting value is minimum, reaches the automatic fault tolerant gesture stability of six rotor wing unmanned aerial vehicles.
The automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle provided by the invention, six rotor wing unmanned aerial vehicle it is each Motor has with respect to the horizontal plane tilts established angle α, inclination established angle β in another direction, including following step along a side Suddenly:
Step S1:The electric current and/or voltage for acquiring multiple motors on six rotor wing unmanned aerial vehicles, according to the electric current and/or electricity Pressure judges that each motor whether there is failure;
Step S2:When it is faulty motor to judge a motor, closes another adjacent with the faulty motor and turn The opposite motor in dynamic direction;
Step S3:The serial number of the paddle i driven according to the faulty motor selects corresponding mapping matrix Ri, and root According to the mapping matrix RiInput attitude angle is converted into attitude angle in order to control, i is the natural number for being less than or equal to 6 more than or equal to 0;
Step S4:The control attitude angle is inputted into the flight control modules of six rotor wing unmanned aerial vehicle to control described six Rotor wing unmanned aerial vehicle flies, and then the flight attitude angle of six rotor wing unmanned aerial vehicle is acquired by sensor;
Step S5:According to the deviation between the control attitude angle and the flight attitude angle, calculating need to be to described six The target propulsive force that rotor wing unmanned aerial vehicle appliesWith target torque τr, and then according to the target propulsive forceWith target torque τrMeter Calculate the rotating speed of each motor.
In the present embodiment, when the electric current of a motor and/or voltage exist such as under type, judge that the motor has event Barrier:
The electric current is more than upper current limit threshold value;
The electric current is less than lower current limit threshold value;
The current change rate is more than current change rate threshold value;
The voltage is more than upper voltage limit threshold value;
The voltage is less than lower voltage limit threshold value;
The voltage changing rate is more than voltage changing rate threshold value.
In the present embodiment, the upper current limit threshold value is 10A, and lower current limit threshold value is 0.5A, current change rate threshold Value is 3A/ms, and upper voltage limit threshold value is 12v, and lower voltage limit threshold value is 11v, voltage changing rate threshold value 2v/ms.
In the present embodiment, fault diagnosis module acquires the electric current and voltage of each motor, when the electric current and electricity of a motor When extruding existing as above abnormal bounce, judge the motor for faulty motor;The faulty motor is positioned, is closed and the failure immediately The corresponding cooperation motor of motor, specially i.e. No. 1 motor and No. 2 motors are to corresponding, and No. 3 motors and No. 4 motors are relative to 5 Number motor and No. 6 motors relative to;The serial number of paddle i is driven to select corresponding mapping matrix R according to the faulty motori, lead to Cross mapping matrix RiMatrix conversion will input attitude angle conversion attitude angle [Φ θ ψ] in order to controlcontrolInput gesture stability module. Gesture stability module calculates the deviation between the control attitude angle after mapping and sensor output flight attitude angle, and makes partially Difference is minimum, i.e., the mould of the difference for the flight torque needed for control moment and practical attitude stabilization that gesture stability module calculates adds The mould for the difference that the control thrust and aircraft altitude of upper gesture stability module input stablize required flight thrust is minimum.And then power The optimal thrust and torque that optimization module applies according to the target control attitude angle of gesture stability module, finally obtain motor most Excellent rotating speed.
Establish the kinetic model of six rotor wing unmanned aerial vehicles
In the present embodiment, it the present invention is based on aerodynamics and rotor flying theory of mechanics, establishes with inclination angle The dynamic model of six rotor wing unmanned aerial vehicles, the dynamic model for gesture stability equalization point, power distribution optimization algorithm and System controllability and measurability is analyzed.
Step M1:Establish body coordinate system FB, FB={ OB, (xB,yB,zB), wherein OBFor the center of body, zB、xB、yB The respectively Z axis of six rotor wing unmanned aerial vehicles, X-axis, Y-axis;zBSix rotor wing unmanned aerial vehicles are indicated in equilibrium state, the side perpendicular to ground To xBIt indicates across the center of the body along the direction that the head of the body extends, yBIt indicates perpendicular to zBAnd xBSide To;
When motor i is horizontally mounted, initial motor coordinate system is established, the z in the initial motor coordinate systemCFor Z axis, table Show the axis direction of motor i and the direction perpendicular to ground, xCFor X-axis, indicate across the center of the motor i along the horn The direction of extension, yCFor Y-axis, indicate perpendicular to zCAnd xCForm the direction of plane.
Step M2:For each motor i, when motor i driving slurries wing i is rotated clockwise, by motor i around xCAxis is inverse Hour hands rotation alphaiAngle and around yCAxis is counterclockwise rotation βiAngle;When motor i driving slurries wing i is rotated counterclockwise, by the electricity Machine i is around xCAxis rotates clockwise αiAngle and around yCAxis is counterclockwise rotation βiAngle;In the present embodiment, the rotation counterclockwise It is located at the outside of six rotor wing unmanned aerial vehicle with the observation position rotated clockwise.
Step M3:For motor i, motor coordinate system after rotation is established Its In, i indicates that the serial number of motor, i are the natural number for being less than or equal to 6 more than 0,Indicate the center of motor i, For The Z axis of motor i, X-axis, Y-axis.For Z axis, the axis direction of motor i is indicated,For X-axis, indicate in the motor i The heart is in β with the direction that the horn extendsiThe direction of angle,For Y-axis, indicate perpendicular toWithDirection.
Step M4:It will be eachAxis and zBAngle between axis is indicated by established angle α, β, due to the i-th paddle Axis and the axial direction of the rotary shaft of the wing are equidirectional, the z of the i-th paddlepiAxis and body coordinate system zBRotation relationship between axis is,
zpi=Ryi)Rxi)zB
Wherein, Ryi) indicate yBThe spin matrix in direction, Rxi) indicate xBThe spin moment in direction indicates as follows respectively:
Work as zBWhen=[0 0 1], it can obtain:
The third element of matrix;
Step S105:I-th paddle will produce along zpiThe thrust f in directioniAnd torque taui,It can be indicated in coordinate system For,
Wherein, kfi、kτiFor with the relevant constant coefficient of paddle physical characteristic, kfiFor the Relation Parameters of thrust and rotating speed, kτiFor The Relation Parameters of torque and rotating speed, ωiFor the rotating speed of each paddle ,+indicate the direction of rotation of the paddle be it is clockwise ,-indicate The direction of rotation of the paddle is counterclockwise.fiFor the numerical value of thrust, fiFor the vector of thrust, τiFor the numerical value of torque, τiFor torque Vector.
Gross thrust caused by all propellers that can be rotated of six rotor wing unmanned aerial vehiclesWith total torque τcIt is sat on ground Mark systemUnder can be expressed as
Wherein, piIndicate originIn body coordinate system FBIn position, u=[ω11|,ω22|…,ω66 |], F1U is inputted in order to control to the transition matrix for the thrust for being applied to unmanned plane, F2U is inputted in order to control to turn for being applied to unmanned plane The transition matrix of square, n are less than or equal to i, and R is earth axes and motor coordinate system after rotationBetween transition matrix, Indicate the reasonable matrix number of 3 × n;
Kinetic model according to newton Euler's formula, six rotor wing unmanned aerial vehicles with inclination angle is as follows,
Wherein, the quality of six rotor wing unmanned aerial vehicles is m, and g is acceleration of gravity, and ω is the attitude angle of six rotor wing unmanned aerial vehicles, For the angular speed of six rotor wing unmanned aerial vehicles, J is rotary inertia,For acceleration, e3For earth axes Z axis (0,0,1), R is ground Coordinate system and motor coordinate system after rotationBetween transition matrix.
Six rotor wing unmanned aerial vehicle model controllabilitys analysis with inclination angle
According to the dynamics model analysis of six rotor wing unmanned aerial vehicles it is found that the posture of six rotor wing unmanned aerial vehicles is mainly by attitude angle ω It indicates, and the posture of six rotor wing unmanned aerial vehicles is that the gross thrust of six rotor wing unmanned aerial vehicles is applied to by paddleWith total torque τcBy turning Change matrix F2It is adjusted.If the posture of six rotor wing unmanned aerial vehicles is fully controllable, transition matrix F is needed2Full rank.
For six rotor wing unmanned aerial vehicles that paddle is horizontally mounted, in the case where 6 propellers all work normally, transition matrix F2Full rank;When 1 motor breaks down, transition matrix F2Order be reduced to 2, therefore six rotor wing unmanned aerial vehicles posture it is endless Complete controllable, which is in-flight also verified in material object, and six rotor wing unmanned aerial vehicles occur suddenly in normal flight operations When bad paddle, six rotor wing unmanned aerial vehicles, which fly hand, to control six rotor wing unmanned aerial vehicles, final six rotor wing unmanned aerial vehicle by normal operating Due to posture and position can not normal control and crash.
And for six rotor wing unmanned aerial vehicles with mounted angle, the case where 6 propeller normal works and 1 are damaged Under, transition matrix F2Equal full rank, theoretically six rotor wing unmanned aerial vehicle postures are fully controllable.But six general rotor wing unmanned aerial vehicles fly control and calculate Method does not support the control for having mounted angle and bad paddle.Therefore need special control algolithm and framework realize six rotors without Man-machine faults-tolerant control.
Mapping matrix algorithm
For there are six rotor wing unmanned aerial vehicles of mounted angle, meet following formula when hovering posture,
By F2U=0 is it is found that u belongs to transition matrix F2Nuclear space, it may be determined that the solution of u is reduced into straight line, according to RF1U=mge3It can determine that u is uniquely solved.
When different paddles damage, different steady state solution u can be obtained1s,u2s,u3s,u4s,u5s,u6s, u1sIt is No. 1 Stable solution when motor-driven paddle damage, u2sStable solution when being damaged for No. 2 motor-driven paddles, u3sFor No. 3 motors Stable solution when the paddle damage of driving, u4sStable solution when being damaged for No. 4 motor-driven paddles, u5sIt is driven for No. 5 motors Paddle damage when stable solution, u6sStable solution when being damaged for No. 6 motor-driven paddles.When motor does not have failure Steady state solution be u0s.When posture of hovering under normal circumstances, the z of body coordinate systemBThe z of axis and earth axeswOverlapping of axles, this When mapping matrix R0For unit matrix.
When motor is there are when failure, according to the steady state solution of gained before it is found that the z of body coordinate systemBAxis and ground The z of coordinate systemwAxis is misaligned, there are certain angle, can solve to obtain the corresponding mapping matrix R of different bad paddles1,R2,…R6
[Φ θ ψ]control=Ri[Φ θ ψ]pilot
It is [Φ θ ψ] to fly hand by the input attitude angle that remote controler providespilot, the roll of Φ six rotor wing unmanned aerial vehicles of expression Angle, θ indicate that the pitch angle of six rotor wing unmanned aerial vehicles, ψ indicate the yaw angle of six rotor wing unmanned aerial vehicles.Pass through mapping matrix RiBe converted to appearance The control attitude angle [Φ θ ψ] of state control modulecontrol.When the case where fault diagnosis module detecting different bad paddles, switching To different mapping matrix R0,R1,R2,…R6
The parameter measurement of 1 six rotor wing unmanned aerial vehicle of table is as follows
The steady state solution that each pattern can be obtained is as follows:
2 six rotor wing unmanned aerial vehicle posture mapping table of table
5. power optimum allocation algorithm
Gesture stability algorithm is that conventional pid algorithm calculates required turn according to the error of targeted attitude and practical posture Square τr, one of target of power distribution is to make total torque τcAs possible close to target torque τr.Another algorithm of power distribution is to make Gross thrustCan balancing gravity, or meet and fly the given thrust setting value demand of hand, i.e. gross thrustAs possible close to mesh Mark thrust
Power distribution is solved by following optimization aim,
And then can be obtained by merging simplification,
Wherein, c is scalar factor, and u is steady state solution, and the value range of u is u0s, u1s,u2s,u3s,u4s,u5s,u6s.In failure Diagnostic module detects the case where different faults paddle, switches corresponding configuration parameter, is calculated by the optimization algorithm optimal Power output, carry out the adjusting of posture.
In the present embodiment, the motor is connected with pedestal with angle, and the pedestal is fixed on six rotor wing unmanned aerial vehicles On the horizontal frames of motor, established angle α, β are tilted to realize.
Can be several Module implementations citings below there are many realization method based on the content of present invention:
Code is integrated in the flying-controlled box PX4 that increases income
The integrated flow figure of the mapping algorithm of attitude angle is inputted as shown in figure 3, the mapping algorithm of the input attitude angle is integrated In position control loop, position control loop needs the setting value for receiving the remote controler input for flying hand to be set as corresponding target Definite value, the position control loop need to judge whether the motor of failure, directly using remote control in the case of no electrical fault The setting value of device needs dispatch map matrix just to be swashed as position and posture setting value in the case that motor breaks down It is living, it then finds the corresponding mapping matrix of corresponding faulty motor and calculates new remote controller position and input attitude angle.
The integrated of improved power distribution module can be there are two types of mode, as shown in figure 4, first way is the power point With needing to judge whether in module that electrical fault occurs, optimization algorithm is distributed using new power in case of a failure, Otherwise using the power allocation algorithm of the PX4 given tacit consent to originally.The second way is directly to change power allocation algorithm in PX4, Power allocation algorithm is added breakdown judge and whether activates the logic of corresponding power allocation algorithm in the PX4.Two kinds of embodiment party Formula can realize power allocation algorithm.
Gazebo is emulated
A, according to the gesture commands mapping block and mapping scheduler being added in first way PX4 for different electrical faults Corresponding power distribution optimization algorithm is added in module in the power distribution module of PX4.
B, corresponding model parameter is changed in some six rotor wing unmanned aerial vehicle in gazebo models, corresponding motor peace is set Fill angle.
C, it is emulated by stil, with joystick testing algorithm.
D, data are collected by ground station software and carries out parser tune ginseng.
Matlab is emulated
A, the dynamic model of six rotor wing unmanned aerial vehicles is established in matlab, is included the model of the balance of power and equalising torque, is added Enter the established angle of motor.
B, the controller of the corresponding unmanned planes of PX4 is established in matlab, includes the positioner of external loop, inner looping Gesture stability module and mixing device.
C, the gesture commands mapping block and mapping scheduler module for different actuator failures are established in matlab, Mixing device in corresponding power be added distribute optimization algorithm.
D, influence of the emulation inclination angle to control, analysis result.
Unmanned plane actual implementation scheme
A, the gesture commands mapping block and mapping scheduler module for different electrical faults are added in PX4, PX4's Corresponding power distribution optimization algorithm is added in mixing device.
B, motor established angle α, β that 3D printing motor printed design comes out
C, practical flight collects data by earth station and carries out algorithm undated parameter.
Specific embodiments of the present invention are described above.It is to be appreciated that the invention is not limited in above-mentioned Particular implementation, those skilled in the art can make various deformations or amendments within the scope of the claims, this not shadow Ring the substantive content of the present invention.

Claims (8)

1. each motor of a kind of automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicles, six rotor wing unmanned aerial vehicle has phase Established angle α, inclination established angle β in another direction, which is characterized in that include the following steps are tilted along a side for horizontal plane:
Step S1:The electric current and/or voltage for acquiring multiple motors on six rotor wing unmanned aerial vehicles, sentence according to the electric current and/or voltage Each motor that breaks whether there is failure;
Step S2:When it is faulty motor to judge a motor, another and rotation side adjacent with the faulty motor is closed To opposite motor;
Step S3:The serial number of the paddle i driven according to the faulty motor selects corresponding mapping matrix Ri, and according to institute State mapping matrix RiInput attitude angle is converted into attitude angle in order to control, i is the natural number for being less than or equal to 6 more than or equal to 0;
Step S4:The control attitude angle is inputted into the flight control modules of six rotor wing unmanned aerial vehicle to control six rotor Unmanned plane during flying.
2. the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle according to claim 1, which is characterized in that further include Following steps:
Step S5:The flight attitude angle of six rotor wing unmanned aerial vehicle is acquired by sensor;
Step S6:According to the deviation between the control attitude angle and the flight attitude angle, calculating need to be to six rotor The target propulsive force that unmanned plane appliesWith target torque τr, and then according to the target propulsive forceWith target torque τrIt calculates The rotating speed of each motor.
3. the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle according to claim 1, which is characterized in that when an electricity When the electric current and/or voltage of machine are existed such as under type, judge that there are failures for the motor:
The electric current is more than upper current limit threshold value;
The electric current is less than lower current limit threshold value;
The current change rate is more than current change rate threshold value;
The voltage is more than upper voltage limit threshold value;
The voltage is less than lower voltage limit threshold value;
The voltage changing rate is more than voltage changing rate threshold value.
4. the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle according to claim 1, which is characterized in that described Further include following steps before step S1:
Step M1:Establish body coordinate system FB, FB={ OB, (xB,yB,zB), wherein OBFor the center of body, zB、xB、yBRespectively For the Z axis of six rotor wing unmanned aerial vehicles, X-axis, Y-axis;zBSix rotor wing unmanned aerial vehicles are indicated in equilibrium state, the direction perpendicular to ground, xB It indicates across the center of the body along the direction that the head of the body extends, yBIt indicates perpendicular to zBAnd xBDirection;
When motor i is horizontally mounted, initial motor coordinate system is established, the z in the initial motor coordinate systemCFor Z axis, electricity is indicated The axis direction of machine i and direction perpendicular to ground, xCFor X-axis, expression extends across the center of the motor i along the horn Direction, yCFor Y-axis, indicate perpendicular to zCAnd xCForm the direction of plane;
Step M2:For each motor i, when motor i driving slurries wing i is rotated clockwise, by motor i around xCAxis is counterclockwise Rotation alphaiAngle and around yCAxis is counterclockwise rotation βiAngle;When motor i driving slurries wing i is rotated counterclockwise, by motor i Around xCAxis rotates clockwise αiAngle and around yCAxis is counterclockwise rotation βiAngle;In the present embodiment, the rotation counterclockwise and institute State the outside that the observation position rotated clockwise is located at six rotor wing unmanned aerial vehicle.
Step M3:For motor i, motor coordinate system after rotation is establishedWherein, i Indicate that the serial number of motor, i are the natural number for being less than or equal to 6 more than 0,Indicate the center of motor i, For motor The Z axis of i, X-axis, Y-axis;For Z axis, the axis direction of motor i is indicated,For X-axis, indicate across the center of the motor i It is in β with the direction that the horn extendsiThe direction of angle,For Y-axis, indicate perpendicular toWithDirection;
Step M4:It will be eachAxis and zBAngle between axis is indicated by tilting established angle α, β, due to the z of the i-th paddlepi Axis and the axial direction of the rotary shaft of the wing are equidirectional, the z of the i-th paddlepiAxis and body coordinate system zBRotation relationship between axis is,
zpi=Ryi)Rxi)zB
Wherein, Ryi) indicate yBThe spin matrix in direction, Rxi) indicate xBThe spin moment in direction indicates as follows respectively:
Work as zBWhen=[0 0 1], it can obtain:
The third element of matrix;
Step M5:I-th paddle will produce along zpiThe thrust f in directioniAnd torque taui,It can be expressed as in coordinate system,
Wherein, kfi、kτiFor with the relevant constant coefficient of paddle physical characteristic, kfiFor the Relation Parameters of thrust and rotating speed, kτiFor torque With the Relation Parameters of rotating speed, ωiFor the rotating speed of each paddle ,+indicate the direction of rotation of the paddle be it is clockwise ,-indicate the paddle The direction of rotation of the wing is counterclockwise.
Gross thrust caused by all propellers that can be rotated of six rotor wing unmanned aerial vehiclesWith total torque τcIn earth axesUnder can be expressed as
Wherein, piIndicate originIn body coordinate system FBIn position, u=[ω11|,ω22|…,ω66|], F1 U is inputted in order to control to the transition matrix for the thrust for being applied to unmanned plane, F2U is inputted in order to control to the torque for being applied to unmanned plane Transition matrix, n are the sum for the motor that can be rotated, and n is less than or equal to i, and R is earth axes and motor coordinate system after rotationBetween transition matrix, Indicate the reasonable matrix number of 3 × n;
Kinetic model according to newton Euler's formula, six rotor wing unmanned aerial vehicles with inclination angle is as follows,
Wherein, the quality of six rotor wing unmanned aerial vehicles is m, and g is acceleration of gravity, and ω is the attitude angle of six rotor wing unmanned aerial vehicles,For six rotations The angular speed of wing unmanned plane, J are rotary inertia,For acceleration, e3For earth axes Z axis (0,0,1).
5. the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle according to claim 1, which is characterized in that the Ri Including R0、R1、R2、R3、R4、R5And R6
When there is no electrical fault, using mapping matrix R0,
Six rotor wing unmanned aerial vehicle includes number one motor, No. second motor, third motor, No. four motor, No. five electricity Machine and No. six motor;
When number one motor, No. second electrical fault, using mapping matrix R1、R2,
When third motor, No. four electrical fault, using mapping matrix R3、R4,
When No. five motor, No. six electrical fault, using mapping matrix R5、R6,
6. the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle according to claim 1, which is characterized in that according to institute State mapping matrix RiInput attitude angle is converted into attitude angle in order to control, specially:
[Φ θ ψ]control=Ri[Φ θ ψ]pilot
[Φ θ ψ]pilotTo input attitude angle, [Φ θ ψ]controlAttitude angle in order to control, Φ indicate that roll angle, θ indicate pitching Angle, ψ indicate yaw angle.
7. the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle according to claim 1, which is characterized in that the step Rapid S5 includes the following steps:
Step S501:Steady state solution is solved, specifically, when six rotor wing unmanned aerial vehicles meet following formula when hovering posture:
By F2U=0 is it is found that steady state solution u belongs to transition matrix F2Nuclear space, can determine that the solution of u is reduced into straight line, root According to RF1U=mge3It can determine that u is uniquely solved;
Step S502:Make total torque τcAs possible close to target torque τr, gross thrustAs possible close to target propulsive force
Power distribution is solved by following optimization aim,
And then can be obtained by merging simplification,
Wherein, c is scalar factor.
8. the automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicle according to claim 1, which is characterized in that the electricity Machine is connected with pedestal with angle, and the pedestal is fixed on the horizontal frames of six rotor wing unmanned aerial vehicle motors.
CN201810460050.6A 2018-05-15 2018-05-15 Automatic fault-tolerant attitude control method for six-rotor unmanned aerial vehicle Active CN108614573B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810460050.6A CN108614573B (en) 2018-05-15 2018-05-15 Automatic fault-tolerant attitude control method for six-rotor unmanned aerial vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810460050.6A CN108614573B (en) 2018-05-15 2018-05-15 Automatic fault-tolerant attitude control method for six-rotor unmanned aerial vehicle

Publications (2)

Publication Number Publication Date
CN108614573A true CN108614573A (en) 2018-10-02
CN108614573B CN108614573B (en) 2021-08-20

Family

ID=63663342

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810460050.6A Active CN108614573B (en) 2018-05-15 2018-05-15 Automatic fault-tolerant attitude control method for six-rotor unmanned aerial vehicle

Country Status (1)

Country Link
CN (1) CN108614573B (en)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109263979A (en) * 2018-10-26 2019-01-25 珠海银通无人机科技有限公司 A kind of dual redundant power aerial vehicle
CN110888451A (en) * 2019-12-20 2020-03-17 国网山西省电力公司电力科学研究院 Fault-tolerant control method and system for multi-rotor unmanned aerial vehicle
CN112327896A (en) * 2020-10-29 2021-02-05 东北大学 Rotor fault-tolerant control method and device, computer storage medium and computer equipment
CN112373677A (en) * 2020-11-18 2021-02-19 三生万物(北京)人工智能技术有限公司 Power loss protection system and protection method for six-rotor unmanned aerial vehicle
CN114208019A (en) * 2019-08-02 2022-03-18 松下知识产权经营株式会社 Motor control device, moving body, motor control method, and program
CN115857309A (en) * 2023-02-27 2023-03-28 华东交通大学 Fault-tolerant control method and system for tiltable six-rotor unmanned aerial vehicle
CN116088557A (en) * 2023-03-31 2023-05-09 湖南大学 Full-drive six-rotor unmanned aerial vehicle pose control method and device
CN116190975A (en) * 2023-01-13 2023-05-30 中国科学院自动化研究所 Fault-tolerant pose distribution method and system for large-caliber radio telescope

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101907893A (en) * 2010-07-02 2010-12-08 北京航空航天大学 Aircraft component attitude adjusting assembly system based on parallel mechanism with six degrees of freedom and debugging method
CN102902277A (en) * 2012-10-19 2013-01-30 北京航空航天大学 Safe landing method of six-rotor aircraft during one motor shutdown
CN103135553A (en) * 2013-01-21 2013-06-05 南京航空航天大学 Four-rotor aircraft fault-tolerant control method
CN103365297A (en) * 2013-06-29 2013-10-23 天津大学 Optical flow-based four-rotor unmanned aerial vehicle flight control method
CN104216417A (en) * 2014-09-22 2014-12-17 北京航空航天大学 Dynamic control allocation method of stratospheric airship based on four-vector propeller
CN104699108A (en) * 2013-12-10 2015-06-10 中国航空工业第六一八研究所 Multi-rotor craft control allocation method
CN105473442A (en) * 2013-06-09 2016-04-06 瑞士苏黎世联邦理工学院 Controlled flight of multicopter experiencing failure affecting effector
CN106970642A (en) * 2017-04-20 2017-07-21 佛山科学技术学院 A kind of multi-rotor unmanned aerial vehicle state monitoring apparatus
CN107077142A (en) * 2016-10-25 2017-08-18 深圳市大疆创新科技有限公司 Multi-rotor aerocraft and its control method, control device and flight control system

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101907893A (en) * 2010-07-02 2010-12-08 北京航空航天大学 Aircraft component attitude adjusting assembly system based on parallel mechanism with six degrees of freedom and debugging method
CN102902277A (en) * 2012-10-19 2013-01-30 北京航空航天大学 Safe landing method of six-rotor aircraft during one motor shutdown
CN103135553A (en) * 2013-01-21 2013-06-05 南京航空航天大学 Four-rotor aircraft fault-tolerant control method
CN105473442A (en) * 2013-06-09 2016-04-06 瑞士苏黎世联邦理工学院 Controlled flight of multicopter experiencing failure affecting effector
CN103365297A (en) * 2013-06-29 2013-10-23 天津大学 Optical flow-based four-rotor unmanned aerial vehicle flight control method
CN104699108A (en) * 2013-12-10 2015-06-10 中国航空工业第六一八研究所 Multi-rotor craft control allocation method
CN104216417A (en) * 2014-09-22 2014-12-17 北京航空航天大学 Dynamic control allocation method of stratospheric airship based on four-vector propeller
CN107077142A (en) * 2016-10-25 2017-08-18 深圳市大疆创新科技有限公司 Multi-rotor aerocraft and its control method, control device and flight control system
CN106970642A (en) * 2017-04-20 2017-07-21 佛山科学技术学院 A kind of multi-rotor unmanned aerial vehicle state monitoring apparatus

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109263979A (en) * 2018-10-26 2019-01-25 珠海银通无人机科技有限公司 A kind of dual redundant power aerial vehicle
CN114208019A (en) * 2019-08-02 2022-03-18 松下知识产权经营株式会社 Motor control device, moving body, motor control method, and program
CN110888451A (en) * 2019-12-20 2020-03-17 国网山西省电力公司电力科学研究院 Fault-tolerant control method and system for multi-rotor unmanned aerial vehicle
CN110888451B (en) * 2019-12-20 2022-12-06 国网山西省电力公司电力科学研究院 Fault-tolerant control method and system for multi-rotor unmanned aerial vehicle
CN112327896A (en) * 2020-10-29 2021-02-05 东北大学 Rotor fault-tolerant control method and device, computer storage medium and computer equipment
CN112373677A (en) * 2020-11-18 2021-02-19 三生万物(北京)人工智能技术有限公司 Power loss protection system and protection method for six-rotor unmanned aerial vehicle
CN112373677B (en) * 2020-11-18 2021-06-22 三生万物(北京)人工智能技术有限公司 Power loss protection system and protection method for six-rotor unmanned aerial vehicle
CN116190975A (en) * 2023-01-13 2023-05-30 中国科学院自动化研究所 Fault-tolerant pose distribution method and system for large-caliber radio telescope
CN116190975B (en) * 2023-01-13 2023-10-27 中国科学院自动化研究所 Fault-tolerant pose distribution method and system for large-caliber radio telescope
CN115857309A (en) * 2023-02-27 2023-03-28 华东交通大学 Fault-tolerant control method and system for tiltable six-rotor unmanned aerial vehicle
CN116088557A (en) * 2023-03-31 2023-05-09 湖南大学 Full-drive six-rotor unmanned aerial vehicle pose control method and device

Also Published As

Publication number Publication date
CN108614573B (en) 2021-08-20

Similar Documents

Publication Publication Date Title
CN108614573A (en) The automatic fault tolerant attitude control method of six rotor wing unmanned aerial vehicles
Bapst et al. Design and implementation of an unmanned tail-sitter
CN104765312B (en) Implementation method for reconfigurable aircraft control system
CN110888451B (en) Fault-tolerant control method and system for multi-rotor unmanned aerial vehicle
CN106707754A (en) Switching system-based modeling and adaptive control method for cargo handling unmanned gyroplane
US11858376B2 (en) System for monitoring a battery system in-flight and a method for its use
Wang et al. An actuator fault detection and reconstruction scheme for hex-rotor unmanned aerial vehicle
CN108647442A (en) A kind of six rotor wing unmanned aerial vehicle Fault Estimation methods based on auxiliary output
CN106896738A (en) A kind of various dimensions quadrotor gesture stability Simulation Experimental Platform
Kou et al. Powered yaw control for distributed electric propulsion aircraft: A model predictive control approach
Wang et al. Hybrid modeling based double-granularity fault detection and diagnosis for quadrotor helicopter
CN112327896A (en) Rotor fault-tolerant control method and device, computer storage medium and computer equipment
CN109308064A (en) A kind of the failure tolerant control method and system of quadrotor drone
CN113467244B (en) Multi-rotor unmanned aerial vehicle fault-tolerant control method and device based on control matrix redistribution
CN115906295B (en) Unmanned aerial vehicle health monitoring method and device based on digital twinning
Wen et al. Design of a real-time UAV fault injection simulation system
CN108681251B (en) Method for determining motor inclination angle of six-rotor unmanned aerial vehicle
US20230204680A1 (en) Connector for charging an electric aircraft and a method of use for a connector for charging an electric aircraft
US11840351B2 (en) Aircraft for self-neutralizing flight
Alberti Aeroswitch project: Development of the flight simulator of a distributed electric propulsion demonstrator
CN117492383B (en) Unmanned aerial vehicle automatic test system and method based on semi-physical simulation
US11835946B2 (en) Systems and methods for redistributing electrical load in an electric aircraft
US12009686B2 (en) System and method for using unrecoverable energy in a battery cell
US11735944B1 (en) System and method for using unrecoverable energy in a battery cell
US20220033066A1 (en) Redundancy systems for small fly-by-wire vehicles

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant