CN113375670A - Method for resolving attitude by using quaternion based on three-axis gyroscope and accelerometer value - Google Patents

Method for resolving attitude by using quaternion based on three-axis gyroscope and accelerometer value Download PDF

Info

Publication number
CN113375670A
CN113375670A CN202110387598.4A CN202110387598A CN113375670A CN 113375670 A CN113375670 A CN 113375670A CN 202110387598 A CN202110387598 A CN 202110387598A CN 113375670 A CN113375670 A CN 113375670A
Authority
CN
China
Prior art keywords
quaternion
attitude
rotation
gyroscope
vector
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.)
Pending
Application number
CN202110387598.4A
Other languages
Chinese (zh)
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.)
State Grid Jiangsu Electric Power Co ltd Xinghua Power Supply Branch
State Grid Jiangsu Electric Power Co Ltd
Taizhou Power Supply Co of State Grid Jiangsu Electric Power Co Ltd
Original Assignee
State Grid Jiangsu Electric Power Co ltd Xinghua Power Supply Branch
State Grid Jiangsu Electric Power Co Ltd
Taizhou Power Supply Co of State Grid Jiangsu Electric Power 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 State Grid Jiangsu Electric Power Co ltd Xinghua Power Supply Branch, State Grid Jiangsu Electric Power Co Ltd, Taizhou Power Supply Co of State Grid Jiangsu Electric Power Co Ltd filed Critical State Grid Jiangsu Electric Power Co ltd Xinghua Power Supply Branch
Priority to CN202110387598.4A priority Critical patent/CN113375670A/en
Publication of CN113375670A publication Critical patent/CN113375670A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B11/00—Automatic controllers
    • G05B11/01—Automatic controllers electric
    • G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
    • G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P.I., P.I.D.
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
    • G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
    • G05D1/0825—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using mathematical models
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

Abstract

The invention belongs to the technical field of flight monitoring patrol application, and particularly discloses a method for resolving an attitude by using quaternion based on a three-axis gyroscope and an accelerometer. And 2, obtaining the Euler angle settlement attitude by applying the Euler angle differential equation. And 3, solving by a quaternion array method, wherein the rotation in the plane (x, y) can be expressed by a complex number. The method for resolving the attitude by using quaternion based on the three-axis gyroscope and the accelerometer value has the advantages that: the flight patrol attitude control system can realize stable, reliable and safe flight patrol attitude control of the aerial patrol machine, and improve the flight patrol efficiency.

Description

