CN108507527B - Method for calculating attitude angle of swing spray pipe - Google Patents

Method for calculating attitude angle of swing spray pipe Download PDF

Info

Publication number
CN108507527B
CN108507527B CN201810187326.8A CN201810187326A CN108507527B CN 108507527 B CN108507527 B CN 108507527B CN 201810187326 A CN201810187326 A CN 201810187326A CN 108507527 B CN108507527 B CN 108507527B
Authority
CN
China
Prior art keywords
coordinate system
nozzle
mems gyroscope
attitude angle
theta
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
Application number
CN201810187326.8A
Other languages
Chinese (zh)
Other versions
CN108507527A (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.)
Beijing Institute of Electronic System Engineering
Original Assignee
Beijing Institute of Electronic System Engineering
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 Beijing Institute of Electronic System Engineering filed Critical Beijing Institute of Electronic System Engineering
Priority to CN201810187326.8A priority Critical patent/CN108507527B/en
Publication of CN108507527A publication Critical patent/CN108507527A/en
Application granted granted Critical
Publication of CN108507527B publication Critical patent/CN108507527B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Gyroscopes (AREA)

Abstract

The invention discloses a method for resolving an attitude angle of a swinging spray pipe, which comprises the following specific steps of: 1. determining an initial value of the attitude angle; 2. determining a measurement value conversion matrix; 3. acquiring data of the MEMS gyroscope; 4. converting MEMS gyroscope data; 5. updating the quaternion; 6. and (6) resolving the attitude angle. The method has engineering realizability and can be used for calculating the attitude angle in the rotating process of the swinging spray pipe. The method has the outstanding characteristics that the decoupling of the attitude angle can be completed in the resolving process, and meanwhile, the automatic reading of the data is completed. The method has universality and can be popularized to the test of the swing spray pipes of other models.

Description

