CN112066984A - Attitude angle resolving method and device, processing equipment and storage medium - Google Patents
Attitude angle resolving method and device, processing equipment and storage medium Download PDFInfo
- Publication number
- CN112066984A CN112066984A CN202010979510.3A CN202010979510A CN112066984A CN 112066984 A CN112066984 A CN 112066984A CN 202010979510 A CN202010979510 A CN 202010979510A CN 112066984 A CN112066984 A CN 112066984A
- Authority
- CN
- China
- Prior art keywords
- attitude
- current
- value
- time
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
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/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
- 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Gyroscopes (AREA)
Abstract
The application relates to an attitude angle resolving method, an attitude angle resolving device, processing equipment and a storage medium. The method comprises the following steps: acquiring current-time triaxial angular velocity acquired by a gyroscope, current-time triaxial acceleration acquired by an acceleration sensor and current-time triaxial magnetic field intensity acquired by a magnetic field sensor; performing integral processing on the three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment; obtaining a current moment attitude value according to the current moment triaxial acceleration and the current moment triaxial magnetic field strength; obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment; correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment; the attitude correction value at the next moment is converted into the attitude angle, so that the attitude angle resolving precision is improved, the resolving error is reduced, and the angle drift phenomenon caused by vibration interference is avoided.
Description
Technical Field
The application relates to the technical field of attitude measurement, in particular to an attitude angle calculating method, an attitude angle calculating device, processing equipment and a storage medium.
Background
In recent years, research on attitude measurement technologies mainly includes an attitude measurement combination, an attitude calculation algorithm and an attitude measurement error compensation method, wherein on the basis of selecting a reference coordinate system, the purpose of attitude calculation is to determine the mathematical correlation between a moving coordinate system (a carrier coordinate system) and the reference coordinate system, and the attitude matrix representation is generally adopted. In order to obtain the attitude matrix, a reasonable attitude calculation method needs to be selected. The traditional attitude calculation algorithm includes an euler angle method (also called three-parameter method), a direction cosine method (also called nine-parameter method) and a quaternion method (also called four-parameter method). However, in the implementation process, the inventor finds that the prior art has at least the following technical problems: the attitude angle calculated by the traditional attitude calculation algorithm has low precision and large error, and is easy to be disturbed by vibration to cause the course angle to generate angle drift.
Disclosure of Invention
In view of the above, it is necessary to provide an attitude angle calculation method, an attitude angle calculation device, a processing apparatus, and a storage medium, which can improve the accuracy of the attitude angle calculation.
An attitude angle calculation method comprises the following steps:
acquiring current-time triaxial angular velocity acquired by a gyroscope, current-time triaxial acceleration acquired by an acceleration sensor and current-time triaxial magnetic field intensity acquired by a magnetic field sensor;
performing integral processing on the three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment;
obtaining a current moment attitude value according to the current moment triaxial acceleration and the current moment triaxial magnetic field strength;
obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment;
correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment;
and converting the attitude correction value of the next moment into an attitude angle.
In one embodiment, the step of performing integration processing on the three-axis angular velocity at the current time to obtain the attitude prediction value at the next time includes the steps of:
superposing the current-time zero offset error of the gyroscope on the current-time triaxial angular velocity to correct the current-time triaxial angular velocity;
and performing integral processing on the corrected three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment.
In one embodiment, when the zero offset error at the current time is the zero offset error at the initial time, the zero offset error at the current time is the mean value of the three-axis angular velocities of the gyroscope in a stationary state within a preset time period.
In one embodiment, the step of obtaining the filter fusion correction coefficient according to the variance of the attitude prediction value at the next time, the variance of the attitude value at the current time, and the error value of the attitude value at the current time includes the steps of:
and acquiring a zero offset error of the next moment according to the variance of the attitude predicted value of the next moment, the variance of the attitude value of the current moment and the error value of the attitude value of the current moment.
In one embodiment, the step of obtaining the attitude value at the current time according to the three-axis acceleration at the current time and the three-axis magnetic field strength at the current time includes the steps of:
acquiring an X axial angle and a Y axial angle under a preset space coordinate system according to the current moment triaxial acceleration;
projecting the triaxial magnetic field intensity on a horizontal plane at the current moment to obtain a Z axial angle under a preset space coordinate system;
and carrying out quaternion conversion on the X axial angle, the Y axial angle and the Z axial angle to obtain the attitude value at the current moment.
In one embodiment, the step of obtaining the current-time triaxial angular velocity acquired by the gyroscope, the current-time triaxial acceleration acquired by the acceleration sensor, and the current-time triaxial magnetic field strength acquired by the magnetic field sensor includes:
when a preset period comes, acquiring the current-time triaxial angular velocity acquired by the gyroscope, the current-time triaxial acceleration acquired by the acceleration sensor and the current-time triaxial magnetic field intensity acquired by the magnetic field sensor.
In one embodiment, before the step of obtaining the current-time three-axis angular velocity acquired by the gyroscope, the current-time three-axis acceleration acquired by the acceleration sensor, and the current-time three-axis magnetic field strength acquired by the magnetic field sensor, the method further includes the steps of:
an initialization procedure is performed.
An attitude angle solver, comprising:
the data acquisition module is used for acquiring the current-time triaxial angular velocity acquired by the gyroscope, the current-time triaxial acceleration acquired by the acceleration sensor and the current-time triaxial magnetic field intensity acquired by the magnetic field sensor;
the attitude prediction module is used for carrying out integral processing on the three-axis angular velocity at the current moment to obtain a predicted attitude value at the next moment;
the current attitude obtaining module is used for obtaining an attitude value at the current moment according to the triaxial acceleration at the current moment and the triaxial magnetic field strength at the current moment;
the correction coefficient acquisition module is used for obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment;
the correction module is used for correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment;
and the conversion module is used for converting the attitude correction value at the next moment into an attitude angle.
A processing device comprising a memory storing a computer program and a processor implementing the steps of the above method when executing the computer program.
A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the steps of the above-mentioned method.
One of the above technical solutions has the following advantages and beneficial effects:
the attitude angle calculating method provided by each embodiment of the application comprises the following steps: acquiring current-time triaxial angular velocity acquired by a gyroscope, current-time triaxial acceleration acquired by an acceleration sensor and current-time triaxial magnetic field intensity acquired by a magnetic field sensor; performing integral processing on the three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment; obtaining a current moment attitude value according to the current moment triaxial acceleration and the current moment triaxial magnetic field strength; obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment; correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment; the attitude correction value at the next moment is converted into an attitude angle, the attitude prediction value at the next moment is predicted through the current moment three-axis angular velocity, and then the acquired filter fusion correction coefficient is used for correcting the attitude prediction value at the next moment, so that the attitude angle calculation precision is improved, the calculation error is reduced, and the angle drift phenomenon caused by vibration interference is avoided.
Drawings
FIG. 1 is a schematic flow chart diagram of a method for attitude angle solution according to an embodiment;
FIG. 2 is a schematic flow chart illustrating the steps of obtaining a predicted attitude value at a next time in one embodiment;
FIG. 3 is a flowchart illustrating a step of obtaining an attitude value at a current time in one embodiment;
FIG. 4 is a block diagram showing the structure of an attitude angle resolver according to an embodiment;
FIG. 5 is a diagram of the internal structure of a processing device in one embodiment.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the present application and are not intended to limit the present application.
In one embodiment, as shown in fig. 1, there is provided an attitude angle solution method, including the steps of:
step S110, acquiring the current-time triaxial angular velocity acquired by the gyroscope, the current-time triaxial acceleration acquired by the acceleration sensor and the current-time triaxial magnetic field strength acquired by the magnetic field sensor.
It should be noted that, each time the system is started, an initialization procedure is executed before data is collected, that is, before step S110.
Specifically, the step of executing the initialization program includes the steps of:
executing a system initialization program; and executing an attitude angle initialization program.
The execution system initialization program includes initialization of the peripheral of the single chip, initialization of the acceleration sensor, initialization of the gyroscope and initialization of the magnetic field sensor, and necessary parameters such as a measuring range and a bandwidth are configured. The single chip microcomputer is used for executing the steps of the attitude angle calculating method.
The attitude angle initialization routine is executed to give each sensor an initial angle, which is used for subsequent attitude estimation. The initial angle given does not need to be too accurate, and the subsequent steps will gradually correct the initial angle, but the initial angle cannot deviate too much, which will increase the convergence time of the subsequent steps.
The initial angle comprises an initial X axial angle, an initial Y axial angle and an initial Z axial angle in a preset space coordinate system, and the acquisition process of the initial X axial angle and the initial Y axial angle is as follows: in one example, the specific steps of obtaining the initial angle are as follows: in a static state, an acceleration sensor samples an acceleration value, the acceleration value is the gravity acceleration of the earth, and an initial X axial angle and an initial Y axial angle are obtained according to component numerical values of the acceleration value on each axis of a preset space coordinate system.
In another example, in the motion state, the acceleration value acquired by the acceleration sensor is the sum of the gravity acceleration and the motion acceleration, the initial X axial angle and the initial Y axial angle acquired by the acceleration value have a deviation, and the initial X axial angle and the initial Y axial angle are filtered by using the three-axis angular velocity acquired by the gyroscope to correct the initial X axial angle and the initial Y axial angle.
The acquisition process of the initial Z-axis angle is as follows: acquiring the magnetic field intensity of the magnetic field sensor, acquiring the magnetic field component of the magnetic field intensity in an XY plane (namely a horizontal plane) of a preset coordinate system according to the initial X axial angle, the initial Y axial angle and the magnetic field intensity, and acquiring the initial Z axial angle according to the direction of the magnetic field component.
In order to further improve the accuracy of the attitude calculation, in one embodiment, when the preset period comes, the current-time triaxial angular velocity acquired by the gyroscope, the current-time triaxial acceleration acquired by the acceleration sensor, and the current-time triaxial magnetic field strength acquired by the magnetic field sensor are acquired. The preset period may be determined according to actual requirements, and in one example, the preset period generates a fixed time interval through a timer interrupt system of the single chip, and immediately executes a data sampling task after the time interval arrives, and stores data in a designated data register.
And step S120, performing integral processing on the three-axis angular velocity at the current moment to obtain a predicted attitude value at the next moment.
It should be noted that, the integral calculation is performed on the three-axis angular velocity at the current time, so as to perform the next prediction on the attitude. The kalman filtering algorithm needs to list a transfer equation of the system state to recur the attitude at the next moment. In one example, the next-time attitude prediction value comprises an attitude quaternion, in another example, the next-time attitude prediction value further comprises a zero-offset error of a gyroscope, and the next-time attitude prediction value can be calculated by applying a kinematic equation and combining the current-time three-axis angular velocity measured by the gyroscope. Because the zero offset error of the gyroscope can only be reduced as much as possible and cannot be eliminated fundamentally, the state transition equation is actually the attitude predicted value of the next moment obtained by performing integral operation on the three-axis angular velocity of the gyroscope at the current moment, and therefore, the accumulated error exists for a long time. However, since the data accuracy of the gyroscope is relatively high and the gyroscope is not influenced by the motion acceleration, the attitude angle calculated in a short time is relatively accurate. And the long-term drift term needs to be subjected to filtering correction by combining the current time attitude value obtained by the triaxial acceleration and the triaxial magnetic field strength in the subsequent steps, so that the long-term accumulated error is reduced. And when the attitude is recurred, the variance of the attitude is predicted in one step, and the latest attitude error level is estimated.
In order to further improve the attitude calculation accuracy, in an embodiment, as shown in fig. 2, the step of performing integration processing on the three-axis angular velocity at the current time to obtain the attitude prediction value at the next time includes the steps of:
step S210, superposing a zero offset error of the gyroscope at the current moment on the triaxial angular velocity at the current moment so as to correct the triaxial angular velocity at the current moment;
and step S220, performing integral processing on the corrected three-axis angular velocity at the current moment to obtain a predicted attitude value at the next moment.
It should be noted that, in the present application, the correction of the sensor is to superimpose the zero offset error of the sensor on the basis of the original data (i.e., the current-time three-axis angular velocity, the current-time three-axis acceleration, and the current-time three-axis magnetic field strength), and obtain more accurate original data of the sensor after the correction. Any sensor in use will have a zero offset error, which can be obtained by calibration and continuously updated iteratively in subsequent steps.
In one example, when the zero offset error at the current time is the zero offset error at the initial time, the zero offset error at the current time is the average value of the three-axis angular velocities of the gyroscope in a static state within a preset time period.
And step S130, obtaining the attitude value at the current moment according to the triaxial acceleration and the triaxial magnetic field strength at the current moment.
It should be noted that, as shown in fig. 3, the step of obtaining the attitude value at the current time according to the three-axis acceleration at the current time and the three-axis magnetic field strength at the current time includes the steps of:
step S310, acquiring an X axial angle and a Y axial angle under a preset space coordinate system according to the current moment triaxial acceleration;
step S320, projecting the triaxial magnetic field intensity on a horizontal plane at the current moment to obtain a Z axial angle under a preset space coordinate system;
and step S330, carrying out quaternion conversion on the X axial angle, the Y axial angle and the Z axial angle to obtain the attitude value at the current moment.
In order to further improve the attitude calculation accuracy, in the step of obtaining the attitude value at the current time according to the three-axis acceleration at the current time and the three-axis magnetic field strength at the current time, the three-axis acceleration at the current time and the three-axis magnetic field strength at the current time need to be corrected. Specifically, in one example, the current-time zero-offset error and the error of the scale factor of the acceleration sensor are obtained by a horizontal calibration method or a six-surface calibration method, the current-time zero-offset error of the acceleration sensor is superimposed on the current-time triaxial acceleration, the current-time triaxial acceleration is corrected, and the corrected current triaxial acceleration is used to obtain the current-time attitude value.
In one example, the magnetic field strength is fitted to form a spherical surface or an ellipsoidal surface by rotating the magnetic field sensor around three axes, the center of the spherical surface is the zero offset error of the magnetic field sensor at the current moment, and the half axis of the spherical surface is the scale factor of the magnetic field sensor.
And step S140, obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment.
It should be noted that the larger the motion acceleration is, the less accurate the calculation result of the attitude value at the current time is, and the motion acceleration can be superimposed as a coefficient on the result obtained by the variance calculation. The calculation of the motion acceleration converts the attitude value of the last moment into an attitude matrix, the attitude matrix is used for converting the gravity acceleration to obtain the projection of the gravity acceleration under a preset coordinate system, and then the projection of the gravity acceleration on each coordinate axis is subtracted from the three-axis acceleration to obtain the motion acceleration of the acceleration sensor. And after the filtering fusion correction coefficient is obtained, multiplying the filtering fusion correction coefficient by the error value of the attitude value at the current moment to obtain the correction value of the attitude prediction value at the next moment.
Further, the step of obtaining a filter fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment comprises the steps of:
and acquiring a zero offset error of the next moment according to the variance of the attitude predicted value of the next moment, the variance of the attitude value of the current moment and the error value of the attitude value of the current moment. The next-time zero-offset error is used for trimming data acquired by the next-time sensor, and specifically, the next-time zero-offset error includes a next-time zero-offset error of the acceleration sensor, a next-time zero-offset error of the gyroscope, and a next-time zero-offset error of the magnetic field sensor.
And S150, correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment.
It should be noted that, in an example, a kalman filter algorithm is used, and based on a filter fusion correction coefficient, a predicted value of an attitude at a next time is corrected, so as to obtain a corrected value of the attitude at the next time.
Step S160, converting the attitude correction value at the next time into an attitude angle.
It should be noted that the attitude correction value at the next time is an attitude quaternion, the quaternion cannot intuitively represent the three-axis attitude angle, and the quaternion needs to be converted into a corresponding attitude euler angle. The euler angles must be used in a predefined order of rotation of the coordinates, i.e. in what order the coordinates are rotated from the inertial frame to the preset coordinate frame of the device. One attitude corresponds to one attitude quaternion, but different euler angles can be calculated by different rotation sequences, and the euler angles are defined by the rotation sequence 321 in the application, namely the mode of rotating around the Z axis, then rotating around the Y axis and then rotating around the X axis. The attitude correction value at the next moment can be converted into an attitude angle by applying a conversion formula from quaternion to Euler angle.
Further, when the attitude angle is obtained, the attitude angle needs to be output, and specifically, the data output link packs and outputs the calculated data according to a predefined format. The output mode of the data CAN be a serial port, a CAN bus or an Ethernet mode according to the requirement. The data output format is packed according to the data content, each data packet has 11 bytes of data, and the data packet includes a data packet header, data content and a check value.
The attitude angle calculating method provided by each embodiment of the application comprises the following steps: acquiring current-time triaxial angular velocity acquired by a gyroscope, current-time triaxial acceleration acquired by an acceleration sensor and current-time triaxial magnetic field intensity acquired by a magnetic field sensor; performing integral processing on the three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment; obtaining a current moment attitude value according to the current moment triaxial acceleration and the current moment triaxial magnetic field strength; obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment; correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment; the attitude correction value at the next moment is converted into an attitude angle, the attitude prediction value at the next moment is predicted through the current moment three-axis angular velocity, and then the acquired filter fusion correction coefficient is used for correcting the attitude prediction value at the next moment, so that the attitude angle calculation precision is improved, the calculation error is reduced, and the angle drift phenomenon caused by vibration interference is avoided.
It should be understood that although the various steps in the flow charts of fig. 1-3 are shown in order as indicated by the arrows, the steps are not necessarily performed in order as indicated by the arrows. The steps are not performed in the exact order shown and described, and may be performed in other orders, unless explicitly stated otherwise. Moreover, at least some of the steps in fig. 1-3 may include multiple sub-steps or multiple stages that are not necessarily performed at the same time, but may be performed at different times, and the order of performance of the sub-steps or stages is not necessarily sequential, but may be performed in turn or alternating with other steps or at least some of the sub-steps or stages of other steps.
In one embodiment, as shown in fig. 4, there is provided an attitude angle solver, including:
the data acquisition module 41 is configured to acquire a current-time triaxial angular velocity acquired by the gyroscope, a current-time triaxial acceleration acquired by the acceleration sensor, and a current-time triaxial magnetic field strength acquired by the magnetic field sensor;
the attitude prediction module 42 is configured to perform integral processing on the three-axis angular velocity at the current time to obtain a predicted attitude value at the next time;
a current attitude obtaining module 43, configured to obtain an attitude value at the current time according to the current-time triaxial acceleration and the current-time triaxial magnetic field strength;
a correction coefficient obtaining module 44, configured to obtain a filter fusion correction coefficient according to the variance of the attitude prediction value at the next time, the variance of the attitude value at the current time, and the error value of the attitude value at the current time;
the correction module 45 is configured to correct the attitude prediction value at the next time based on the filtering fusion correction coefficient to obtain an attitude correction value at the next time;
and a conversion module 46, configured to convert the attitude correction value at the next time into an attitude angle.
For specific definition of the attitude angle solution device, see the above definition of the attitude angle solution method, and are not described herein again. All or part of each module in the attitude angle calculation device can be realized by software, hardware and a combination thereof. The modules can be embedded in a hardware form or independent from a processor in the processing device, and can also be stored in a memory in the processing device in a software form, so that the processor can call and execute operations corresponding to the modules.
In one embodiment, a processing device is provided, which may be a single chip microcomputer, and the internal structure diagram of the processing device may be as shown in fig. 5. The processing device includes a processor, a memory, an interface, and a database connected by a system bus. Wherein the processor of the processing device is configured to provide computing and control capabilities. The memory of the processing device includes a nonvolatile storage medium, an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of an operating system and computer programs in the non-volatile storage medium. The database of the processing equipment is used for storing data such as triaxial acceleration, triaxial magnetic field intensity and triaxial angular velocity. The interface of the processing device is used for connecting the sensor. The computer program is executed by a processor to implement an attitude angle solution method.
It will be appreciated by those skilled in the art that the configuration shown in fig. 5 is a block diagram of only a portion of the configuration relevant to the present application, and does not constitute a limitation on the processing device to which the present application is applied, and that a particular processing device may include more or less components than those shown, or combine certain components, or have a different arrangement of components.
In one embodiment, there is provided a processing device comprising a memory and a processor, the memory having stored therein a computer program, the processor implementing the following steps when executing the computer program:
acquiring current-time triaxial angular velocity acquired by a gyroscope, current-time triaxial acceleration acquired by an acceleration sensor and current-time triaxial magnetic field intensity acquired by a magnetic field sensor;
performing integral processing on the three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment;
obtaining a current moment attitude value according to the current moment triaxial acceleration and the current moment triaxial magnetic field strength;
obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment;
correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment;
and converting the attitude correction value of the next moment into an attitude angle.
In one embodiment, the processor, when executing the computer program, further performs the steps of:
superposing the current-time zero offset error of the gyroscope on the current-time triaxial angular velocity to correct the current-time triaxial angular velocity;
and performing integral processing on the corrected three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment.
In one embodiment, the processor, when executing the computer program, further performs the steps of:
acquiring an X axial angle and a Y axial angle under a preset space coordinate system according to the current moment triaxial acceleration;
projecting the triaxial magnetic field intensity on a horizontal plane at the current moment to obtain a Z axial angle under a preset space coordinate system;
and carrying out quaternion conversion on the X axial angle, the Y axial angle and the Z axial angle to obtain the attitude value at the current moment.
In one embodiment, a computer-readable storage medium is provided, having a computer program stored thereon, which when executed by a processor, performs the steps of:
acquiring current-time triaxial angular velocity acquired by a gyroscope, current-time triaxial acceleration acquired by an acceleration sensor and current-time triaxial magnetic field intensity acquired by a magnetic field sensor;
performing integral processing on the three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment;
obtaining a current moment attitude value according to the current moment triaxial acceleration and the current moment triaxial magnetic field strength;
obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment;
correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment;
and converting the attitude correction value of the next moment into an attitude angle.
In one embodiment, the computer program when executed by the processor further performs the steps of:
superposing the current-time zero offset error of the gyroscope on the current-time triaxial angular velocity to correct the current-time triaxial angular velocity;
and performing integral processing on the corrected three-axis angular velocity at the current moment to obtain a predicted value of the attitude at the next moment.
In one embodiment, the computer program when executed by the processor further performs the steps of:
acquiring an X axial angle and a Y axial angle under a preset space coordinate system according to the current moment triaxial acceleration;
projecting the triaxial magnetic field intensity on a horizontal plane at the current moment to obtain a Z axial angle under a preset space coordinate system;
and carrying out quaternion conversion on the X axial angle, the Y axial angle and the Z axial angle to obtain the attitude value at the current moment.
It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by hardware instructions of a computer program, which can be stored in a non-volatile computer-readable storage medium, and when executed, can include the processes of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium used in the embodiments provided herein may include non-volatile and/or volatile memory, among others. Non-volatile memory can include read-only memory (ROM), Programmable ROM (PROM), Electrically Programmable ROM (EPROM), Electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), Dynamic RAM (DRAM), Synchronous DRAM (SDRAM), Double Data Rate SDRAM (DDRSDRAM), Enhanced SDRAM (ESDRAM), Synchronous Link DRAM (SLDRAM), Rambus Direct RAM (RDRAM), direct bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).
The technical features of the above embodiments can be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the above embodiments are not described, but should be considered as the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the claims. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent shall be subject to the appended claims.
Claims (10)
1. An attitude angle calculation method is characterized by comprising the following steps:
acquiring current-time triaxial angular velocity acquired by a gyroscope, current-time triaxial acceleration acquired by an acceleration sensor and current-time triaxial magnetic field intensity acquired by a magnetic field sensor;
performing integral processing on the three-axis angular velocity at the current moment to obtain a predicted attitude value at the next moment;
obtaining an attitude value at the current moment according to the triaxial acceleration at the current moment and the triaxial magnetic field strength at the current moment;
obtaining a filtering fusion correction coefficient according to the variance of the attitude prediction value at the next moment, the variance of the attitude value at the current moment and the error value of the attitude value at the current moment;
correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment;
and converting the attitude correction value of the next moment into an attitude angle.
2. The attitude angle calculation method according to claim 1, wherein the step of performing integration processing on the three-axis angular velocity at the current time to obtain the attitude prediction value at the next time includes the steps of:
superposing the current-time zero offset error of the gyroscope on the current-time triaxial angular velocity to correct the current-time triaxial angular velocity;
and performing integral processing on the corrected three-axis angular velocity at the current moment to obtain the attitude predicted value at the next moment.
3. The attitude angle calculation method according to claim 2, wherein when the zero offset error at the current time is the zero offset error at the initial time, the zero offset error at the current time is the mean value of the three-axis angular velocities of the gyroscope in a stationary state within a preset time period.
4. The attitude angle calculation method according to claim 2, wherein the step of obtaining a filter fusion correction coefficient based on the variance of the attitude prediction value at the next time, the variance of the attitude value at the current time, and the error value of the attitude value at the current time includes the steps of:
and acquiring a zero offset error of the next moment according to the variance of the attitude predicted value of the next moment, the variance of the attitude value of the current moment and the error value of the attitude value of the current moment.
5. The attitude angle calculation method according to claim 1, wherein the step of obtaining the attitude value at the current time from the three-axis acceleration at the current time and the three-axis magnetic field strength at the current time includes the steps of:
acquiring an X axial angle and a Y axial angle under a preset space coordinate system according to the current time triaxial acceleration;
the projection of the three-axis magnetic field intensity on the horizontal plane at the current moment is used for acquiring the Z-axis angle under the preset space coordinate system;
and carrying out quaternion conversion on the X axial angle, the Y axial angle and the Z axial angle to obtain the attitude value at the current moment.
6. The attitude angle calculation method according to claim 1, wherein the step of obtaining the current-time triaxial angular velocity acquired by the gyroscope, the current-time triaxial acceleration acquired by the acceleration sensor, and the current-time triaxial magnetic field strength acquired by the magnetic field sensor includes:
when a preset period comes, acquiring the current-time triaxial angular velocity acquired by the gyroscope, the current-time triaxial acceleration acquired by the acceleration sensor and the current-time triaxial magnetic field intensity acquired by the magnetic field sensor.
7. The attitude angle calculation method according to any one of claims 1 to 6, wherein before the step of obtaining the current-time three-axis angular velocity acquired by the gyroscope, the current-time three-axis acceleration acquired by the acceleration sensor, and the current-time three-axis magnetic field strength acquired by the magnetic field sensor, the method further comprises the steps of:
an initialization procedure is performed.
8. An attitude angle resolver, comprising:
the data acquisition module is used for acquiring the current-time triaxial angular velocity acquired by the gyroscope, the current-time triaxial acceleration acquired by the acceleration sensor and the current-time triaxial magnetic field intensity acquired by the magnetic field sensor;
the attitude prediction module is used for carrying out integral processing on the three-axis angular velocity at the current moment to obtain a predicted attitude value at the next moment;
the current attitude obtaining module is used for obtaining an attitude value at the current moment according to the current-moment triaxial acceleration and the current-moment triaxial magnetic field strength;
a correction coefficient obtaining module, configured to obtain a filter fusion correction coefficient according to the variance of the next-time attitude prediction value, the variance of the current-time attitude value, and the error value of the current-time attitude value;
the correction module is used for correcting the attitude predicted value at the next moment based on the filtering fusion correction coefficient to obtain an attitude correction value at the next moment;
and the conversion module is used for converting the attitude correction value at the next moment into an attitude angle.
9. A processing device comprising a memory and a processor, the memory storing a computer program, characterized in that the processor realizes the steps of the method of any one of claims 1 to 7 when executing the computer program.
10. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the steps of the method according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010979510.3A CN112066984B (en) | 2020-09-17 | 2020-09-17 | Attitude angle resolving method and device, processing equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010979510.3A CN112066984B (en) | 2020-09-17 | 2020-09-17 | Attitude angle resolving method and device, processing equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112066984A true CN112066984A (en) | 2020-12-11 |
| CN112066984B CN112066984B (en) | 2022-08-30 |
Family
ID=73681730
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010979510.3A Active CN112066984B (en) | 2020-09-17 | 2020-09-17 | Attitude angle resolving method and device, processing equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112066984B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113188505A (en) * | 2021-04-14 | 2021-07-30 | 湖南三一智能控制设备有限公司 | Attitude angle measuring method and device, vehicle and intelligent arm support |
| CN113865571A (en) * | 2021-08-20 | 2021-12-31 | 无锡宇宁智能科技有限公司 | Method and device for improving application precision of mobile phone compass and readable storage medium |
| CN117450992A (en) * | 2023-09-21 | 2024-01-26 | 中国海洋工程研究院(青岛) | Gesture and wear monitoring and early warning method and system |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103674009A (en) * | 2012-09-04 | 2014-03-26 | 百度在线网络技术（北京）有限公司 | Method and device for obtaining movement locus of mobile terminal and mobile terminal |
| CN104764452A (en) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | Hybrid position-posture tracking method based on inertia and optical tracking systems |
| CN105136145A (en) * | 2015-08-11 | 2015-12-09 | 哈尔滨工业大学 | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method |
| CN105716610A (en) * | 2016-01-28 | 2016-06-29 | 北京航空航天大学 | Carrier attitude and heading calculation method assisted by geomagnetic field model and system |
| CN106767776A (en) * | 2016-11-17 | 2017-05-31 | 上海兆芯集成电路有限公司 | Mobile device and the method for asking for mobile device attitude |
| CN107272718A (en) * | 2017-06-19 | 2017-10-20 | 歌尔科技有限公司 | Attitude control method and device based on Kalman filtering |
| CN108519090A (en) * | 2018-03-27 | 2018-09-11 | 东南大学—无锡集成电路技术研究所 | A kind of implementation method of the binary channels integrated attitude determination algorithm of the UKF algorithms based on optimization |
| CN108534744A (en) * | 2018-01-30 | 2018-09-14 | 歌尔科技有限公司 | A kind of attitude angle acquisition methods, device and handle |
| CN108827299A (en) * | 2018-03-29 | 2018-11-16 | 南京航空航天大学 | A kind of attitude of flight vehicle calculation method based on improvement quaternary number second order complementary filter |
| CN109163721A (en) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | Attitude measurement method and terminal device |
| CN110146077A (en) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | Pose of mobile robot angle calculation method |
| CN111551175A (en) * | 2020-05-27 | 2020-08-18 | 北京计算机技术及应用研究所 | Complementary filtering attitude calculation method of attitude heading reference system |
-
2020
- 2020-09-17 CN CN202010979510.3A patent/CN112066984B/en active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103674009A (en) * | 2012-09-04 | 2014-03-26 | 百度在线网络技术（北京）有限公司 | Method and device for obtaining movement locus of mobile terminal and mobile terminal |
| CN104764452A (en) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | Hybrid position-posture tracking method based on inertia and optical tracking systems |
| CN105136145A (en) * | 2015-08-11 | 2015-12-09 | 哈尔滨工业大学 | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method |
| CN105716610A (en) * | 2016-01-28 | 2016-06-29 | 北京航空航天大学 | Carrier attitude and heading calculation method assisted by geomagnetic field model and system |
| CN106767776A (en) * | 2016-11-17 | 2017-05-31 | 上海兆芯集成电路有限公司 | Mobile device and the method for asking for mobile device attitude |
| CN107272718A (en) * | 2017-06-19 | 2017-10-20 | 歌尔科技有限公司 | Attitude control method and device based on Kalman filtering |
| CN108534744A (en) * | 2018-01-30 | 2018-09-14 | 歌尔科技有限公司 | A kind of attitude angle acquisition methods, device and handle |
| CN108519090A (en) * | 2018-03-27 | 2018-09-11 | 东南大学—无锡集成电路技术研究所 | A kind of implementation method of the binary channels integrated attitude determination algorithm of the UKF algorithms based on optimization |
| CN108827299A (en) * | 2018-03-29 | 2018-11-16 | 南京航空航天大学 | A kind of attitude of flight vehicle calculation method based on improvement quaternary number second order complementary filter |
| CN109163721A (en) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | Attitude measurement method and terminal device |
| CN110146077A (en) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | Pose of mobile robot angle calculation method |
| CN111551175A (en) * | 2020-05-27 | 2020-08-18 | 北京计算机技术及应用研究所 | Complementary filtering attitude calculation method of attitude heading reference system |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113188505A (en) * | 2021-04-14 | 2021-07-30 | 湖南三一智能控制设备有限公司 | Attitude angle measuring method and device, vehicle and intelligent arm support |
| CN113865571A (en) * | 2021-08-20 | 2021-12-31 | 无锡宇宁智能科技有限公司 | Method and device for improving application precision of mobile phone compass and readable storage medium |
| CN117450992A (en) * | 2023-09-21 | 2024-01-26 | 中国海洋工程研究院(青岛) | Gesture and wear monitoring and early warning method and system |
| CN117450992B (en) * | 2023-09-21 | 2024-05-03 | 中国海洋工程研究院(青岛) | Gesture and wear monitoring and early warning method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112066984B (en) | 2022-08-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112066984B (en) | Attitude angle resolving method and device, processing equipment and storage medium | |
| CN108507568B (en) | Method and device for compensating temperature drift error and integrated navigation system | |
| CN108871311B (en) | Pose determination method and device | |
| US20200286245A1 (en) | Posture estimation method, posture estimation apparatus and computer readable storage medium | |
| CN108957495A (en) | GNSS and MIMU Combinated navigation method | |
| CN108731664B (en) | Robot state estimation method, device, computer equipment and storage medium | |
| CN116067370B (en) | IMU gesture resolving method, IMU gesture resolving equipment and storage medium | |
| CN111522034B (en) | Positioning method, equipment and device based on inertial navigation | |
| CN112798021A (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
| CN110361003B (en) | Information fusion method, apparatus, computer device and computer readable storage medium | |
| CN115752471A (en) | Sensor data processing method and device and computer readable storage medium | |
| CN116086493A (en) | Nine-axis IMU calibration method, system, electronic equipment and storage medium | |
| CN112629565B (en) | Method, device and equipment for calibrating rotation relation between camera and inertial measurement unit | |
| CN109866217B (en) | Robot mileage positioning method, device, terminal equipment and computer storage medium | |
| CN112525144A (en) | Nonlinear attitude detection compensation method and terminal | |
| CN106931965B (en) | Method and device for determining terminal posture | |
| CN110160530B (en) | Spacecraft attitude filtering method based on quaternion | |
| CN116608859A (en) | Navigation method, storage medium and device of self-adaptive unscented Kalman filtering based on threshold processing | |
| CN115037703B (en) | Data processing method, apparatus, computer storage medium and computer program product | |
| CN102932686B (en) | Method for processing sensor data in remote controller | |
| CN114147717B (en) | Robot motion track estimation method, device, controller and storage medium | |
| CN112873209B (en) | Positioning sensor time delay calibration method and device, computer equipment and storage medium | |
| CN115355905A (en) | State estimation method and device for foot type robot and computer equipment | |
| CN111189471A (en) | Correction method, correction device and computer storage medium | |
| US20240085453A1 (en) | Method for evaluating sensor data, processing unit for evaluating sensor data, and sensor system |
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 |