Method for resolving attitude by using quaternion based on three-axis gyroscope and accelerometer value
Technical Field
The invention belongs to the technical field of flight monitoring patrol application, and particularly relates to a method for resolving an attitude by using quaternions based on a three-axis gyroscope and an accelerometer value.
Background
With the high-speed development of social economy, production plants, university campuses and logistics centers have to build partial parks of building groups with huge floor areas even to reach the level of square kilometers due to production requirements.
The security of huge factory area is laid and is only leaned on fixed camera or other sensors to cooperate the manual work to patrol and examine and realize that all-weather high efficiency security protection is patrolled and examined and become more and more difficult, and the main reason that limits its efficiency and reliability lies in: the perception blind area that the in-process was laid owing to lay position and camera sensor performance and bring in security protection camera, sensor can't be avoided, on traditional operation mode, the scheme of adopting the manual work to patrol and examine usually remedies the perception blind area, but the manual work is patrolled and examined the most obvious operation characteristics in the operation of patrolling and examining in the garden of super large tracts of land and is just inefficiency and with high costs, patrols and examines the in-process at the border usually, and the time difference of repeatedly patrolling and examining is up to more than half an hour.
In summary, people will generally supplement the flight patrol by using an aerial monitoring component, such as an aerial monitoring patrol machine, and how to control the aerial monitoring patrol machine to perform stable, reliable and safe flight patrol is a current urgent solution.
Therefore, in view of the above problems, the present invention provides a method of resolving an attitude using quaternions based on a three-axis gyroscope and an accelerometer value.
Disclosure of Invention
The purpose of the invention is as follows: the invention aims to provide a method for resolving an attitude by using quaternions based on a three-axis gyroscope and an accelerometer value, which can realize stable, reliable and safe flight patrol attitude control of an aerial patrol machine and improve the flight patrol efficiency.
The technical scheme is as follows: the invention provides a method for resolving an attitude by using quaternion based on a three-axis gyroscope and an accelerometer value, which comprises the following steps of 1, describing a plane rotation by using an Euler angle, obtaining a coordinate system after setting the transformation relation of the coordinate system to rotate around an alpha angle, wherein the Z coordinate is unchanged because the inner projection is carried out by rotating around, namely,
rx2＝OA+OB+BC
＝ODcosa+BDsina+BFsina
＝rx1cosa+ry1sina
ry2＝DE-AD
＝DFcosa-ODsina
＝ry1cosa-ODsina
＝ry1cosa-rx1sina
rz2＝rz1(ii) a Conversion to matrix form is represented as:
Figure RE-GDA0003094857880000021
after finishing, the method is as follows:
Figure RE-GDA0003094857880000022
rotary array
Figure RE-GDA0003094857880000023
So from rotation to what can be written above is only rotation about one axis, if the euler angle rotation in three-dimensional space requires three turns,
Figure RE-GDA0003094857880000024
Figure RE-GDA0003094857880000025
Figure RE-GDA0003094857880000026
the above operation results in a direction cosine matrix representing the rotation. Step 2, obtaining the settlement attitude of the Euler angle by applying the Euler angle differential equation,
Figure RE-GDA0003094857880000027
the left side in the above formula is the euler angle updated this time, corresponding to roll, pitch, yaw of the aircraft, the right side is the angle measured in the previous period, the three angular velocities are the angle of rotation in the previous period by the three-axis gyroscope directly installed on the aircraft, the unit is radian, the T-gyroscope angular velocity at intervals is calculated, and therefore the current euler angle can be calculated by solving the differential equation. Step 3, solving by a quaternion method, wherein the rotation in the plane (x, y) can be expressed by complex numbers, and the rotation in the three dimensions can also be described by unit quaternion in the same way, firstly defining a quaternion,
Figure RE-GDA0003094857880000031
the vector is used for representing a rotating shaft in a three-dimensional space, w is a scalar and represents a rotating angle, and then the rotating shaft rotates by w degrees, so that a quaternion can represent a complete rotation, only a unit quaternion can represent the rotation, and a direction cosine matrix described by an Euler angle is described by a quaternion, and then:
Figure RE-GDA0003094857880000032
in the technical scheme, in the software calculation in the step 3, firstly, values (three-dimensional vectors) acquired by an accelerometer are converted into unit vectors, namely the vectors are in a mode, the transmitted parameters are values of gyroscopes x, y and z and values of accelerometers x, y and z, quaternions are converted into three elements in a third row in direction cosines, and vx, vy and vz are actually unit vectors of gravity converted by a body coordinate reference system of the euler angles (quaternions) at the last time; axyz is a gravity vector measured by an accelerometer on a coordinate reference system of the machine body, namely the actually measured gravity vector; axyz is a gravity vector obtained by measurement, vxyz is a gravity vector calculated by integrating the attitude of the gyroscope, the gravity vectors are all gravity vectors on a coordinate reference system of the machine body, an error vector between the gravity vectors is an error between the attitude of the gyroscope after integration and the attitude obtained by adding measurement, and the error between the vectors can be represented by a vector cross product (also called vector outer product and cross product), exyz is a cross product of two gravity vectors, the cross product vector is still positioned on a coordinate system of the machine body, the integral error of the gyroscope is also in the coordinate system of the machine body, the magnitude of the cross product is directly proportional to the integral error of the gyroscope, and the integral error of the gyroscope is just taken to correct the gyroscope.
Compared with the prior art, the method for resolving the attitude by using quaternion based on the three-axis gyroscope and the accelerometer value has the advantages that: the flight patrol attitude control system can realize stable, reliable and safe flight patrol attitude control of the aerial patrol machine, and improve the flight patrol efficiency.
Drawings
FIG. 1 is a schematic diagram of the method of the present invention for resolving attitude using quaternion based on three-axis gyroscopes and accelerometer values depicting rotation in Euler angles;
FIG. 2 is a general form of a PID algorithm of the method of the invention for resolving attitude using quaternions based on three-axis gyroscopes and accelerometer values;
FIG. 3 is a cascade PID processing logic of the method of the invention for resolving attitude using quaternions based on three-axis gyroscopes and accelerometer values;
FIG. 4 is a task control system logic for a method of resolving attitude using quaternions based on a three-axis gyroscope and accelerometer values of the present invention;
FIG. 5 is a flight mode decision logic for the method of the present invention for resolving attitude using quaternions based on a three-axis gyroscope and accelerometer values;
FIG. 6 is a height control logic for a quad-rotor model of the present invention based on a method of three-axis gyroscope and accelerometer values using quaternions to resolve attitude;
FIG. 7 is the position control submodule logic of the method of the present invention for resolving attitude using quaternions based on three-axis gyroscopes and accelerometer values.
Detailed Description
The invention is further elucidated with reference to the drawings and the embodiments.
Examples
The method for resolving the attitude based on the three-axis gyroscope and the accelerometer value by using the quaternion as shown in fig. 1 comprises the following steps of 1, describing a plane rotation by using an Euler angle, converting a coordinate system, and obtaining the coordinate system after the coordinate system rotates around an angle alpha, wherein the Z coordinate is unchanged because the inner projection is carried out by rotating around,
rx2＝OA+OB+BC
＝ODcosa+BDsina+BFsina
＝rx1cosa+ry1sina
ry2＝DE-AD
＝DFcosa-ODsina
＝ry1cosa-ODsina
＝ry1cosa-rx1sina
rz2＝rz1(ii) a Conversion into matrix formThe formula is shown as:
Figure RE-GDA0003094857880000051
after finishing, the method is as follows:
Figure RE-GDA0003094857880000052
rotary array
Figure RE-GDA0003094857880000053
So from rotation to what can be written above is only rotation about one axis, if the euler angle rotation in three-dimensional space requires three turns,
Figure RE-GDA0003094857880000054
Figure RE-GDA0003094857880000055
Figure RE-GDA0003094857880000056
the above operation results in a direction cosine matrix representing the rotation. Step 2, obtaining the settlement attitude of the Euler angle by applying the Euler angle differential equation,
Figure RE-GDA0003094857880000057
the left side in the above formula is the euler angle updated this time, corresponding to roll, pitch, yaw of the aircraft, the right side is the angle measured in the previous period, the three angular velocities are the angle of rotation in the previous period by the three-axis gyroscope directly installed on the aircraft, the unit is radian, the T-gyroscope angular velocity at intervals is calculated, and therefore the current euler angle can be calculated by solving the differential equation. Step 3, solving by a quaternion array method, wherein the rotation in the plane (x, y) can be expressed by a complex number, and the same principle is adoptedRotation in three dimensions can also be described in terms of unit quaternions, where a quaternion is first defined,
Figure RE-GDA0003094857880000058
the vector is used for representing a rotating shaft in a three-dimensional space, w is a scalar and represents a rotating angle, and then the rotating shaft rotates by w degrees, so that a quaternion can represent a complete rotation, only a unit quaternion can represent the rotation, and a direction cosine matrix described by an Euler angle is described by a quaternion, and then:
Figure RE-GDA0003094857880000061
further preferably, in the software calculation in step 3, first, the value (three-dimensional vector) acquired by the accelerometer is converted into a unit vector, that is, the vector is processed in a mode, the input parameters are the values of the gyroscope x, y and z and the values of the accelerometer x, y and z, the quaternion is converted into three elements in the third row in the direction cosine, and vx, vy and vz are actually the unit vector of the gravity converted by the body coordinate reference system of the euler angle (quaternion) at the last time; axyz is a gravity vector measured by an accelerometer on a coordinate reference system of the machine body, namely the actually measured gravity vector; axyz is a gravity vector obtained by measurement, vxyz is a gravity vector calculated by integrating the attitude of the gyroscope, the gravity vectors are all gravity vectors on a coordinate reference system of the machine body, an error vector between the gravity vectors is an error between the attitude of the gyroscope after integration and the attitude obtained by adding measurement, and the error between the vectors can be represented by a vector cross product (also called vector outer product and cross product), exyz is a cross product of two gravity vectors, the cross product vector is still positioned on a coordinate system of the machine body, the integral error of the gyroscope is also in the coordinate system of the machine body, the magnitude of the cross product is directly proportional to the integral error of the gyroscope, and the integral error of the gyroscope is just taken to correct the gyroscope.
The PID and the derivative algorithm thereof are one of the most extensive algorithms in modern industrial application, the general form of the PID algorithm is that the controlled quantity is controlled by an error signal as shown in figure 2, and the controller is the summation of three links of proportion, integral and differential. Here we specify (at time t):
1. the input quantity is rin (t);
2. output is rout (t);
3. the offset is err (t) rin (t) -rout (t);
the control law of pid is:
Figure RE-GDA0003094857880000062
the PID is actually a control process for the deviation, and the proportional link does not work when the deviation is 0, and the proportional link only works when the deviation exists. The integration link is mainly used for eliminating static error, the static error is a difference value between an output value and a set value after the system is stabilized, the integration link is actually a deviation accumulation process, accumulated errors are added to the original system so as to counteract the static error caused by the system, the change rule of a difference signal is a change trend, advanced adjustment is carried out according to the change trend of a deviation signal, and the rapidity of the system is further improved;
and then discretizing the PID contact system, thereby facilitating implementation on a processor,
Figure RE-GDA0003094857880000071
assuming that the time sampling interval is T, at time K, T:
deviation err (k) rin (k) -rout (k);
the integral loop is represented in the form of a sum, i.e.:
err(K)+err(K+1)+........；
the differential ring segment is expressed in the form of a slope, i.e.:
[err(K)-err(K-1)]/T；
thus forming a discrete representation of the following PID:
Figure RE-GDA0003094857880000072
then u (k) can be expressed as:
u(k)＝Kp(err(k-1)+Ki∑err(j)+Kd(err(k)-err(k-1)))；
the above is position type PID, and the actual flight control system usually adopts incremental PID, which can be obtained from u (k):
Δu(k)＝kp(err(k)-err(k-1))+ki(err(k)-2err(k-1)+err(k-2))；
the above is the incremental expression of the discretized PID, and it can be seen from the formula that the incremental expression result is related to the deviation of the last three times, thus greatly improving the stability of the system (it should be noted that the final output result should be u (k)) + incremental adjustment value.
The cascade PID and the cascade PID are adjusted in parallel by an inner ring and an outer ring, so that the stability of the system is improved, the interference is avoided, the system is adjusted slowly and excessively, the outer ring is the error per se, the inner ring is the speed, if the position of the outer ring is controlled, the position of the inner ring is controlled, and the speed of the inner ring is controlled because the position is changed by integrating the speeds in three directions. In the same attitude control, the outer ring is the angle difference, the inner ring is the acceleration, because the realization of the angle is realized by the angular velocity transition, the transition process is the process, and in practice, if the response is fast, the inner ring can be directly controlled, or the attitude can be directly controlled.
The cascade PID two PID control algorithms are just concatenated (more precisely, are nested), which enhances the anti-interference performance (i.e. enhances the stability) of the system, because two controllers control the aircraft, which can control more variables than a single controller, so that the adaptability of the aircraft is stronger, and the logical structure is shown in FIG. 3.
An inner ring P: from small to large, the four shafts are more and more difficult to pull, and the four shafts are more and more felt to resist the pulling of the user; when a larger numerical value is reached, the four shafts vibrate at high frequency and can be seen by naked eyes, and when the four shafts are pulled, the four shafts can rapidly oscillate for several times and are stable after several seconds; continuously increasing without adding man-made interference, and diverging and turning over the machine by the self; only when the inner ring P exists, the four shafts slowly fall down in one direction, and the normal phenomenon is the angular speed static difference of the system.
An inner ring I: it can be seen from the PID principle that the integral is only used to eliminate the static difference, and therefore the integral term coefficients are not perceived as being large, since doing so reduces the system stability. From small to large, the four-axis can be fixed in a position and not move, and does not fall down any more, and the value of I is continuously increased, and the four-axis can be unstable and can disperse by oneself after being pulled.
An inner ring D: the differential term D here is a differential term under the standard PID principle, i.e., this error — the previous error. The differential in the angular velocity ring is the angular acceleration, the vibration of the four axes is strong originally, the change of the gyro value is large, noise is more easily introduced by differentiating the vibration, and therefore sliding filtering or IIR filtering can be properly performed in general. From small to large, the performance of the airplane is not changed much, and the airplane is more stable only when going back to the center; when the value of D is continuously increased, the high-frequency vibration (or the sound generated by the motor) of the four shafts at the balance position can be seen visually, and the D term is an auxiliary term as already explained, so that the D term can be ignored and not added if the vibration of the machine frame is large.
Outer ring P: after the PID of the inner ring is completely set, the airplane can be stabilized at a certain position and does not move, the inner ring P can obviously return slowly from the inclined position from small to large, the airplane can be pulled by hands and then released, and the airplane can return slowly to the middle to reach the balance position; the value of P is continuously increased, the remote controller is used for giving different angles, and the tracking speed and response of the airplane can be seen to be faster and faster; continuing to increase the value of P, the aircraft becomes very sensitive, with increasingly greater maneuverability and a tendency to diverge.
In the lower computer program design and the lower computer software design, the unmanned aerial vehicle and the unmanned vehicle both run in the Nuttx real-time operating system environment, and the basic logic architecture is shown in fig. 4.
The flight mode sub-module is mainly used for identifying a task mode which should be executed by the current aircraft, wherein an input control command is considered as a leading factor, and the ideas of sensor failure protection, hardware fault protection, model structure protection and fault tolerance are surrounded, so that once software and hardware conditions seriously unfavorable for flight occur, the controller forcibly converts the mode into full-manual operation to improve the safety; the specific decision logic is shown in fig. 5.
The attitude control submodule (typical software model of attitude control) includes traditional concepts of pitch, roll, yaw angle and altitude, wherein the first three categories relate to a control algorithm with a single attitude sensor in the IMU as the core, and the logical concept is shown in fig. 6.
The position control sub-module and the position control function determine the accuracy and reliability of the unmanned vehicle autonomous execution task to a great extent, in the position control sub-program of the lower computer navigation control module, a composite positioning technology of inertial navigation + GNSS + optical flow sensors is adopted to provide positioning support for the unmanned vehicle, the inertial navigation and the optical flow provide dynamic reference for the unmanned vehicle system in the control, the position control sub-module and the position control sub-program have the characteristics of high speed and high instantaneous precision, but the defects are obvious accumulated errors caused by an inertial navigation model along with the accumulation of time, so that the GNSS is used for providing 10HZ absolute position information for the positioning service of the navigation module, on one hand, the GNSS is used for executing the navigation program and correcting the errors of the inertial navigation system, and the basic logic is shown in FIG. 7.
Designing an upper computer, and aiming at the actual application scene of security inspection, carrying out secondary development on the basis of the development source items PX4 and 3DR mission planer, wherein the interactive method aims to change the characteristics of the complexity and professional degree of the traditional upper computer into a graphical interface and close most of data used for debugging and development; a user can directly interface with hardware such as a serial port data link, a high-definition digital image transmission and analog image transmission acquisition card on the basis of the software to realize corresponding functions of remote measurement and real-time image viewing.
(1) The telemetering gesture comprehensive display module: the flight attitude, the height, the speed, the mode, the attitude swing amplitude, the remote measurement RSSI, the GPS time and the comprehensive display window of the voltage and the current are read through a serial port, and the system can be prompted to warn in an error reporting mode; (2) the map interface is used for visually monitoring the real-time position of the carrier, a task route and partial GPS state information (effective star number searching and positioning accuracy factor HDOP); (3) and the operating console is used for sending a task command to the task controller of the remote unmanned vehicle through serial port wireless communication or a serial port USB module, and after the operating console is connected with equipment through a USB or a serial port data link, the operating console can normally execute a remote measuring function and simultaneously carry out task planning and command sending.
The foregoing is only a preferred embodiment of this invention and it should be noted that modifications can be made by those skilled in the art without departing from the principle of the invention and these modifications should also be considered as the protection scope of the invention.