Method for calculating attitude angle of swing spray pipe
Technical Field
The invention relates to an attitude angle calculation method, in particular to a swing nozzle attitude angle calculation method.
Background
Generally, in the test process of the swing nozzle, a graduated scale is adopted to measure the attitude angle of the swing nozzle. The graduated scale belongs to a mechanical device, cannot automatically read, and needs manual measurement and reading; and the graduated scale can only measure one-dimensional motion, and when the swing spray pipe moves simultaneously in two directions, the graduated scale can not realize decoupling.
Accordingly, it is desirable to provide a method for automatically resolving the swing nozzle attitude angle.
Disclosure of Invention
The invention aims to solve at least one of the problems and provides a method for calculating the attitude angle of the swing nozzle.
In order to solve the technical problems, the invention adopts the following technical scheme:
a method for calculating an attitude angle of a swinging spray pipe comprises the following specific steps:
step 1: determining an initial attitude angle according to an initial installation angle of the MEMS gyroscope on the swinging nozzle, so as to determine an initial quaternion;
step 2: fixedly connecting the MEMS gyroscope on the swinging nozzle, and determining a conversion matrix from a coordinate system of the swinging nozzle body to a measurement coordinate system of the MEMS gyroscope according to an initial attitude angle;
and step 3: in the test process of the swing nozzle, data measured by the MEMS gyroscope are collected;
and 4, step 4: converting the data acquired in the step 3 into a coordinate system of the swinging nozzle through a conversion matrix;
and 5: updating and calculating the quaternion according to the initial quaternion in the first step and the test data obtained in the fourth step;
step 6: and resolving the attitude angle according to the corresponding relation between the quaternion and the attitude angle. The attitude angle is the attitude angle of the swinging nozzle.
Preferably, step 1 is specifically:
an initial angle is formed between a coordinate system of the swinging spray pipe and a measurement coordinate system of the MEMS gyroscope
Figure BDA0001590666340000011
According to the conversion relation of the coordinates, the conversion relation from the coordinate system of the swing nozzle body to the measurement coordinate system of the MEMS gyroscope can be obtained, and the quaternion is used for identification:
Figure BDA0001590666340000021
wherein:
Figure BDA0001590666340000022
an initial value of a pitch angle is obtained; d1Is a quaternion rotation axis X-direction component; d2Is a Y-direction component of a quaternion rotation axis; d3Is a quaternion rotation axis Z-direction component; q. q.s0、q1、q2、q3Is the initial quaternion.
Preferably, in step 2, a transformation matrix from the oscillating nozzle body coordinate system to the MEMS gyroscope measurement coordinate system is:
Figure BDA0001590666340000023
preferably, the data measured by the MEMS gyroscope in step 3 includes θxc1、θyc1、θzc1Wherein thetaxc1Is an X-direction MEMS gyroscope measurement; thetayc1Is a Y-direction MEMS gyroscope measurement; thetazc1Are Z-direction MEMS gyroscope measurements.
Preferably, the transformation matrix in step 4 is:
Figure BDA0001590666340000024
wherein, thetaxbSwinging the nozzle body in the X direction to obtain a coordinate system conversion value; thetaybSwinging the nozzle body in the Y direction to obtain a coordinate system conversion value; thetazbThe coordinate system conversion value of the nozzle body swings in the Z direction.
Preferably, the quaternion update calculation method in step 5 is as follows:
Figure BDA0001590666340000025
Figure BDA0001590666340000031
wherein: theta is a measured value synthesis result; q. q.s01、q11、q21、q31Is an updated quaternion.
Preferably, the attitude angle calculation method in step 6 specifically includes:
Figure BDA0001590666340000032
wherein:
Figure BDA0001590666340000033
is a pitch angle; psi is the yaw angle; gamma is the roll angle.
The invention has the following beneficial effects:
the invention provides a method for calculating the attitude angle of a swinging spray pipe, which has engineering realizability and can calculate the attitude angle in the rotating process of the swinging spray pipe. The method has the outstanding characteristics that the decoupling of the attitude angle can be completed in the resolving process, meanwhile, the automatic reading of the data is completed, and the method has universality and can be popularized to the test of the swing spray pipes of other models.
Drawings
The following describes embodiments of the present invention in further detail with reference to the accompanying drawings.
FIG. 1 shows a layout of a swinging nozzle and MEMS gyroscope in the method of the present invention.
Description of reference numerals: 1. swinging the spray pipe; 2. a MEMS gyroscope; 3. an initial value of a pitch angle; 4. swinging a nozzle coordinate system; 5. the MEMS gyroscope measures a coordinate system.
Detailed Description
In the following description, for purposes of explanation, numerous specific details are set forth in order to provide a thorough understanding of one or more embodiments. It may be evident, however, that such embodiment(s) may be practiced without these specific details. In other instances, well-known structures and devices are shown in block diagram form in order to facilitate describing one or more embodiments.
In order to more clearly illustrate the invention, the invention is further described below with reference to preferred embodiments and the accompanying drawings. It is to be understood by persons skilled in the art that the following detailed description is illustrative and not restrictive, and is not to be taken as limiting the scope of the invention.
The invention provides a method for calculating an attitude angle of a swinging nozzle 1, which is specifically described by combining the accompanying drawings. Fig. 1 shows a layout of a pendulum nozzle 1 and a MEMS gyroscope 2.
The method for calculating the attitude angle of the swinging nozzle 1 is based on a coordinate system defined as follows:
swinging nozzle coordinate system definition:
the swinging spray pipe is horizontally arranged, wherein ObXb points to the tail part of the spray pipe; ObYb is vertical to ObXb and points right above; the ObZb, the ObXb and the ObYb form a right-hand coordinate system;
MEMS gyroscope measurement coordinate system definition:
oc1Z c1 coincides with the ObZb axis; the plane Oc1Xc1Yc1 formed by Oc1Xc1 and Oc1Yc1 is superposed with the plane ObXbYb formed by ObXb and ObYb, and the included angle between the Oc1Xc1 and ObXb is
Figure BDA0001590666340000041
The included angle between the Oc1Yc1 and the ObYb is
Figure BDA0001590666340000042
The method is implemented according to the following steps:
first step attitude angle initial value determination
According to the coordinate system defined above, there is an initial angle between the coordinate system of the oscillating nozzle 1 and the measurement coordinate system of the MEMS gyroscope 2
Figure BDA0001590666340000043
According to the conversion relation of the coordinates, the conversion relation from the body coordinate system of the swing nozzle 1 to the measurement coordinate system of the MEMS gyroscope 2 can be obtained, and the quaternion is used for identification.
Figure BDA0001590666340000044
Figure BDA0001590666340000045
Wherein:
Figure BDA0001590666340000046
-initial pitch angle value 3;
d1-a quaternion axis of rotation X-direction component;
d2-a quaternion rotation axis Y component;
d3-a quaternion axis of rotation Z-direction component;
q0、q1、q2、q3-initial quaternion.
Second measurement transformation matrix determination
The MEMS gyroscope 2 is fixedly connected to the swinging nozzle 1, and a conversion matrix between an MEMS gyroscope measurement coordinate system 5 and a swinging nozzle coordinate system 4 is determined through an initial attitude angle.
Figure BDA0001590666340000051
Mb←1: the oscillating nozzle body coordinate system 4 is transformed into the MEMS gyroscope measurement coordinate system 5.
Third step MEMS gyroscope 2 data acquisition
In the test process of the swing nozzle 1, data measured by the MEMS gyroscope 2 are collected, and the data comprise: thetaxc1、θyc1、θzc1。
θxc1: x-direction MEMS gyroscope 2 measurements; thetayc1: y-direction MEMS gyroscope 2 measurements; thetazc1: z-direction MEMS gyroscope 2 measurements.
Fourth step MEMS gyroscope data conversion
The measured values of the MEMS gyroscope 2 are converted from the MEMS gyroscope measurement coordinate system 5 to the pendulum nozzle coordinate system 4 by means of a measurement conversion matrix.
Figure BDA0001590666340000052
Wherein:
θxb: 4, converting a coordinate system of the X-direction swinging nozzle body into a value;
θyb: a coordinate system 4 conversion value of the Y-direction swinging nozzle body;
θzb: and 4, converting a coordinate system of the Z-direction swinging nozzle body into a value.
Fourth step quaternion update
And updating and calculating the quaternion according to the initial quaternion of the first step and the test data obtained in the fourth step.
Figure BDA0001590666340000053
Figure BDA0001590666340000061
Wherein:
θ: synthesizing the measured values;
q01、q11、q21、q31: and updating the quaternion.
Sixth step attitude angle solution
And resolving the attitude angle according to the corresponding relation between the quaternion and the attitude angle. This attitude angle is the attitude angle of the swing nozzle 1.
Figure BDA0001590666340000062
Wherein:
Figure BDA0001590666340000063
: a pitch angle;
psi: a yaw angle;
γ: the roll angle.
Therefore, the calculation of the attitude angle of the swing spray pipe is completed, and the method can be used for calculating the attitude angle in the rotation process of the swing spray pipe. The method has the outstanding characteristics that the decoupling of the attitude angle can be completed in the resolving process, and meanwhile, the automatic reading of the data is completed. The method has universality and can be popularized to the test of the swing spray pipes of other models.
It should be understood that the above-mentioned embodiments of the present invention are only examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention, and it will be obvious to those skilled in the art that other variations or modifications may be made on the basis of the above description, and all embodiments may not be exhaustive, and all obvious variations or modifications may be included within the scope of the present invention.

