CN111966954B - Large-attitude quaternion full-angle domain Euler angle conversion method - Google Patents
Large-attitude quaternion full-angle domain Euler angle conversion method Download PDFInfo
- Publication number
- CN111966954B CN111966954B CN202010832716.3A CN202010832716A CN111966954B CN 111966954 B CN111966954 B CN 111966954B CN 202010832716 A CN202010832716 A CN 202010832716A CN 111966954 B CN111966954 B CN 111966954B
- Authority
- CN
- China
- Prior art keywords
- angle
- quaternion
- euler
- attitude
- interval
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 106
- 238000006243 chemical reaction Methods 0.000 title claims abstract description 55
- 239000011159 matrix material Substances 0.000 claims abstract description 33
- 230000036544 posture Effects 0.000 claims abstract description 23
- 238000005096 rolling process Methods 0.000 claims abstract description 18
- 238000004364 calculation method Methods 0.000 claims description 19
- 230000009191 jumping Effects 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000001427 coherent effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 2
- 238000006467 substitution reaction Methods 0.000 claims 1
- 230000008569 process Effects 0.000 abstract description 6
- 230000007547 defect Effects 0.000 description 4
- 206010034719 Personality change Diseases 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- WKBPZYKAUNRMKP-UHFFFAOYSA-N 1-[2-(2,4-dichlorophenyl)pentyl]1,2,4-triazole Chemical compound C=1C=C(Cl)C=C(Cl)C=1C(CCC)CN1C=NC=N1 WKBPZYKAUNRMKP-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000011031 topaz Substances 0.000 description 1
- 229910052853 topaz Inorganic materials 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a large-attitude quaternion full-angle domain Euler angle conversion method, and belongs to the field of attitude capture and control. The invention divides the postures into three types according to different intervals of the pitch angle, and respectively provides a method for converting quaternion in each interval into Euler angles and a method for selecting or rejecting the Euler angles, thereby realizing conversion in a full angle domain. The problem that the yaw angle and the rolling angle are singular when the pitch angle is in a deadlock interval is solved through a cross iteration method; the problem of wrong selection of conversion results when the pitch angle spans +/-90 degrees is solved through a sine sign method; solving the multi-value accepting and rejecting problem of quaternion conversion Euler angles under the full-angle domain; the attitude is analyzed and estimated through the internal information of the rotation matrix, and the accuracy of the converted numerical value is improved. The invention can process the quaternion data of the large posture on the controller with the weak floating point number operation capability, and realizes the full-angle domain conversion of the quaternion of the large posture into the Euler angle. The invention is applied to the fields of unmanned aerial vehicle large-attitude obstacle avoidance, robot pose acquisition, human body joint attitude capture and the like.
Description
Technical Field
The invention relates to a large-attitude quaternion full-angle domain Euler angle conversion method, in particular to a quaternion full-angle domain Euler angle conversion method for large-range attitude acquisition equipment, and belongs to the field of attitude capture and control.
Background
The attitude calculation of the carrier is widely applied to the hot fields of aerospace, industrial robots, human body attitude identification and the like. The attitude calculation is to calculate the attitude data of the carrier in real time by using the filtered data of a gyroscope, an accelerometer and the like through methods such as calculation fusion and the like. Common attitude description parameters are directional cosine matrix, euler angle, quaternion, rotation vector, rodgerge parameters, etc. Quaternions and Euler angles in a plurality of attitude description parameters are the two most commonly used methods in current engineering practice, and compared with the quaternions and the Euler angles, the attitude representation by the Euler angles is more visual and clear, and the geometric significance is more obvious; and the quaternion has the advantages of simple operation, no singularity, full-range attitude representation and the like. In order to intuitively understand the attitude information, euler angles are usually selected as attitude representation methods, namely pitch angles, yaw angles and rolling angles are used for representing the spatial attitude of the carrier. When the attitude of the aircraft is resolved, the pitch angle of the aircraft is usually only changed within a small range, but when some special scenes with large attitude are encountered, such as large-attitude obstacle avoidance, human joint attitude capture and other scenes, only the condition that the pitch angle is within a small range can not be considered, and the attitude change condition under the full angle needs to be considered. At this time, the euler angle is selected for representation, the inherent 'deadlock' defect of the euler angle occurs, and the quaternion does not have the problem, so that how to obtain accurate euler angle attitude information by the quaternion through a certain conversion method becomes a technical problem to be solved at present. In order to solve the problem of the full angle between the quaternion and the euler angle, in a patent library and a document library, a patent "an acquisition method for converting the quaternion full angle domain into the euler angle facing large-attitude maneuver" and a document "a new conversion algorithm for converting the quaternion and the euler angle in the full angle" provide solutions, but the two technologies have the following defects.
When the attitude of the aircraft is resolved, the pitch angle of the aircraft is usually only changed within a small range, but when some special scenes with large attitude are encountered, such as large-attitude obstacle avoidance, human joint attitude capture and other scenes, only the condition that the pitch angle is within a small range can not be considered, and the attitude change condition under the full angle needs to be considered. At this time, the euler angle is selected for representation, the inherent 'deadlock' defect of the euler angle occurs, the quaternion does not have the problem, and the conversion between the accurate quaternion and the visual euler angle becomes the technical problem to be solved in different application scenes. Currently, the euler angles can be converted into quaternions through hypercomplex mapping, and the process is one-to-one, so that quaternions can be obtained easily through the euler angles. However, a quaternion usually corresponds to one or two euler angles in a large attitude range, which is the difficulty of converting quaternions into euler angles. In order to solve the problem, a quadrant-based quaternion euler angle conversion method is proposed in a new full-angle quaternion and euler angle conversion algorithm written in Zhang Sai, cao Xibin and Zhouyan, but the 'deadlock' problem of the euler angles is not considered in the document, so that conversion errors can occur in partial angle intervals, and the algorithm proposed in the document is difficult to complete conversion tasks in engineering practice. In addition, a method for estimating the rotation direction of the Euler angle through quaternion is indirectly provided for an aircraft in a 'method for acquiring the quaternion full-angle domain conversion Euler angle for large-attitude maneuver' of topaz and Wulihua, however, the method only adopts a unified algorithm to optimize the defect of quadrant division, and does not solve the problems of singular values and multi-value values of the quaternion conversion Euler angle in a deadlock interval.
Disclosure of Invention
The invention aims to provide a large-attitude quaternion full-angle domain Euler angle conversion method, which can solve the following technical problems: (1) Solving the problem of singular yaw angle and rolling angle when the pitch angle is in a deadlock interval by a cross iteration method; (2) The problem of wrong selection of conversion results when the pitch angle spans +/-90 degrees is solved through a sine sign method; (3) Solving the multi-value accepting and rejecting problem of quaternion conversion Euler angles under the full-angle domain; (4) And the attitude is analyzed and estimated through the internal information of the rotation matrix, so that the accuracy of the converted numerical value is obviously improved. The invention can process the quaternion data of the large attitude on the controller with insufficient floating point number operation capability, and realizes the full-angle domain conversion of the quaternion of the large attitude into the Euler angle. The invention has the application fields of unmanned aerial vehicle large-attitude obstacle avoidance, robot pose acquisition, human body joint attitude capture and the like.
The purpose of the invention is realized by the following technical scheme.
The invention relates to a method for converting large-attitude quaternion into Euler angles in a full-angle domain, which divides attitudes into three categories according to different intervals of pitch angles, and respectively provides a method for converting quaternion into Euler angles in each interval and a method for selecting or rejecting the quaternion to realize conversion in the full-angle domain. The first part is a single-value interval (-90 degrees + epsilon, + 90-epsilon) corresponding to the quaternion pitch angle conversion, the multi-value conditions of the rolling angle and the yaw angle are compensated and calculated in the first part, the rolling angle and the yaw angle which cannot be directly solved by a single-value function are obtained, and the problem of quaternion Euler angles in the interval in the first part is solved. The second part is corresponding multivalued intervals (-180 degrees, -90 degrees-epsilon) and (90 degrees + epsilon, +180 degrees) when quaternion conversion pitch angle, and the multivalued conditions of the pitch angle, the roll angle and the yaw angle are compensated and calculated in the second part, so that the pitch angle, the roll angle and the yaw angle which can not be directly solved by a single-value function are obtained. The third part is dead zone intervals (-90-epsilon, -90-epsilon + epsilon) and (+ 90-epsilon, + 90-epsilon + epsilon) with the pitch angle in the front and back neighborhoods of the dead lock angle, and the cross iteration method is used for solving the problem that the yaw angle and the roll angle are singular when the pitch angle is in the dead lock interval. After the conversion is finished, selecting a correct attitude angle for the multi-valued attitude calculated by the first part and the second part by adopting an absolute value sum of squares method, and selecting a correct attitude angle for the multi-valued attitude calculated by the third part by adopting a sine sign method. According to the invention, the attitude conversion result under the full angular domain is coherent and accurate by optimizing the conversion result through the information contained in the rotating matrix, and a reliable data basis is provided for attitude analysis control.
The invention discloses a method for converting a large-attitude quaternion full-angle domain into an Euler angle, which comprises the following steps of:
the method comprises the following steps: the attitude is divided into three parts according to different intervals of the pitch angle theta: the first part is an interval (-90 degrees + epsilon, +90 degrees-epsilon) corresponding to a single value when the quaternion converts the pitch angle; the second part is interval (-180 degrees, -90 degrees, -epsilon) and (90 degrees + epsilon, +180 degrees) corresponding to multiple values when quaternion conversion pitch angle is carried out, and the third part is interval of dead zone (-90 degrees, -90 degrees + epsilon) and (+ 90 degrees-epsilon, +90 degrees + epsilon) of pitch angle in the front and rear neighborhoods of 'dead lock angle', wherein epsilon is a positive small value, and epsilon is determined according to the operation precision.
Step two: and performing compensation operation on the multi-value conditions of the roll angle and the yaw angle in the first partial interval to obtain the roll angle and the yaw angle of which a single-value function cannot be directly solved, so that the problem of quaternion-to-Euler angle conversion in the partial interval is solved.
In the first partial interval, since arcsin is a single-valued function in the interval, the rotation matrix C under the Z-Y-X coordinate system is used 0 Directly obtaining the pitch angle theta of the current moment 0 ：
θ 0 ＝-arsin(C 13 )
Wherein C 13 Is a matrix C 0 The first row and the third column are correspondingly positioned.
θ 0 ,ψ 0 Roll angle, pitch angle and yaw angle, q 0 ,q 1 ,q 2 ,q 3 One real unit and three imaginary units of quaternions, respectively.
The relation between the rotation matrix and the quaternion matrix provides a mathematical basis for conversion of quaternion and Euler angles, and the rest steps are to convert the quaternion into the Euler angles through the information of the rotation matrix, namely, according to elements at different positions in the matrix, the quaternion corresponding to each position in the matrix is used for resolving and optimizing the correct Euler angles.
By a rotation matrix C 0 To obtain a yaw angle
And roll angle psi 0 The calculation formula of (c):
ψ0＝arctan(C 12 /C 11 )
but since the range of the arctan function is in
Therefore, the Euler angle calculated according to the quaternion will fail to be solved, so it is necessary to do so>
Angle and psi 0 The angle is subjected to compensation operation of a full angle. Two points need to be considered when calculating the compensation:
the first point is that: according to C 33 、C 11 Positive and negative of (c) and cos (θ) 0 ) Whether the requested angle is at
Within the interval.
The second point is that: according to C 23 And C 12 The positive and negative of (d) determines whether to perform + pi compensation or-pi compensation.
To roll angle
And yaw angle psi 0 The calculation formula for performing the compensation operation of the full angle is as follows:
θ 0 ＝-arcsin(C 13 )
where sign (·) is a sign function.
Step three: and performing compensation operation on the multi-value conditions of the pitch angle, the roll angle and the yaw angle in the second partial interval to obtain the pitch angle, the roll angle and the yaw angle of which a single-value function cannot be directly solved.
In the second partial interval, the arcsin is not a single-value function in the interval, so that the pitch angle theta cannot be directly obtained by using the first part 0 Is calculated as the pitch angle theta 0 According to C 13 Is corrected to calculate the pitch angle theta 0 . At the same time, roll angle
Angle and yaw angle psi 0 The angle still needs to assume a full angle similar to the first partAnd (3) compensation operation, wherein-pi correction is used, and the specific correction formula is as follows:
step four: and a cross iteration method is used in the third partial interval to solve the problem that the yaw angle and the rolling angle are singular when the pitch angle is in the deadlock interval.
In the third partial region, the cosine of the pitch angle cos (θ) 0 ) Close to 0. Thus according to the rotation matrix C 0 When calculating, (C) 23 /C 33 ) And (C) 12 /C 11 ) In both terms, the numerator and denominator tend to be 0 at the same time, i.e. the "indeterminate" in mathematics, which results in a large jump in the calculation result, which is the "dead lock ring" phenomenon inherent in the euler angle representing the attitude in engineering practice. In order to solve the problem of deadlock ring, the problem that calculated values of a yaw angle and a rolling angle of a pitch angle in a deadlock interval are singular is solved through a cross iteration method; the implementation method of the cross iteration method comprises the following steps:
step 4-a: since it cannot be used (C) 23 /C 33 ) And (C) 12 /C 11 ) Two groups of numbers are directly calculated by searching a rotation matrix C 0 The other terms in (a) are operated on instead. Theta 0 In the dead zone interval, we get:
then used (1 + sin theta) 0 ) And (sin θ) 0 -1) avoiding the use of cos (. Theta.) (θ) 0 ) The problem of singular calculated values caused when the yaw angle and the roll angle are calculated. Since there are two dead lock angles of +90 ° and-90 °, respectively, different calculation formulas are used to determine ψ 0 And
the relational expression (c) of (c).
Step 4-b: if in the range (+ 90-epsilon )
Step 4-c: if in the interval (-90 ° - ε, -90 ° + ε): to obtain
Step 4-d: steps 4-b and 4-c only yield psi 0 And
cannot calculate the roll angle alone>
And yaw angle psi 0 Therefore, the 'dead zone' range is set to be +/-90-epsilon to +/-90 + epsilon, and the time t before the 'dead zone' range is obtained 0 Psi 0 Corner sum
Angle to update the current time t 1 Euler angle of (i.e. using t) 0 Phi of time 0 Angle calculation t 1 At a moment in time->
Angle, i.e. using t 0 Of time of day
Angle calculation t 1 Phi of time 1 And continuously performing cross iteration until the pitch angle theta leaves the dead zone. The cross iteration method is a method for estimating the attitude by using the internal information of the rotation matrix based on the principle that the attitude cannot jump in a large range, and can remarkably improve the conversion precision of the quaternion conversion Euler angle in a deadlock interval.
Step five: when the pitch angle theta is outside the dead zone interval, the multi-value results of the converted yaw angle and the converted rolling angle are appropriately chosen and optimized by adopting an absolute value sum of squares method, and when the pitch angle theta is in the dead zone interval, the multi-value problem of the pitch angle theta when the pitch angle theta spans +/-90 degrees is solved by adopting a sine sign method, so that the posture has continuity, and large-range jumping cannot occur in a very small time interval.
And (4) conversion between quaternions and Euler angles under full angles is realized through the steps two to four, but in the step two and the step three, the pitch angle theta is out of the dead zone interval, and the condition that one set of quaternions corresponds to two Euler angles is calculated by using the quaternion conversion Euler angle formula in the step two and the step three. In this case, it is preferable to appropriately select the conversion result by using the sum of squares of absolute values, that is, to perform a sum of squares by subtracting the absolute values of the calculated three euler angles from the absolute values of the three euler angles at the previous time. The method of 'sum of squares of absolute values' can ensure that the Euler angle calculated at the next moment has consistency with the Euler angle calculated at the previous moment. In the fourth step, because of using the cross iteration method, the multi-value problem caused by using an inverse function does not exist in the rolling angle and the yaw angle, but the multi-value problem still exists when the pitch angle theta spans +/-90 degrees at the moment, and the pitch angle theta is subjected to the trade-off by adopting the sine sign method, namely the pitch angle theta is subjected to the C 23 (C 12 ) And (5) performing auxiliary judgment. The judgment method comprises the following steps:
two alternative algorithms of 'sum of absolute squares' and 'sine sign' are comprehensively used, so that the posture has coherence, and large-range jumping cannot occur in a very small time interval.
Further comprises the following steps: the method for converting the large-attitude quaternion full-angle domain into the Euler angle, which is described in the first step to the fifth step, is applied to the fields of unmanned aerial vehicle large-attitude obstacle avoidance, robot pose acquisition or human body joint attitude capture, quaternion data of a large attitude can be processed on a micro control processor with weak floating point number operation capability, the Euler angle conversion of the large-attitude quaternion full-angle domain is realized, and then an accurate and jump-free Euler angle is obtained, so that a carrier can be better controlled according to the attitude.
Preferably, the method for converting the large-posture quaternion into the Euler angle in the full-angle domain is applied to the field of human joint posture capture. In the field of capturing the postures of the human joints, the situation that a pitch angle stays near a dead lock angle or a large posture often occurs in part of the human joints under physiological constraint, stable and coherent posture data can be obtained through the conversion method from the first step to the fifth step, a reliable data basis is provided for posture recurrence and motion analysis, and further the technical problems related to capturing the postures of the human joints are solved.
Has the beneficial effects that:
1. the invention discloses a method for converting a large-attitude quaternion full-angle domain into Euler angles, which divides the attitude into three types according to different intervals of a pitch angle: the first part is a dead zone interval in the neighborhood before and after the dead lock angle, the second part is an interval corresponding to the single value of the quaternion and the Euler angle, and the third part is an interval corresponding to the multi-value of the quaternion and the Euler angle; the method comprises the steps of analyzing quaternion-Euler angle conversion processes in a full-angle domain in detail, giving out quaternion-Euler angle conversion methods and accepting and rejecting methods in all intervals respectively to achieve conversion in the full-angle domain, and giving out different multi-value accepting and rejecting methods in three intervals divided in the full-angle domain, so that conversion values in all the intervals are guaranteed to be reliable and accurate.
2. The invention discloses a method for converting a large-attitude quaternion into Euler angles in a full-angle domain, which is characterized in that a cross iteration method is adopted to calculate alternative values of a rolling angle and a yaw angle in an Euler angle deadlock interval by utilizing internal information of a rotation matrix, so that the continuity of the attitude is ensured, and the attitude is better analyzed and solved.
3. The invention discloses a method for converting a large-attitude quaternion full-angle domain into an Euler angle, which solves the problem of multi-value accepting and rejecting errors when a pitch angle crosses a dead lock angle by adopting a sine sign method, further ensures that the pitch angle in a dead lock interval is smooth and does not jump, and obtains a more stable attitude.
4. The invention discloses a large-attitude quaternion full-angle domain Euler angle conversion method which is applied to the fields of unmanned aerial vehicle large-attitude obstacle avoidance, robot pose acquisition, human body joint attitude capture and the like, can process large-attitude quaternion data on a MCU (microprogrammed control unit) with insufficient floating-point number computing capability, realizes large-attitude quaternion full-angle domain Euler angle conversion, further obtains an accurate and jump-free Euler angle through quaternion conversion, and better controls a carrier according to the attitude.
5. The invention discloses a large-posture quaternion full-angle domain transformation Euler angle method, which is characterized in that in the field of human joint posture capture, partial joints of a human often have the situation that a pitch angle stays near a dead lock angle or a large posture under physiological constraint.
Drawings
FIG. 1 is a flow chart of a method for transforming a quaternion into an Euler angle in a large-attitude full range.
Fig. 2 is a schematic diagram of cross-iterative computation of dead zone euler angles.
FIG. 3 is a schematic diagram of a preferred method of selecting angles in the overall process.
Detailed Description
For better illustrating the objects and advantages of the present invention, the following description is provided in conjunction with the accompanying drawings and examples.
In order to verify the feasibility of the method, an Euler angle is converted into quaternions on a human body posture acquisition module with STM32 as a microprocessor, the pitch angle is assumed to be continuously changed from 0 degrees to 360 degrees, and the roll angle and the yaw angle are basically kept unchanged at 30 degrees.
The method for converting quaternion into Euler angle in a large-attitude full range disclosed by the embodiment comprises the following specific implementation steps:
the method comprises the following steps: due to the limitation of floating point number operation force of the selected microprocessor, when the pitch angle is in a 0.1-degree neighborhood of a deadlock angle, a relatively obvious rolling angle and yaw angle singularity phenomenon can occur, so 0.1 is selected as a value of epsilon. The attitude is divided into three parts according to different intervals of the pitch angle theta: the first part is a range (-89.9 °, +89.9 °) corresponding to a single value when the pitch angle is converted by quaternion, the second part is a range (-180 °, -90.1 °) and (90.1 °, +180 °) corresponding to multiple values when the pitch angle is converted by quaternion, and the third part is a dead zone range (-90.1 °, -89.9 °) and (+ 89.9 °, +90.1 °) in the vicinity of the front and rear of the "dead lock angle" in the pitch angle.
Step two: and (3) substituting quaternions acquired by the human body posture acquisition equipment into the rotation matrix and calculating the value of the rotation matrix:
determining the pitch angle at the current moment in the first partial interval according to the attitude angle at the previous moment, bringing quaternion into a rotation matrix to obtain each element, and calculating to obtain Euler angle
θ 1 ,ψ 1 ：
θ 11 ＝-arcsin(C 13 )
Step three: determining the pitch angle at the current moment in the second partial interval according to the attitude angle at the previous moment, bringing quaternion into the rotation matrix to obtain each element, and calculating to obtain Euler angle
θ 2 ,ψ 2 ：
θ 2 ＝-π*sign(C 13 )-arcsin(C 13 )
Step four: when the pitch angle theta enters a deadlock interval (-90.1 degrees, -89.9 degrees) or (+ 89.9 degrees, +90.1 degrees), the roll angle is calculated by adopting a cross iteration method
And yaw angle psi, while pitch angle theta is calculated and discarded by sinusoidal notation. The method comprises the following specific steps:
inputting Euler angle at the previous moment when the deadlock interval is entered and quaternion at the current moment, bringing the quaternion into a rotation matrix to obtain corresponding elements of the matrix, and then bringing the quaternion into a formula to obtain a rolling angle according to a method shown in figure 2
And yaw angle psi 1 。
Step 4-a: when the pitch angle θ is in the interval (+ 89.9 °, +90.1 °):
step 4-b: when the pitch angle θ is in the interval (-90.1 °, -89.9 °):
in Table 1, the following
ψ 0 ＝30°，θ 0 A gradual increase from 89.91 to 89.99 is a calculation illustration, by which it can be seen that the roll angle->
And the yaw angle psi does not drift, and the method has greater practicability and accuracy compared with the method which directly uses the uncertain result calculated by rotation.
TABLE 1
Step five: and in the first step to the fourth step, a pitch angle, a roll angle and a yaw angle which cannot be directly solved by a single-value function are calculated by a corresponding method, an accurate Euler angle is obtained in a deadlock interval to replace an odd dissimilarity angle, and a multi-value result of calculation needs to be reasonably rejected after calculation.
When the pitch angle theta is inOutside the dead zone interval, the multi-value results of the converted yaw angle and the converted roll angle are appropriately chosen by adopting an absolute value sum of squares method: calculating in the interval of the second step and the third step to obtain the Euler angle
θ 1 ,ψ 1 And &>
θ 2 ,ψ 2 And the euler angle of the last moment>
θ 0 ,ψ 0 And (3) comparison:
comparing the sizes of ans1 and ans2, and selecting a small Euler angle as the current moment.
When the pitch angle theta is in the dead zone interval, the multi-value problem of the pitch angle theta when the pitch angle theta spans +/-90 degrees is solved by adopting a sine sign method, and if the absolute value sum of squares of non-deadlock intervals is used, an error pitch angle theta value can be chosen when the absolute value sum of squares of the non-deadlock intervals is changed to 90.01, so that the pitch angle theta value is chosen by adopting the sine sign method. The specific steps are according to the current time
The value of pitch angle θ is calculated by:
the calculation of this method is illustrated in table 2 by taking the pitch angle θ spanning +90 °, and it can be seen that the converted angle is desirable.
TABLE 2
Further comprises the following steps: the method for converting the large-attitude quaternion full-angle domain into the Euler angle is applied to the fields of unmanned aerial vehicle large-attitude obstacle avoidance, robot pose acquisition or human body joint pose capture, quaternion data of a large attitude can be processed on a micro control processor with insufficient floating point number operation capability, the Euler angle conversion of the large-attitude quaternion full-angle domain is achieved, an accurate and jump-free Euler angle is obtained, and a carrier can be better controlled according to the attitude.
The above detailed description is intended to illustrate the objects, aspects and advantages of the present invention, and it should be understood that the above detailed description is only exemplary of the present invention and is not intended to limit the scope of the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the scope of the present invention.
Claims (4)
1. A method for converting a large-attitude quaternion full-angle domain into an Euler angle is characterized by comprising the following steps: comprises the following steps of (a) carrying out,
the method comprises the following steps: the attitude is divided into three parts according to different intervals of the pitch angle theta: the first part is an interval (-90 degrees + epsilon, +90 degrees-epsilon) corresponding to a single value when the quaternion converts the pitch angle; the second part is intervals (-180 degrees, -90 degrees-epsilon) and (90 degrees + epsilon, +180 degrees) corresponding to multiple values when quaternion angle is converted into a pitch angle, the third part is dead zone intervals (-90 degrees-epsilon, -90 degrees + epsilon) and (+ 90 degrees-epsilon, + epsilon) of the pitch angle in the front and back adjacent areas of the dead lock angle, wherein epsilon is a positive small value, and epsilon is determined according to the operation precision;
step two: performing compensation operation on the multi-value conditions of the rolling angle and the yaw angle in the first partial interval to obtain the rolling angle and the yaw angle of which a single-value function cannot be directly solved, and solving the problem of quaternion-to-Euler angle conversion in the partial interval;
the second step of the method is realized by the following steps,
in the first partial interval, since arcsin is a single-valued function in the interval, the rotation matrix C under the Z-Y-X coordinate system is used 0 Directly obtaining the pitch angle theta of the current moment 0 ：
θ 0 ＝-arsin(C 13 )
Wherein C is 13 Is a matrix C 0 Elements corresponding to the first row and the third column;
θ 0 ，ψ 0 roll angle, pitch angle and yaw angle, q 0 ，q 1 ，q 2 ，q 3 A real unit and three imaginary units of quaternion respectively;
the relation of the rotation matrix and the quaternion matrix provides a mathematical basis for conversion of quaternion and Euler angle, and the rest steps are to convert quaternion into Euler angle through the information of the rotation matrix, namely to calculate and optimize the correct Euler angle by using the quaternion corresponding to each position of the matrix according to the elements of different positions in the matrix;
by a rotation matrix C 0 To obtain a yaw angle
And roll angle psi 0 The calculation formula of (2):
ψ 0 ＝arctan(C 12 /C 11 )
but due to the range of the arctan function
Therefore, the Euler angle calculated according to the quaternion will fail to be solved, so it is necessary to do so>
Angle and psi 0 Carrying out compensation operation of the angle in a full angle manner; two points need to be considered when calculating the compensation:
the first point is that: according to C 33 、C 11 Positive and negative of (c) and cos (θ) 0 ) Whether the requested angle is at
Within the interval;
the second point is that: according to C 23 And C 12 Determining whether to perform + pi compensation or-pi compensation;
to pair
Angle and psi 0 The calculation formula for the compensation operation of the angle in the full angle is as follows:
θ 0 ＝-arcsin(C 13 )
step three: performing compensation operation on the multi-value conditions of the pitch angle, the roll angle and the yaw angle in the second partial interval to obtain the pitch angle, the roll angle and the yaw angle of which a single-value function cannot be directly solved;
the third step is to realize the method as follows,
in the second partial interval, because arcsin is not a single-valued function in the interval, the pitch angle theta cannot be directly obtained by using the first part 0 Is calculated as the pitch angle theta 0 According to C 13 Is corrected to calculate the pitch angle theta 0 (ii) a At the same time, roll angle
Angle and yaw angle psi 0 The angle still needs to adopt the full angle compensation operation similar to the first part, and at the moment, the-pi correction is used, and the specific correction formula is as follows:
θ 0 ＝-π*sign(C 13 )-arcsin(C 13 )
step four: the problem that the yaw angle and the rolling angle are singular when the pitch angle is in the deadlock interval is solved by using a cross iteration method in the third partial interval;
the implementation method of the fourth step is that,
in the third partial region, the cosine cos (θ) of the pitch angle 0 ) Is close to 0; thus according to the rotation matrix C 0 When calculating, (C) 23 /C 33 ) And (C) 12 /C 11 ) The two items have the condition that the numerator and denominator simultaneously tend to 0, namely the 'indeterminate' in mathematics, and the calculation result has great jump, wherein the phenomenon is the 'dead lock ring' phenomenon inherent when the Euler angle represents the posture; in order to solve the problem of deadlock ring, the problem that calculated values of a yaw angle and a rolling angle of a pitch angle in a deadlock interval are singular is solved by a cross iteration method;the implementation method of the cross iteration method is as follows:
step 4-a: since it cannot be used (C) 23 /C 33 ) And (C) 12 /C 11 ) Two groups of numbers are directly calculated by searching a rotation matrix C 0 Performing substitution operation on other terms in (1); theta 0 In the dead zone interval, we get:
at this time, (1+sin theta) 0 ) And (sin θ) 0 -1) avoiding the use of cos (. Theta.) (θ) 0 ) The problem of singular calculated values caused when the yaw angle and the roll angle are calculated; since there are two dead lock angles of +90 ° and-90 °, respectively, different calculation formulas are used to determine ψ 0 And
the relational expression of (1);
step 4-b: if in the range (+ 90-epsilon )
Step 4-c: if in the interval (-90 ° - ε, -90 ° + ε): to obtain
Step 4-d: steps 4-b and 4-c only yield psi 0 And
cannot calculate the roll angle alone>
And yaw angle psi 0 Therefore, the 'dead zone' range is set to be +/-90-epsilon to +/-90 + epsilon, and the time t before the 'dead zone' range is obtained 0 Psi 0 Angle and->
Angle to update the current time t 1 Euler angle of (i.e. using t) 0 Phi of time 0 Angle calculation t 1 At a moment in time->
Angle, i.e. using t 0 Time of day>
Angle calculation t 1 Phi of time 1 Continuously performing cross iteration until the pitch angle theta leaves a 'dead zone'; the cross iteration method is a method for estimating the attitude by using the internal information of the rotation matrix based on the principle that the attitude cannot jump in a large range, and can remarkably improve the conversion precision of the quaternion conversion Euler angle in a deadlock interval by using the cross iteration method;
step five: when the pitch angle theta is outside the dead zone interval, the multi-value results of the converted yaw angle and the converted rolling angle are appropriately chosen and optimized by adopting an absolute value sum of squares method, and when the pitch angle theta is in the dead zone interval, the multi-value problem of the pitch angle theta when the pitch angle theta spans +/-90 degrees is solved by adopting a sine sign method, so that the posture has continuity, and large-range jumping cannot occur in a very small time interval.
2. The method of claim 1, wherein the method for full-angle domain transformation of the large-attitude quaternion comprises the following steps: the method for converting the large-attitude quaternion full-angle domain into the Euler angle is applied to the fields of unmanned aerial vehicle large-attitude obstacle avoidance, robot pose acquisition or human body joint attitude capture, quaternion data of a large attitude can be processed on a micro control processor with low floating point number operation capability, the Euler angle conversion of the large-attitude quaternion full-angle domain is achieved, an accurate and jump-free Euler angle is obtained, and a carrier is controlled better according to the attitude.
3. The method of claim 2, wherein the method comprises the following steps: applying the large-attitude quaternion full-angle domain Euler angle conversion method from the first step to the fifth step to the field of human body joint attitude capture; in the field of capturing the postures of the human joints, the situation that a pitch angle stays near a dead lock angle or a large posture often occurs in part of the human joints under physiological constraint, stable and coherent posture data can be obtained through the conversion method from the first step to the fifth step, a reliable data base is provided for posture recurrence and motion analysis, and then the technical problem related to capturing the postures of the human joints is solved.
4. The method of claim 1, wherein the method comprises the following steps: the method is implemented by the steps of,
the quaternion and the Euler angle under the full angle are converted through the second step to the fourth step, but in the second step and the third step, the pitch angle theta is outside a dead zone interval, and the quaternion conversion Euler angle formula in the second step and the third step is used for calculating that the condition that one group of quaternion corresponds to two Euler angles can occur; at the moment, the conversion result is properly chosen and optimized by adopting an absolute value sum of squares method, namely, the calculated absolute values of the three angles of the Euler angles are respectively subtracted from the absolute values of the three angles of the Euler angles at the previous moment, and the square summation is carried out;the method of 'sum of squares of absolute values' can ensure that the Euler angle calculated at the next moment has consistency with the Euler angle calculated at the previous moment; in the fourth step, because of using the 'cross iteration method', the multi-value problem caused by using an inverse function does not exist in the rolling angle and the yaw angle, but the multi-value problem still exists when the pitch angle theta spans +/-90 degrees at the moment, and the pitch angle theta is subjected to the trade-off by adopting the 'sine sign method', namely the pitch angle theta is subjected to the trade-off by means of C 23 (C 12 ) Performing auxiliary judgment; the judgment method comprises the following steps:
two alternative algorithms of 'sum of absolute squares' and 'sine sign' are comprehensively used, so that the posture has coherence, and large-range jumping cannot occur in a very small time interval.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010832716.3A CN111966954B (en) | 2020-08-18 | 2020-08-18 | Large-attitude quaternion full-angle domain Euler angle conversion method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010832716.3A CN111966954B (en) | 2020-08-18 | 2020-08-18 | Large-attitude quaternion full-angle domain Euler angle conversion method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111966954A CN111966954A (en) | 2020-11-20 |
| CN111966954B true CN111966954B (en) | 2023-04-07 |
Family
ID=73387804
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010832716.3A Active CN111966954B (en) | 2020-08-18 | 2020-08-18 | Large-attitude quaternion full-angle domain Euler angle conversion method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111966954B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113103239B (en) * | 2021-04-29 | 2022-09-13 | 哈尔滨工业大学 | Robot attitude trajectory generation method and device and storage medium |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6859917B2 (en) * | 2017-10-13 | 2021-04-14 | 株式会社Ｊｖｃケンウッド | Angular velocity derivation device and angular velocity derivation method |
| CN109782787B (en) * | 2019-03-08 | 2020-10-30 | 北京航空航天大学 | Dual-mode MPC control method for attitude of under-actuated spacecraft under assistance of sunlight pressure |
| CN110608724B (en) * | 2019-09-10 | 2021-12-24 | 上海航天控制技术研究所 | Direct solving method for drift-free attitude in satellite maneuvering imaging process |
-
2020
- 2020-08-18 CN CN202010832716.3A patent/CN111966954B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN111966954A (en) | 2020-11-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Li et al. | Adaptive trajectory tracking of nonholonomic mobile robots using vision-based position and velocity estimation | |
| Liu et al. | Relative pose estimation for cylinder-shaped spacecrafts using single image | |
| CN108680156B (en) | Robot positioning method for multi-sensor data fusion | |
| CN108225370B (en) | Data fusion and calculation method of motion attitude sensor | |
| CN107066645A (en) | A kind of seven freedom biasing mechanism arm is against solution method | |
| CN107818598B (en) | Three-dimensional point cloud map fusion method based on visual correction | |
| CN105096341B (en) | Mobile robot position and orientation estimation method based on trifocal tensor and key frame strategy | |
| CN111966954B (en) | Large-attitude quaternion full-angle domain Euler angle conversion method | |
| Wang et al. | Certifiably optimal mutual localization with anonymous bearing measurements | |
| Cheng et al. | Approximate piecewise constant curvature equivalent model and their application to continuum robot configuration estimation | |
| CN114454174A (en) | Mechanical arm motion capturing method, medium, electronic equipment and system | |
| CN107167116B (en) | Visual detection method for spatial arc pose | |
| Wu et al. | Geometric interpretation of the general POE model for a serial-link robot via conversion into DH parameterization | |
| CN105488535A (en) | Three-dimensional point cloud matching method | |
| CN110686650A (en) | Monocular vision pose measuring method based on point characteristics | |
| CN109785393B (en) | Camera self-calibration method based on plane motion constraint | |
| WO2018214179A1 (en) | Low-dimensional bundle adjustment calculation method and system | |
| Mehta et al. | New approach to visual servo control using terminal constraints | |
| Tang et al. | Stereo visual-inertial odometry using structural lines for localizing indoor wheeled robots | |
| CN111145267A (en) | IMU (inertial measurement unit) assistance-based 360-degree panoramic view multi-camera calibration method | |
| WO2020010625A1 (en) | Method and system for optimizing kinematic model of robot, and storage device. | |
| Medeiros et al. | An Embedded Quaternion-Based Extended Kalman Filter Pose Estimation for Six Degrees of Freedom Systems | |
| CN113838120B (en) | Large-rotation-angle motion displacement measurement method based on digital image correlation | |
| CN111612847B (en) | Point cloud data matching method and system for robot grabbing operation | |
| Li et al. | Industrial Robot Hand–Eye Calibration Combining Data Augmentation and Actor-Critic Network |
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 |