Claims (2)

1. The method for resolving the attitude by using quaternion based on the three-axis gyroscope and the accelerometer value is characterized in that: comprises the following steps of (a) carrying out,
step 1, describing a transformation relation of a plane rotation and a coordinate system by using an Euler angle, setting a rotation angle alpha of the coordinate system around to obtain the coordinate system, wherein the Z coordinate is unchanged due to the rotation of the inner projection,
Figure DEST_PATH_IMAGE002
；
conversion to matrix form is represented as:
Figure DEST_PATH_IMAGE004
；
after finishing, the method is as follows:
Figure DEST_PATH_IMAGE006
Figure DEST_PATH_IMAGE008
；
rotary array
Figure DEST_PATH_IMAGE010
；
So from rotation to what can be written above is only rotation about one axis, if the euler angle rotation in three-dimensional space requires three turns,
Figure DEST_PATH_IMAGE012
；
Figure DEST_PATH_IMAGE014
；
the operation obtains a direction cosine matrix representing rotation;
step 2, obtaining the settlement attitude of the Euler angle by applying the Euler angle differential equation,
Figure DEST_PATH_IMAGE016
；
the left side in the above formula is the Euler angle updated this time, corresponding to roll, pitch and yaw of the aircraft, the right side is the angle measured in the previous period, the three angular velocities are the angles of the three-axis gyroscope directly mounted on the aircraft and rotated in the previous period, the unit is radian, and the T gyroscope angular velocity at intervals is calculated, so that the differential equation can be solved by solving the differential equation
Extracting a current Euler angle;
step 3, solving by a quaternion method, wherein the rotation in the plane (x, y) can be expressed by complex numbers, and the rotation in the three dimensions can also be described by unit quaternion in the same way, firstly defining a quaternion,
Figure DEST_PATH_IMAGE018
；
the vector is used for representing a rotating shaft in a three-dimensional space, w is a scalar and represents a rotating angle, and then the rotating shaft rotates by w degrees, so that a quaternion can represent a complete rotation, only a unit quaternion can represent the rotation, and a direction cosine matrix described by an Euler angle is described by a quaternion, and then:
Figure DEST_PATH_IMAGE020
。
2. the method for resolving attitude using quaternion based on three-axis gyroscope and accelerometer values of claim 1, wherein: in the software calculation in the step 3, firstly, values (three-dimensional vectors) acquired by the accelerometer are converted into unit vectors, namely the vectors are processed in a mode, the transmitted parameters are values of gyroscopes x, y and z and values of accelerometers x, y and z, and the quaternion is converted into three elements in the third row in the direction cosine, wherein vx, vy and vz are actually unit vectors of the gravity converted by the body coordinate reference system of the euler angle (quaternion) in the last time; axyz is a gravity vector measured by an accelerometer on a coordinate reference system of the machine body, namely the actually measured gravity vector; axyz is a gravity vector obtained by measurement, vxyz is a gravity vector calculated by integrating the attitude of the gyroscope, the gravity vectors are all gravity vectors on a coordinate reference system of the machine body, an error vector between the gravity vectors is an error between the attitude of the gyroscope after integration and the attitude obtained by adding measurement, and the error between the vectors can be represented by a vector cross product (also called vector outer product and cross product), exyz is a cross product of two gravity vectors, the cross product vector is still positioned on a coordinate system of the machine body, the integral error of the gyroscope is also in the coordinate system of the machine body, the magnitude of the cross product is directly proportional to the integral error of the gyroscope, and the integral error of the gyroscope is just taken to correct the gyroscope.
CN202110387598.4A 2021-04-12 2021-04-12 Method for resolving attitude by using quaternion based on three-axis gyroscope and accelerometer value Pending CN113375670A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110387598.4A CN113375670A (en) 2021-04-12 2021-04-12 Method for resolving attitude by using quaternion based on three-axis gyroscope and accelerometer value

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110387598.4A CN113375670A (en) 2021-04-12 2021-04-12 Method for resolving attitude by using quaternion based on three-axis gyroscope and accelerometer value