Claims (5)

1. A method for calculating an attitude angle of a swinging nozzle is characterized by comprising the following specific steps:
step 1: determining an initial attitude angle according to an initial installation angle of the MEMS gyroscope on the swinging nozzle, so as to determine an initial quaternion;
step 2: fixedly connecting the MEMS gyroscope on the swinging nozzle, and determining a conversion matrix from a coordinate system of the swinging nozzle to a measurement coordinate system of the MEMS gyroscope according to an initial attitude angle;
and step 3: in the test process of the swing nozzle, data measured by the MEMS gyroscope are collected;
and 4, step 4: converting the data acquired in the step 3 into a coordinate system of the swinging nozzle through a conversion matrix;
and 5: updating and calculating the quaternion according to the initial quaternion obtained in the step 1 and the data converted into the coordinate system of the swing nozzle obtained in the step 4;
step 6: resolving the attitude angle according to the corresponding relation between the quaternion and the attitude angle;
the step 1 specifically comprises the following steps:
step 2.1: oscillating nozzle coordinate system definition
The swinging spray pipe is horizontally arranged, wherein ObXb points to the tail part of the spray pipe; ObYb is vertical to ObXb and points right above; the ObZb, the ObXb and the ObYb form a right-hand coordinate system;
step 2.2: MEMS gyroscope measurement coordinate system definition
Oc1Z c1 coincides with the ObZb axis; the plane Oc1Xc1Yc1 formed by Oc1Xc1 and Oc1Yc1 is superposed with the plane ObXbYb formed by ObXb and ObYb, and the included angle between the Oc1Xc1 and ObXb is
Figure FDA0002657870230000011
The included angle between the Oc1Yc1 and the ObYb is
Figure FDA0002657870230000012
Step 2.3: an initial angle is formed between a coordinate system of the swinging spray pipe and a measurement coordinate system of the MEMS gyroscope
Figure FDA0002657870230000013
According to the conversion relation of the coordinates, the conversion relation from the swinging nozzle coordinate system to the MEMS gyroscope measurement coordinate system can be obtained, and the quaternion is used for identification:
Figure FDA0002657870230000014
wherein: d1Is a quaternion rotation axis X-direction component; d2Is a Y-direction component of a quaternion rotation axis; d3Is a quaternion rotation axis Z-direction component; q. q.s0、q1、q2、q3Is an initial quaternion;
the quaternion updating calculation method in the step 5 comprises the following steps:
Figure FDA0002657870230000021
Figure FDA0002657870230000022
wherein: thetaxbA coordinate system conversion value of the swinging nozzle in the X direction is obtained; thetaybThe coordinate system conversion value of the swinging nozzle in the Y direction is obtained; thetazbA coordinate system conversion value of the Z-direction swinging spray pipe is obtained; theta is a measured value synthesis result; q. q.s01、q11、q21、q31Is an updated quaternion.
2. The method for calculating the attitude angle of the oscillating nozzle according to claim 1, wherein the conversion matrix from the coordinate system of the oscillating nozzle to the measurement coordinate system of the MEMS gyroscope in the step 2 is:
Figure FDA0002657870230000023
3. the oscillating nozzle attitude angle calculation method according to claim 1, wherein the data measured by the MEMS gyroscope in step 3 includes θxc1、θyc1、θzc1Wherein thetaxc1Is ME in the X directionMS gyroscope measurement; thetayc1Is a Y-direction MEMS gyroscope measurement; thetazc1Are Z-direction MEMS gyroscope measurements.
4. The method for calculating the attitude angle of the oscillating nozzle according to claim 2, wherein the specific calculation method for converting the data into the coordinate system of the oscillating nozzle through the transformation matrix in the step 4 is as follows:
Figure FDA0002657870230000024
wherein, thetaxc1Is an X-direction MEMS gyroscope measurement; thetayc1Is a Y-direction MEMS gyroscope measurement; thetazc1Is a Z-direction MEMS gyroscope measurement; thetaxbA coordinate system conversion value of the swinging nozzle in the X direction is obtained; thetaybThe coordinate system conversion value of the swinging nozzle in the Y direction is obtained; thetazbIs the coordinate system conversion value of the Z-direction swinging nozzle.
5. The swing nozzle attitude angle calculation method according to claim 1, wherein the attitude angle calculation method in step 6 specifically comprises:
Figure FDA0002657870230000031
wherein:
Figure FDA0002657870230000032
is a pitch angle; psi is the yaw angle; gamma is the roll angle.
CN201810187326.8A 2018-03-07 2018-03-07 Method for calculating attitude angle of swing spray pipe Active CN108507527B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810187326.8A CN108507527B (en) 2018-03-07 2018-03-07 Method for calculating attitude angle of swing spray pipe

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810187326.8A CN108507527B (en) 2018-03-07 2018-03-07 Method for calculating attitude angle of swing spray pipe