Publications (1)

Publication Number Publication Date
CN113375670A true CN113375670A (en) 2021-09-10

Family

ID=77570023

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110387598.4A Pending CN113375670A (en) 2021-04-12 2021-04-12 Method for resolving attitude by using quaternion based on three-axis gyroscope and accelerometer value

Country Status (1)

Country Link
CN (1) CN113375670A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115406405A (en) * 2022-04-15 2022-11-29 汉博来自控科技(上海)有限公司 Solution for level sensor of forklift truck
CN115487478A (en) * 2022-09-28 2022-12-20 西安交通大学 Intelligent basketball training method and system

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105739513A (en) * 2016-02-05 2016-07-06 北京航空航天大学 Quadrotor flying robot non-linear trajectory tracking controller and tracking control method thereof
CN107368087A (en) * 2016-05-13 2017-11-21 威海明达创新科技有限公司 Miniature four-axle aircraft and its control method
CN110413006A (en) * 2019-08-30 2019-11-05 杭州电子科技大学 A kind of unmanned plane formation group network system and method based on 5G communication

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105739513A (en) * 2016-02-05 2016-07-06 北京航空航天大学 Quadrotor flying robot non-linear trajectory tracking controller and tracking control method thereof
CN107368087A (en) * 2016-05-13 2017-11-21 威海明达创新科技有限公司 Miniature four-axle aircraft and its control method
CN110413006A (en) * 2019-08-30 2019-11-05 杭州电子科技大学 A kind of unmanned plane formation group network system and method based on 5G communication

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
刘震等: "基于MEMS传感器的高精度姿态角测量研究", 《中国测试》 *
吴东旭: ""共轴双旋翼飞行器姿态控制研究"", 《中国优秀博硕士学位论文全文数据库(硕士) 工程科技Ⅱ辑》 *
王慧: "《计算机控制系统》", 31 December 2000 *
黎明安: "《动力学控制基础与应用》", 31 December 2013 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115406405A (en) * 2022-04-15 2022-11-29 汉博来自控科技(上海)有限公司 Solution for level sensor of forklift truck
CN115487478A (en) * 2022-09-28 2022-12-20 西安交通大学 Intelligent basketball training method and system

Similar Documents

Publication Publication Date Title
CN104932512B (en) A kind of four rotor posture control methods based on MIMO nonlinear uncertain Backsteppings
US20210141395A1 (en) Stable flight control method for multi-rotor unmanned aerial vehicle based on finite-time neurodynamics
Jun et al. State estimation of an autonomous helicopter using Kalman filtering
Sa et al. Build your own visual-inertial drone: A cost-effective and open-source autonomous drone
CN110018691B (en) Flight state estimation system and method for small multi-rotor unmanned aerial vehicle
Cao et al. Image dynamics-based visual servoing for quadrotors tracking a target with a nonlinear trajectory observer
CN113375670A (en) Method for resolving attitude by using quaternion based on three-axis gyroscope and accelerometer value
Wang et al. High-order nonlinear differentiator and application to aircraft control
CN106774374B (en) Automatic unmanned aerial vehicle inspection method and system
CN107247459B (en) Anti-interference flight control method and device
CN101417711B (en) Disturbance compensation mechanism of two axis balance annular shelf
Sa et al. Dynamic System Identification, and Control for a cost effective open-source VTOL MAV
Kritskiy et al. Increasing the reliability of drones due to the use of quaternions in motion
CN109521785A (en) It is a kind of to clap Smart Rotor aerocraft system with oneself
CN100587644C (en) Integrated single loop controller for camera optical axis stable tracing
El Hadri et al. Attitude estimation with gyros-bias compensation using low-cost sensors
Haotian et al. Accurate attitude estimation of HB2 standard model based on QNCF in hypersonic wind tunnel test
Wang et al. Nonlinear position control approaches for quadcopters using a novel state representation
Yang et al. Model-free integrated navigation of small fixed-wing UAVs full state estimation in wind disturbance
Wang et al. Attitude estimation for UAV with low-cost IMU/ADS based on adaptive-gain complementary filter
Rademeyer Vision-based flight control for a quadrotor UAV
Shang et al. Low-cost quadrotor attitude solution based on improved complementary filtering
Zou et al. Visual track system applied in quadrotor aerial robot
Jun et al. State estimation via sensor modeling for helicopter control using an indirect kalman filter
Cengiz et al. ARCHI-Pilot: A Model-Based Autopilot Software Implementation using FreeRTOS with Automatic Code Generation

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210910