Publications (2)

Publication Number Publication Date
CN108507527A CN108507527A (en) 2018-09-07
CN108507527B true CN108507527B (en) 2020-12-22

Family

ID=63376202

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810187326.8A Active CN108507527B (en) 2018-03-07 2018-03-07 Method for calculating attitude angle of swing spray pipe

Country Status (1)

Country Link
CN (1) CN108507527B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110260888B (en) * 2019-06-06 2021-10-15 航天科工仿真技术有限责任公司 Swing angle measuring method, device and system
CN112596411B (en) * 2020-12-10 2022-09-09 内蒙航天动力机械测试所 Semi-physical control method for cold pendulum of solid rocket engine jet pipe

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101846510A (en) * 2010-05-28 2010-09-29 北京航空航天大学 High-precision satellite attitude determination method based on star sensor and gyroscope
JP2011220825A (en) * 2010-04-09 2011-11-04 Toyota Motor Corp Attitude estimating device and method, attitude controlling device and method, and program
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011220825A (en) * 2010-04-09 2011-11-04 Toyota Motor Corp Attitude estimating device and method, attitude controlling device and method, and program
CN101846510A (en) * 2010-05-28 2010-09-29 北京航空航天大学 High-precision satellite attitude determination method based on star sensor and gyroscope
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于陀螺仪的机器人自主运动定位研究;韩啸;《硕士学位论文电子期刊》;20131201;正文第18-31页 *

Also Published As

Publication number Publication date
CN108507527A (en) 2018-09-07

Similar Documents

Publication Publication Date Title
CN108225324B (en) Indoor positioning method based on intelligent terminal and integrating geomagnetic matching and PDR
CN107044852B (en) Total station survey method under out-of-flatness state
CN111537002B (en) Calibration method and orientation method for laser strapdown inertial measurement unit installation error
CN108507527B (en) Method for calculating attitude angle of swing spray pipe
Fedorov et al. Using of measuring system MPU6050 for the determination of the angular velocities and linear accelerations
CN108317999B (en) Communication iron tower inclination angle measuring method based on inertial sensor
CN114216456B (en) Attitude measurement method based on fusion of IMU and robot body parameters
CN109724602A (en) A kind of attitude algorithm system and its calculation method based on hardware FPU
CN109297665B (en) On-line test method for capturing track by using high-precision aerodynamic model
CN104182614A (en) System and method for monitoring attitude of mechanical arm with six degrees of freedom
CN108413988A (en) Robot end's theodolite coordinate system quick calibrating method
CN108458698A (en) Total powerstation does not flatten the unknown point three-dimensional coordinate computational methods of measurement
CN105607760A (en) Trace restoration method and system based on micro inertial sensor
CN106405581A (en) Evaluation method for coupling direction precision, caused by satellite structure deformation, of multiple types of loads
CN103364167B (en) A kind of view window refraction offset correction method
CN103759922B (en) A kind of space remote sensor scan mirror pointing accuracy measuring method
CN102506812A (en) VT checking method for stability judgment of reference points in deformation monitoring
CN113189541B (en) Positioning method, device and equipment
CN113375693A (en) Geomagnetic course error correction method
CN208671989U (en) Small distance movement track tracing device and system
CN114509071B (en) Attitude measurement method for wind tunnel test model
CN112484751B (en) Method for measuring position and attitude of spacecraft verifier in relatively large space test field coordinate system
CN110260888B (en) Swing angle measuring method, device and system
CN112629410A (en) Non-contact measuring equipment and method for inclination angle of space rod piece
CN106595584A (en) Antenna attitude data acquisition device and method and antenna device

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