CN112611380B - Attitude detection method based on multi-IMU fusion and attitude detection device thereof - Google Patents

Attitude detection method based on multi-IMU fusion and attitude detection device thereof Download PDF

Info

Publication number
CN112611380B
CN112611380B CN202011411697.3A CN202011411697A CN112611380B CN 112611380 B CN112611380 B CN 112611380B CN 202011411697 A CN202011411697 A CN 202011411697A CN 112611380 B CN112611380 B CN 112611380B
Authority
CN
China
Prior art keywords
imu
yaw angle
interference
angle
magnetic field
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
CN202011411697.3A
Other languages
Chinese (zh)
Other versions
CN112611380A (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.)
Yanshan University
Original Assignee
Yanshan University
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 Yanshan University filed Critical Yanshan University
Priority to CN202011411697.3A priority Critical patent/CN112611380B/en
Publication of CN112611380A publication Critical patent/CN112611380A/en
Application granted granted Critical
Publication of CN112611380B publication Critical patent/CN112611380B/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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
    • 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

Abstract

The invention provides a posture detection method based on multi-IMU fusion and a posture detection device thereof. The method comprises the steps that when a system is initialized, an electronic compass mounted under a main IMU acquires magnetic field strength to determine effectiveness of an initial yaw angle, in the measurement process, when the electronic compass detects strong magnetic interference, inclination angles of the main IMU and an auxiliary IMU are calculated, one IMU with the inclination angle closest to 45 degrees or 135 degrees is used for observing a yaw angle increment, and the yaw angle with weak drift fused with multiple IMUs is obtained. And calculating an interference coefficient S and a mutation factor I according to the magnetic field interference, acceleration or angular velocity mutation condition, and fusing a yaw angle calculated by an AHRS attitude calculation algorithm and a yaw angle with weak drift fused by multiple IMUs to obtain a yaw angle with multiple IMUs fused and anti-magnetic interference. The invention can improve the anti-magnetic field interference capability of the attitude sensor and weaken the drift of the relative yaw angle measured by the IMU without an electronic compass.

Description

Attitude detection method based on multi-IMU fusion and attitude detection device thereof
Technical Field
The invention relates to the field of embedded technology, in particular to a posture detection method based on multi-IMU fusion and a posture detection device thereof.
Background
In recent years, electronic inertia measurement units have been widely used in the fields of unmanned aerial vehicles, unmanned vehicles, smart phones and the like, but have problems of drift and magnetic interference in yaw angle measurement. The IMU with the axis of 6, namely the IMU without the electronic compass, has great drift when measuring the yaw angle, is not suitable for long-time use, the measured yaw angle can slide when the acceleration or the angular velocity changes rapidly, and the IMU with the axis of 9, namely the IMU with the electronic compass depends on the measurement of weak earth magnetic field when measuring the yaw angle, and is easily interfered by the magnetic field environment.
At present, a plurality of methods for measuring the yaw angle are available, such as RTK supporting positioning and orientation, map-based visual positioning, laser north finding, a mechanical code disc, an optical three-dimensional motion capture analysis system Vicon and the like. It is therefore still valuable to address or mitigate the drift and magnetic interference of the IMU.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a multi-inertial measurement unit IMU-based fused attitude detection method and a multi-IMU-based fused attitude detection device, which mainly design a multi-IMU installation method to ensure that one IMU always exists in the device during measurement to obtain a yaw angle with smaller drift, thereby weakening the drift of the measured yaw angle of the IMU without an electronic compass, and calculating an interference coefficient and a mutation coefficient according to the magnetic field interference, acceleration or angular velocity mutation conditions, so that the multi-IMU data is fused, the anti-interference capability and the detection precision of the measurement device are improved, and the device has certain practical application value.
The invention provides a gesture detection method based on multi-IMU fusion, which operates in an embedded system, wherein the embedded system comprises hardware and software, the hardware comprises an embedded processor, a main IMU and a plurality of auxiliary IMUs, the main IMU is mounted with an electronic compass, the auxiliary IMUs are not required to be mounted with the electronic compass, and the software operates on an embedded microprocessor; the attitude detection method comprises the following specific steps:
s1, when the attitude detection starts, initializing, respectively calibrating an accelerometer and a gyroscope of the main IMU and the auxiliary IMU, and simultaneously calibrating an electronic compass mounted under the main IMU;
s2, calculating magnetic field interference strength according to the magnetic field strength acquired by the electronic compass, and determining the initial value of the yaw angle according to the magnetic field interference strength:
s21, setting the three-axis magnetic field intensity of the environment to be M (M) measured by the electronic compassx,my,mz) Then the expression of the magnetic field strength m is:
Figure BDA0002817092380000021
s22, setting the geomagnetic intensity as mGroundThen the magnetic field interference intensity mInterference deviceThe expression of (a) is:
minterference device＝m-mGround
And recording the initial magnetic field interference intensity as mFirst stage；
S23, if the magnetic field interferes with the intensity mInterference deviceIf the yaw angle is greater than the set threshold value, the effective position of the yaw angle is 0, and meanwhile, the initial value of the yaw angle is set to be 0 and recorded in the variable Y;
s24, if the magnetic field interferes with the intensity mInterference deviceIf the current yaw angle is smaller than the set threshold value, the effective position of the yaw angle is 1, the main IMU combines with the electronic compass data, an AHRS attitude calculation algorithm is used for calculating to obtain the current yaw angle, the calculated current yaw angle is used as the initial value of the yaw angle, and the initial value is recorded in a variable Y;
s3, operating an algorithm for weakening the drift of the yaw angle by fusing multiple IMUs, calculating the inclination angles of the main IMU and the auxiliary IMU, observing the yaw angle increment by using the IMU with the inclination angle closest to 45 degrees or 135 degrees, and obtaining the yaw angle of the weak drift of the multiple IMU fusion:
s31, obtaining the Euler angle of the i-th IMU three-axis attitude by adopting the 6-axis IMU attitude calculation algorithm, namely the roll angle RiAngle of pitch PiAnd yaw angle YiWherein i is 0, 1, 2 … …, i is 0 representing data of the primary IMU, i is 1, 2 … … representing data of the secondary IMU;
s32, calculating the included angle between the Z axis and the normal vector vertical to the ground plane upwards, namely the inclined angle T according to the roll angle and the yaw angle of each IMU in the step S31i：
Figure BDA0002817092380000031
S33, for the convenience of comparing the tilt angles of IMUs to select an IMU for observation, the tilt angle T of the ith IMU in the step S32 is determinediConverted into a tilt D for comparisoni：
Figure BDA0002817092380000032
S34, setting the yaw angle increment of the ith IMU as delta YiIf the yaw angle of the IMU having the smallest absolute value of the tilt angle for comparison in step S33 is selected as the observed yaw angle, the increment Δ Y of the observed yaw angle is calculatedWatch withComprises the following steps:
ΔYwatch with＝ΔYk(k＝index(min(|Di|)))
The increment of the yaw angle of the weak drift Δ YMeasuringComprises the following steps:
ΔYmeasuring＝sign(ΔY0ΔYWatch with)ΔYWatch with
The yaw angle of the multi-IMU fused weak drift is:
Y＝Y+ΔYmeasuring；
S4, operating a multi-IMU fusion anti-magnetic interference yaw angle measurement algorithm, calculating an interference coefficient S and a mutation factor I according to magnetic field interference, acceleration or angular velocity mutation conditions, and fusing a yaw angle calculated by an AHRS attitude calculation algorithm with a yaw angle of weak drift of multi-IMU fusion in the step S3 to obtain a multi-IMU fusion anti-magnetic interference yaw angle:
s41, calculating the magnetic field disturbance intensity m according to the method in the step S2Interference deviceCombined with step S22To the initial magnetic field disturbance intensity mFirst stageObtaining the magnetic field interference intensity change mBecomeThe expression of (a) is:
mbecome＝mInterference device-mBeginning of the design
Let the magnetic field variation influence coefficient be k, and thereby obtain the expression of the interference coefficient S:
S＝kmbecome+(1-k)mInterference device；
S42, according to the acceleration A (a) collected by the main IMU accelerometerx,ay,az) And angular velocity W (W) acquired by the gyroscopex,wy,wz) Using an increment of acceleration dA (a)x,ay,az) Acceleration abrupt threshold dAMaxIncrement of angular velocity dW (w)x,wy,wz) And an abrupt change threshold value dW of angular velocityMaxAnd (3) quantifying the mutation to obtain an expression of a mutation factor I as follows:
Figure BDA0002817092380000041
wherein h is an acceleration sudden change influence coefficient;
s43, setting the interference threshold value as S according to the interference coefficient S and the mutation factor I obtained in the steps S41 and S42MaxSetting the mutation threshold value as IMaxThe expression for calculating the fusion coefficient v is:
Figure BDA0002817092380000042
s44, according to whether the interference coefficient S reaches the interference threshold SMaxAnd whether the mutation factor I has reached a mutation threshold IMaxAnd obtaining an expression of the fused yaw angle Y:
Figure BDA0002817092380000043
wherein, Delta YMeasuringFor weak drift obtained in step S34Delta Y is the difference between the yaw angle obtained by the main IMU and the electronic compass data through calculation by using an AHRS attitude calculation algorithm and the current yaw angle Y;
s5, resolving the master IMU by using a 6-axis IMU attitude resolving algorithm to obtain a pitch angle and a roll angle of the device;
and S6, repeating the steps S3 to S5, and finishing the real-time detection of the posture of the device.
It is preferable that, in step S1, the magnetic field disturbance is quantified, and the initial yaw angle is obtained from the quantified magnetic field disturbance.
Preferably, in step S3, the 6-axis IMU attitude solution algorithm is an algorithm for solving attitude using data of a three-axis accelerometer and a three-axis gyroscope, wherein a kalman filter algorithm or a complementary filter algorithm may be optionally used.
Preferably, in the yaw angle measurement algorithm with the multi-IMU fusion and magnetic interference resistance, the yaw angle of the detection device with the magnetic interference resistance is obtained through the interference coefficient and the mutation factor and the fusion of the data of the main IMU and the auxiliary IMU.
In another aspect of the invention, an attitude detection device using the attitude detection method based on multi-IMU fusion is provided, wherein the algorithm for weakening yaw angle drift through multi-IMU fusion and the yaw angle measurement algorithm for resisting magnetic interference through multi-IMU fusion are both operated in a device manufactured by adopting an IMU installation method for weakening yaw angle drift through multi-IMU fusion; the IMU installation method for weakening yaw angle drift through fusion of multiple IMUs is characterized in that the IMUs are installed in a certain angle relationship, so that the Z axis of one IMU always forms an included angle of not less than 20 degrees with a normal vector perpendicular to the horizontal plane and the horizontal plane in the movement of the measuring device, and the coordinate axis of the electronic compass is parallel to the coordinate axis of the main IMU and has the same direction with the coordinate axis of the main IMU.
Preferably, the included angle of 20 degrees is a boundary angle value which is preferably selected in an IMU installation method for weakening yaw angle drift through multi-IMU fusion, and other angle values nearby can be selected.
Compared with the prior art, the invention has the following advantages:
1. the invention provides an IMU installation method for weakening drift of a yaw angle by fusing multiple IMUs, so that one IMU always exists in the device selection and installation process, and the yaw angle with smaller drift can be obtained;
2. the invention provides an algorithm for weakening drift of a yaw angle by fusing multiple IMUs, which weakens the drift of the yaw angle measured by the IMU without an electronic compass and improves the accuracy of an attitude detection algorithm;
3. the invention provides a multi-IMU (inertial measurement unit) fused anti-magnetic interference yaw angle measurement algorithm, which improves the anti-magnetic interference capability of measuring a yaw angle based on an IMU (inertial measurement unit), weakens the influence of sudden change of acceleration and angular velocity data on yaw angle measurement, and improves the fault-tolerant capability of an attitude detection algorithm.
Drawings
FIG. 1 is a system block diagram of a multi-IMU fusion-based attitude detection method and an attitude detection apparatus thereof according to the present invention;
FIG. 2 is a diagram of an IMU installation method in the multi-IMU fusion-based attitude detection method and the attitude detection apparatus of the present invention;
FIG. 3 is an example diagram of a reliable measurement coverage of a weak drift yaw angle in the multi-IMU fusion based attitude detection method and the attitude detection apparatus thereof according to the present invention;
FIG. 4 is a flowchart of the multi-IMU fusion-based attitude detection method and the embedded processor program in the attitude detection apparatus according to the present invention.
Detailed Description
The invention will be described in detail with reference to the accompanying drawings for describing the technical content, the achieved purpose and the efficacy of the invention.
As shown in fig. 1, the gesture detection method is implemented in an embedded system, the embedded system includes hardware and software, the hardware includes an embedded processor, a main IMU and a plurality of auxiliary IMUs, wherein the main IMU is mounted with an electronic compass, the auxiliary IMUs do not need to be mounted with an electronic compass, and the software is implemented on the embedded microprocessor; the auxiliary IMU sends three Euler angles representing the self posture to the embedded processor, the main IMU sends three Euler angles representing the self posture, the magnetic field intensity, the three-axis acceleration and the angular velocity to the embedded processor, and the embedded processor can send instructions to the main IMU to switch the posture resolving algorithm.
The method for weakening the drift of the yaw angle by fusing the multiple IMUs and the yaw angle measurement algorithm for resisting the magnetic interference by fusing the multiple IMUs are both operated in a device manufactured by adopting the IMU installation method for weakening the drift of the yaw angle by fusing the multiple IMUs; the IMU installation method for weakening yaw angle drift through fusion of multiple IMUs is characterized in that the IMUs are installed in a certain angle relationship, so that the Z axis of one IMU always forms an included angle of not less than 20 degrees with a normal vector perpendicular to the horizontal plane and the horizontal plane in the movement of the measuring device, and the coordinate axis of the electronic compass is parallel to the coordinate axis of the main IMU and has the same direction with the coordinate axis of the main IMU.
In a preferred embodiment of the invention, the included angle of 20 degrees is a boundary angle value which is preferred in the IMU installation method for weakening the drift of the yaw angle by fusing multiple IMUs, and other nearby angle values can be selected.
As shown in fig. 4, the attitude detection method based on multi-IMU fusion specifically includes the following steps:
and S1, when the attitude detection starts, initializing, respectively calibrating the accelerometer and gyroscope of the main IMU and the auxiliary IMU, and simultaneously calibrating the electronic compass mounted under the main IMU.
S2, calculating the magnetic field interference intensity according to the magnetic field intensity collected by the electronic compass, and determining the initial value of the yaw angle according to the magnetic field interference intensity:
s21, setting the three-axis magnetic field intensity of the environment where the electronic compass is located as M (M) measured by the electronic compassx,my,mz) Then the expression of the magnetic field strength m is:
Figure BDA0002817092380000061
s22, setting the geomagnetic intensity as mGroundBecause the geomagnetic intensity is very weak, magnetic interference is generated in the near field, the intensity is generally much greater than the geomagnetic intensity, and approximately, the magnetic interference intensity mInterference deviceThe expression of (a) is:
minterference device＝m-mGround
And recording the initial magnetic field interference intensity as mFirst stage。
S23, if the magnetic field interferes with the intensity mInterference deviceAnd if the deviation angle is larger than the set threshold value, the effective position of the deviation angle is 0, and meanwhile, the initial value of the deviation angle is set to be 0 and recorded in the variable Y.
S24, if the magnetic field interferes with the intensity mInterference deviceAnd if the current yaw angle is smaller than the set threshold value, the effective position of the yaw angle is 1, the main IMU combines with the data of the electronic compass, the current yaw angle is obtained by resolving through an attitude resolving algorithm of an AHRS (attitude and heading reference system), and the resolved current yaw angle is used as an initial value of the yaw angle and is recorded in a variable Y.
S3, operating an algorithm for weakening the drift of the yaw angle by fusing multiple IMUs, calculating the inclination angles of the main IMU and the auxiliary IMU, observing the yaw angle increment by using the IMU with the inclination angle closest to 45 degrees or 135 degrees, and obtaining the yaw angle of the weak drift of the multiple IMU fusion:
s31, obtaining the Euler angle of the i-th IMU three-axis attitude by adopting the 6-axis IMU attitude calculation algorithm, namely the roll angle RiAngle of pitch PiAnd yaw angle YiWherein i is 0, 1, 2 … …, i is 0 representing data of the primary IMU, i is 1, 2 … … representing data of the secondary IMU; the 6-axis IMU attitude calculation algorithm is an algorithm for calculating the attitude by using data of a three-axis accelerometer and a three-axis gyroscope, and a Kalman filtering algorithm or a complementary filtering algorithm can be selected.
S32, calculating the included angle between the Z axis and the normal vector vertical to the ground plane upwards, namely the inclined angle T according to the roll angle and the yaw angle of each IMU in the step S31i：
Figure BDA0002817092380000071
S33, for the convenience of comparing the tilt angles of IMUs to select an IMU for observation, the tilt angle T of the ith IMU in the step S32 is determinediConverted into a tilt D for comparisoniComparing the inclination angles DiThe IMU for observation can be directly chosen by comparing its absolute values:
Figure BDA0002817092380000072
s34, setting the yaw angle increment of the ith IMU as delta YiIf the yaw angle of the IMU having the smallest absolute value of the tilt angle for comparison in step S33 is selected as the observed yaw angle, the increment Δ Y of the observed yaw angle is calculatedWatch withComprises the following steps:
ΔYwatch with＝ΔYk(k＝index(min(|Di|)))
The increment of the yaw angle of the weak drift Δ YMeasuringComprises the following steps:
ΔYside survey＝sign(ΔY0ΔYWatch with)ΔYWatch with
The yaw angle of the multi-IMU fused weak drift is:
Y＝Y+ΔYmeasuring。
S4, operating a multi-IMU fusion anti-magnetic interference yaw angle measurement algorithm, calculating an interference coefficient S and a mutation factor I according to magnetic field interference, acceleration or angular velocity mutation conditions, and fusing a yaw angle calculated by an AHRS attitude calculation algorithm with a yaw angle of weak drift of multi-IMU fusion in the step S3 to obtain the multi-IMU fusion anti-magnetic interference yaw angle, wherein the AHRS attitude calculation algorithm is an algorithm for calculating attitude by using data of a three-axis accelerometer, a three-axis gyroscope and a three-axis electronic compass:
s41, calculating the magnetic field disturbance intensity m according to the method in the step S2Interference deviceCombining the initial magnetic field disturbance intensity m obtained in step S22First stageObtaining the magnetic field interference intensity change mBecomeThe expression of (a) is:
mbecome＝mInterference device-mBeginning of the design
Let the magnetic field variation influence coefficient be k, and thereby obtain the expression of the interference coefficient S:
S＝kmbecome+(1-k)mInterference device。
S42, according to the acceleration A (a) collected by the main IMU accelerometerx,ay,az) And angular velocity W (W) acquired by the gyroscopex,wy,wz) Using an increment of acceleration dA (a)x,ay,az) Acceleration abrupt threshold dAMaxIncrement of angular velocity dW (w)x,wy,wz) And an abrupt change threshold value dW of angular velocityMaxAnd (3) quantifying the mutation to obtain an expression of a mutation factor I as follows:
Figure BDA0002817092380000081
wherein h is an acceleration sudden change influence coefficient.
S43, setting the interference threshold value as S according to the interference coefficient S and the mutation factor I obtained in the steps S41 and S42MaxSetting mutation threshold as IMaxThe expression for calculating the fusion coefficient v is:
Figure BDA0002817092380000082
s44, according to whether the interference coefficient S reaches the interference threshold SMaxAnd whether the mutation factor I has reached a mutation threshold IMaxAnd obtaining an expression of the fused yaw angle Y:
Figure BDA0002817092380000083
wherein, Delta YMeasuringAnd Δ Y is the difference between the yaw angle calculated by the main IMU in combination with the electronic compass data using the AHRS attitude calculation algorithm and the current yaw angle Y, which is the increment of the yaw angle of the weak drift obtained in step S34.
And S5, the master IMU calculates the pitch angle and the roll angle of the device by using a 6-axis IMU attitude calculation algorithm.
And S6, repeating the steps S3 to S5, and finishing the real-time detection of the posture of the device.
Further, in order to obtain an initial yaw angle of the device, in step S1, the magnetic field disturbance is quantized, and an initial yaw angle is obtained from the quantized magnetic field disturbance.
In the yaw angle measurement algorithm with the multi-IMU integrated with the anti-magnetic interference, the data of the main IMU and the auxiliary IMU are integrated by calculating the interference coefficient and the mutation factor, and the anti-magnetic interference yaw angle of the detection device is obtained.
The attitude detection method and the attitude detection device based on multi-IMU fusion of the invention are further described by combining the embodiment as follows:
when the method is applied to posture detection of carriers such as unmanned planes, unmanned vehicles and the like, the posture detection method operates in an embedded system, the embedded system comprises hardware and software, as shown in fig. 1, the hardware comprises an embedded processor STM32, a main IMU and three auxiliary IMUs, wherein the main IMU is an MPU9250, the auxiliary IMUs are MPUs 6050, and the software operates on an embedded microprocessor.
The embedded processor reads the three-axis acceleration, the angular velocity and the magnetic field intensity of the main IMU through 1 IIC interface, reads the three-axis acceleration and the angular velocity of the 3 auxiliary IMUs through the 3 IIC interfaces, can be connected with the controller through interfaces such as a serial port and the like, and sends the fused yaw angle to the controller through the interfaces.
The measuring device is designed by adopting an IMU installation method for weakening yaw angle drift through fusion of multiple IMUs, namely, the installation of each IMU keeps a certain angle relationship, so that the Z axis of one IMU always has an included angle which is not less than 20 degrees with a normal vector vertical to the horizontal plane and the horizontal plane in the movement of the measuring device, the coordinate axis of the electronic compass is parallel to the coordinate axis of the main IMU and has the same direction with the coordinate axis of the main IMU, the installation mode ensures that the measuring device is subjected to strong magnetic interference in low-speed movement, and one IMU can always obtain a relative yaw angle with smaller drift.
As shown in fig. 2, four IMUs are mounted on four faces of a tetrahedron having a special positional relationship, the bottom face of the tetrahedron is a regular triangle, the included angles between the three upper faces and the bottom face of the tetrahedron are all 60 degrees, the main IMU is mounted on the bottom face, the Z axis is vertical to the bottom face and upward, the X axis is parallel to one side, namely, a coordinate system C0 shown in fig. 2, the auxiliary IMU is pasted on the three upper faces of the tetrahedron, the Z axis is vertical to the face which the primary IMU is located outward, the X axis is parallel to the bottom face, namely, coordinate systems C1, C2 and C3 shown in fig. 2, and assuming that the euler angle of the mounting position of the main IMU is (0,0,0), the euler angles of the mounting positions of the three auxiliary IMUs are (0,60,60), (0, -60, -60) and (60,0, 90).
The coverage of reliable measurements of the weak drift yaw angle for a device designed according to the above method can be represented in three-dimensional mapping software, as shown in fig. 3. The coordinate origin of C1, C2 and C3 is coincided with C0, the Z axis of each coordinate system is taken as an axis, the origin is taken as a vertex, the effective inclination angle is taken as an included angle between a generatrix and the axis, a diagonal cone is drawn, then the cone covered by the invalid inclination angle in the cone is dug out, the included angle between the generatrix of the cone drawn in the figure 3 and the axis is 30-60 degrees, the space is basically covered, and when the included angle is enlarged to 20-70 degrees, the cone can be completely covered.
As shown in fig. 4, the attitude detection method for multi-IMU fusion of carriers such as unmanned aerial vehicles and unmanned vehicles includes the following specific implementation steps:
s1, when attitude detection starts, initialization is carried out firstly, each IMU acquires temperature drift for calibration of an accelerometer and a gyroscope, an electronic compass of a main IMU acquires magnetic field intensity, if the magnetic field intensity exceeds the geomagnetic intensity range, namely 0.5-0.6 Gauss, an initial yaw angle is invalid, an initial yaw angle effective position is 0, an initial yaw angle value is 0, otherwise, the initial yaw angle is valid, the initial yaw angle effective position is 1, and the electronic compass is used for resolving the current yaw angle as an initial value.
S2, after the initialization is completed, calculating the magnetic field interference strength according to the magnetic field strength acquired by the electronic compass, and determining the initial value of the yaw angle according to the magnetic field interference strength:
s21, setting the three-axis magnetic field intensity of the environment where the electronic compass is located as M (M) measured by the electronic compassx,my,mz) Then the expression of the magnetic field strength m is:
Figure BDA0002817092380000101
s22, setting the geomagnetic intensity as mGroundBecause the geomagnetic intensity is very weak, magnetic interference is generated in the near field, the intensity is generally much greater than the geomagnetic intensity, and approximately, the magnetic interference intensity mInterference deviceThe expression of (a) is:
minterference device＝m-mGround
And recording the initial magnetic field interference intensity as mFirst stage。
S23, ifMagnetic field interference intensity mInterference deviceAnd if the deviation angle is larger than the set threshold value, the effective position of the deviation angle is 0, and meanwhile, the initial value of the deviation angle is set to be 0 and recorded in the variable Y.
S24, if the magnetic field interferes with the intensity mInterference deviceAnd if the current yaw angle is smaller than the set threshold value, determining the effective position of the yaw angle as 1, resolving the current yaw angle by the main IMU in combination with the electronic compass data by using an AHRS (attitude and heading reference system) attitude resolving algorithm, and recording the resolved current yaw angle as an initial value of the yaw angle in a variable Y.
S3, operating an algorithm for weakening the drift of the yaw angle by fusing multiple IMUs, calculating the inclination angles of the main IMU and the auxiliary IMU, observing the yaw angle increment by using the IMU with the inclination angle closest to 45 degrees or 135 degrees, and obtaining the yaw angle of the weak drift of the multiple IMU fusion:
s31, obtaining the Euler angle of the i-th IMU three-axis attitude by adopting the 6-axis IMU attitude calculation algorithm, namely the roll angle RiAngle of pitch PiAnd yaw angle YiWherein i is 0, 1, 2 … …, i is 0 representing data of the primary IMU, i is 1, 2 … … representing data of the secondary IMU; the 6-axis IMU attitude calculation algorithm is an algorithm for calculating the attitude by using data of a three-axis accelerometer and a three-axis gyroscope, and a Kalman filtering algorithm or a complementary filtering algorithm can be selected.
S32, calculating the included angle between the Z axis and the normal vector vertical to the ground plane upwards, namely the inclined angle T according to the roll angle and the yaw angle of each IMU in the step S31i：
Figure BDA0002817092380000111
S33, selecting IMUs for observation by comparing the inclination angles of the IMUs conveniently, wherein the yaw angle increment of each IMU in the sampling period is delta YiThe tilt angle T of the ith IMU in step S32iConverted into a tilt D for comparisoni，：
Figure BDA0002817092380000112
S34, setting the yaw angle increment of the ith IMU as delta YiIf the yaw angle of the IMU having the smallest absolute value of the tilt angle for comparison in step S33 is selected as the observed yaw angle, the increment Δ Y of the observed yaw angle is calculatedWatch withComprises the following steps:
ΔYwatch with＝ΔYk(k＝index(min(|Di|)))
The increment of the yaw angle of the weak drift Δ YMeasuringComprises the following steps:
ΔYmeasuring＝sign(ΔY0ΔYWatch with)ΔYWatch with。
S4, operating a multi-IMU fusion anti-magnetic interference yaw angle measurement algorithm, calculating an interference coefficient S and a mutation factor I according to magnetic field interference, acceleration or angular velocity mutation conditions, and fusing a yaw angle calculated by an AHRS attitude calculation algorithm with a yaw angle of weak drift of multi-IMU fusion in the step S3 to obtain the multi-IMU fusion anti-magnetic interference yaw angle, wherein the AHRS attitude calculation algorithm is an algorithm for calculating attitude by using data of a three-axis accelerometer, a three-axis gyroscope and a three-axis electronic compass:
s41, calculating the magnetic field disturbance intensity m according to the method in the step S2Interference deviceCombining the initial magnetic field disturbance intensity m obtained in step S22First stageObtaining the magnetic field interference intensity change mBecomeThe expression of (c) is:
mbecome＝mInterference device-mFirst stage
Let the magnetic field variation influence coefficient be k, and thereby obtain the expression of the interference coefficient S:
S＝kmbecome+(1-k)mInterference device。
S42, according to the acceleration A (a) collected by the main IMU accelerometerx,ay,az) And angular velocity W (W) acquired by the gyroscopex,wy,wz) Using an increment of acceleration dA (a)x,ay,az) Acceleration abrupt threshold dAMaxIncrement of angular velocity dW (w)x,wy,wz) And an abrupt change threshold value dW of the angular velocityMaxAnd (3) quantifying the mutation to obtain an expression of a mutation factor I as follows:
I＝h|dA|+(1-h)|dW|
wherein h is an acceleration sudden change influence coefficient.
S43, setting the interference threshold value as S according to the interference coefficient S and the mutation factor I obtained in the steps S41 and S42MaxSetting the mutation threshold value as IMaxThe expression for calculating the fusion coefficient v is:
Figure BDA0002817092380000121
s44, the interference coefficient S reaches the interference threshold SMaxAnd the mutation factor I does not reach the mutation threshold IMaxIf so, the multi-IMU fusion yaw angle drift weakening algorithm of the step S3 is operated to calculate the increment of the yaw angle Y with weak drift, and the detailed calculation process is shown in the step S3; when the interference coefficient S reaches the interference threshold SMaxAnd the mutation factor I also reaches the mutation threshold IMaxAt this time, a fusion coefficient v needs to be calculated, the detailed calculation process is shown in step S43, and the observation angle of the electronic compass and the yaw angle of the weak drift of the multiple IMUs are fused as follows:
Figure BDA0002817092380000122
and S5, the master IMU calculates the pitch angle and the roll angle of the device by using a 6-axis IMU attitude calculation algorithm.
And S6, repeating the steps S3 to S5, and finishing the real-time detection of the posture of the device.
The above-mentioned embodiments are merely illustrative of the preferred embodiments of the present invention, and do not limit the scope of the present invention, and various modifications and improvements made to the technical solution of the present invention by those skilled in the art without departing from the spirit of the present invention shall fall within the protection scope defined by the claims of the present invention.

Claims (5)

1. The attitude detection method based on multi-IMU fusion is characterized in that the attitude detection method is operated in an embedded system, the embedded system comprises hardware and software, the hardware comprises an embedded processor, a main IMU and a plurality of auxiliary IMUs, the main IMU is mounted with an electronic compass, the auxiliary IMUs are not required to be mounted with the electronic compass, and the software is operated on an embedded microprocessor; the attitude detection method comprises the following specific steps:
s1, when the attitude detection starts, initializing, respectively calibrating an accelerometer and a gyroscope of the main IMU and the auxiliary IMU, and simultaneously calibrating an electronic compass mounted under the main IMU;
s2, calculating magnetic field interference strength according to the magnetic field strength acquired by the electronic compass, and determining the initial value of the yaw angle according to the magnetic field interference strength:
s21, setting the three-axis magnetic field intensity of the environment where the electronic compass is located as M (M) measured by the electronic compassx,my,mz) Then the expression of the magnetic field strength m is:
Figure FDA0003635632600000011
s22, setting the geomagnetic intensity as mGroundThen the magnetic field interference intensity mInterference deviceThe expression of (a) is:
minterference device＝m-mGround
And recording the initial magnetic field interference intensity as mFirst stage；
S23, if the magnetic field interferes with the intensity mInterference deviceIf the yaw angle is greater than the set threshold value, the effective position of the yaw angle is 0, and meanwhile, the initial value of the yaw angle is set to be 0 and recorded in the variable Y;
s24, if the magnetic field interferes with the intensity mInterference deviceIf the current yaw angle is smaller than the set threshold value, the effective position of the yaw angle is 1, the main IMU combines with the electronic compass data, an AHRS attitude calculation algorithm is used for calculating to obtain the current yaw angle, the calculated current yaw angle is used as the initial value of the yaw angle, and the initial value is recorded in a variable Y;
s3, operating an algorithm for weakening the drift of the yaw angle by fusing multiple IMUs, calculating the inclination angles of the main IMU and the auxiliary IMU, observing the yaw angle increment by using the IMU with the inclination angle closest to 45 degrees or 135 degrees, and obtaining the yaw angle of the weak drift of the multiple IMU fusion:
s31, obtaining the Euler angle of the i-th IMU three-axis attitude by adopting the 6-axis IMU attitude calculation algorithm, namely the roll angle RiAngle of pitch PiAnd yaw angle YiWherein i is 0, 1, 2 … …, i is 0 representing data of the primary IMU, i is 1, 2 … … representing data of the secondary IMU;
s32, calculating the included angle between the Z axis and the normal vector vertical to the ground plane upwards, namely the inclination angle T according to the roll angle and the pitch angle of each IMU in the step S31i：
Figure FDA0003635632600000021
S33, for the convenience of comparing the tilt angles of IMUs to select an IMU for observation, the tilt angle T of the ith IMU in the step S32 is determinediConverted into a tilt angle D for comparisoni：
Figure FDA0003635632600000022
S34, setting the yaw angle increment of the ith IMU as delta YiIf the yaw angle of the IMU having the smallest absolute value of the tilt angle for comparison in step S33 is selected as the observed yaw angle, the increment Δ Y of the observed yaw angle is calculatedWatch withComprises the following steps:
ΔYwatch with＝ΔYk(k＝index(min(|Di|)))
The increment of the yaw angle of the weak drift Δ YMeasuringComprises the following steps:
ΔYmeasuring＝sign(ΔY0ΔYWatch with)ΔYWatch with
The yaw angle of the multi-IMU fused weak drift is:
Y＝Y+ΔYmeasuring；
S4, operating a multi-IMU fusion anti-magnetic interference yaw angle measurement algorithm, calculating an interference coefficient S and a mutation factor I according to magnetic field interference, acceleration or angular velocity mutation conditions, and fusing a yaw angle calculated by an AHRS attitude calculation algorithm with a yaw angle of weak drift of multi-IMU fusion in the step S3 to obtain a multi-IMU fusion anti-magnetic interference yaw angle:
s41, calculating the magnetic field disturbance intensity m according to the method in the step S2Interference deviceCombining the initial magnetic field disturbance intensity m obtained in step S22First stageObtaining the magnetic field interference intensity change mBecomeThe expression of (a) is:
mbecome＝mInterference device-mFirst stage
Let the magnetic field variation influence coefficient be k, and thereby obtain the expression of the interference coefficient S:
S＝kmbecome+(1-k)mInterference device；
S42, according to the acceleration A (a) collected by the main IMU accelerometerx,ay,az) And angular velocity W (W) acquired by the gyroscopex,wy,wz) Using an increment of acceleration dA (a)x,ay,az) Acceleration abrupt threshold dAMaxIncrement of angular velocity dW (w)x,wy,wz) And an abrupt change threshold value dW of angular velocityMaxAnd (3) quantifying the mutation to obtain an expression of a mutation factor I as follows:
Figure FDA0003635632600000031
wherein h is an acceleration sudden change influence coefficient;
s43, setting the interference threshold value as S according to the interference coefficient S and the mutation factor I obtained in the steps S41 and S42MaxSetting the mutation threshold value as IMaxThe expression for calculating the fusion coefficient v is:
Figure FDA0003635632600000032
s44, according to whether the interference coefficient S reaches the interference threshold SMaxAnd whether the mutation factor I has reached a mutation threshold IMaxAnd obtaining an expression of the fused yaw angle Y:
Figure FDA0003635632600000033
wherein, Delta YMeasuringFor the increment of the yaw angle of the weak drift obtained in the step S34, Δ Y is a difference between the yaw angle obtained by the main IMU in combination with the electronic compass data and solved by using the AHRS attitude solution algorithm and the current yaw angle Y;
s5, resolving the master IMU by using a 6-axis IMU attitude resolving algorithm to obtain a pitch angle and a roll angle of the device;
and S6, repeating the steps S3 to S5, and finishing the real-time detection of the posture of the device.
2. The multi-IMU fusion-based pose detection method of claim 1, wherein in step S1, magnetic field interference is quantified, and an initial yaw angle is obtained from the quantified magnetic field interference.
3. The multi-IMU fusion-based attitude detection method of claim 1, wherein in step S3, the 6-axis IMU attitude solution algorithm is an algorithm for solving attitude using data of a three-axis accelerometer and a three-axis gyroscope, wherein a kalman filter algorithm or a complementary filter algorithm is also selected.
4. The attitude detection method based on multi-IMU fusion of claim 1, wherein in a yaw angle measurement algorithm of multi-IMU fusion anti-magnetic interference, the anti-magnetic interference yaw angle of the detection device is obtained by calculating an interference coefficient and a mutation factor and fusing data of the primary IMU and the secondary IMU.
5. An attitude detection device using the multi-IMU fusion based attitude detection method according to any one of claims 1-4, wherein the multi-IMU fusion yaw angle drift reduction algorithm and the multi-IMU fusion magnetic interference rejection yaw angle measurement algorithm are both run in a device manufactured by a multi-IMU fusion yaw angle drift reduction IMU installation method; the IMU installation method for weakening yaw angle drift through fusion of multiple IMUs is characterized in that the IMUs are installed in a certain angle relationship, so that the Z axis of one IMU always forms an included angle of not less than 20 degrees with a normal vector perpendicular to the horizontal plane and the horizontal plane in the movement of the measuring device, and the coordinate axis of the electronic compass is parallel to the coordinate axis of the main IMU and has the same direction with the coordinate axis of the main IMU.
CN202011411697.3A 2020-12-03 2020-12-03 Attitude detection method based on multi-IMU fusion and attitude detection device thereof Active CN112611380B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011411697.3A CN112611380B (en) 2020-12-03 2020-12-03 Attitude detection method based on multi-IMU fusion and attitude detection device thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011411697.3A CN112611380B (en) 2020-12-03 2020-12-03 Attitude detection method based on multi-IMU fusion and attitude detection device thereof

Publications (2)

Publication Number Publication Date
CN112611380A CN112611380A (en) 2021-04-06
CN112611380B true CN112611380B (en) 2022-07-01

Family

ID=75229717

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011411697.3A Active CN112611380B (en) 2020-12-03 2020-12-03 Attitude detection method based on multi-IMU fusion and attitude detection device thereof

Country Status (1)

Country Link
CN (1) CN112611380B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113126642B (en) * 2021-04-26 2022-12-02 广东华南计算技术研究所 Yaw angle measuring method based on multi-MEMS inertial sensor
CN114217628A (en) * 2021-12-24 2022-03-22 北京理工大学重庆创新中心 Double-path IMU unit unmanned aerial vehicle controller based on 5G communication and control method
CN115076561A (en) * 2022-05-18 2022-09-20 燕山大学 Tele-immersion type binocular holder follow-up system and method applied to engineering machinery
CN115060259A (en) * 2022-08-19 2022-09-16 江苏德一佳安防科技有限公司 Multi-IMU fusion positioning system and method

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7587277B1 (en) * 2005-11-21 2009-09-08 Miltec Corporation Inertial/magnetic measurement device
CN105300381A (en) * 2015-11-23 2016-02-03 南京航空航天大学 Rapid convergence method based on improved complementary filter for attitude of self-balance mobile robot
CN105588567A (en) * 2016-01-25 2016-05-18 北京航空航天大学 Automatic magnetometer correction assisted AHRS (altitude and heading reference system) and method
WO2017063387A1 (en) * 2015-10-13 2017-04-20 上海华测导航技术股份有限公司 Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
CN109084763A (en) * 2018-08-21 2018-12-25 燕山大学 Wearable three-dimensional indoor positioning device and method based on attitude angle measurement
CN109579836A (en) * 2018-11-21 2019-04-05 阳光凯讯(北京)科技有限公司 A kind of indoor pedestrian's bearing calibration method based on MEMS inertial navigation
CN110017837A (en) * 2019-04-26 2019-07-16 沈阳航空航天大学 A kind of Combinated navigation method of the diamagnetic interference of posture

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1872087A4 (en) * 2005-04-19 2012-10-17 Jaymart Sensors Llc Miniaturized inertial measurement unit and associated methods
US9207079B2 (en) * 2012-06-21 2015-12-08 Innovative Solutions & Support, Inc. Method and system for compensating for soft iron magnetic disturbances in a heading reference system
CN109506646A (en) * 2018-11-20 2019-03-22 石家庄铁道大学 A kind of the UAV Attitude calculation method and system of dual controller
CN109612471B (en) * 2019-02-02 2021-06-25 北京理工大学 Moving body attitude calculation method based on multi-sensor fusion

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7587277B1 (en) * 2005-11-21 2009-09-08 Miltec Corporation Inertial/magnetic measurement device
WO2017063387A1 (en) * 2015-10-13 2017-04-20 上海华测导航技术股份有限公司 Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
CN105300381A (en) * 2015-11-23 2016-02-03 南京航空航天大学 Rapid convergence method based on improved complementary filter for attitude of self-balance mobile robot
CN105588567A (en) * 2016-01-25 2016-05-18 北京航空航天大学 Automatic magnetometer correction assisted AHRS (altitude and heading reference system) and method
CN109084763A (en) * 2018-08-21 2018-12-25 燕山大学 Wearable three-dimensional indoor positioning device and method based on attitude angle measurement
CN109579836A (en) * 2018-11-21 2019-04-05 阳光凯讯(北京)科技有限公司 A kind of indoor pedestrian's bearing calibration method based on MEMS inertial navigation
CN110017837A (en) * 2019-04-26 2019-07-16 沈阳航空航天大学 A kind of Combinated navigation method of the diamagnetic interference of posture

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Sensor fusion-based attitude estimation using low-cost MEMS-IMU for mobile robot navigation;Lu Lou等;《2011 6th IEEE Joint International Information Technology and Artificial Intelligence Conference》;20110822;第465-468页 *
一种面向AHRS的改进互补滤波融合算法;杜杉杉等;《国外电子测量技术》;20150315(第03期);全文 *
用蚁群优化算法进行误差修正的手臂姿态跟踪的研究;张立国等;《高技术通讯》;20150415(第04期);全文 *
采用四元数无迹卡尔曼滤波的低成本车载姿态航向系统;刘畅等;《电子测量技术》;20180608(第11期);全文 *

Also Published As

Publication number Publication date
CN112611380A (en) 2021-04-06

Similar Documents

Publication Publication Date Title
CN112611380B (en) Attitude detection method based on multi-IMU fusion and attitude detection device thereof
CN109931926B (en) Unmanned aerial vehicle seamless autonomous navigation method based on station-core coordinate system
CN109238262B (en) Anti-interference method for course attitude calculation and compass calibration
CN109991636A (en) Map constructing method and system based on GPS, IMU and binocular vision
JP5383801B2 (en) Apparatus for generating position and route map data for position and route map display and method for providing the data
Hyyti et al. A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs.
CN109540126A (en) A kind of inertia visual combination air navigation aid based on optical flow method
CN110007354B (en) Device and method for measuring flight parameters of semi-aviation transient electromagnetic receiving coil of unmanned aerial vehicle
CN108458714B (en) Euler angle solving method without gravity acceleration in attitude detection system
CN106814753B (en) Target position correction method, device and system
EP3933166A1 (en) Attitude measurement method
CN108318038A (en) A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method
CN109540135B (en) Method and device for detecting pose and extracting yaw angle of paddy field tractor
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN108731676B (en) Attitude fusion enhanced measurement method and system based on inertial navigation technology
CN103940442A (en) Location method and device adopting accelerating convergence algorithm
CN106403952A (en) Method for measuring combined attitudes of Satcom on the move with low cost
CN110986997A (en) Method and system for improving indoor inertial navigation precision
Liu et al. Tightly coupled modeling and reliable fusion strategy for polarization-based attitude and heading reference system
US10466054B2 (en) Method and system for estimating relative angle between headings
JP3860580B2 (en) Search method for tilt angle of tilt-compensated electronic compass
CN113063416B (en) Robot posture fusion method based on self-adaptive parameter complementary filtering
CN108801253A (en) Robot builds figure positioning system and robot
CN109612464B (en) Multi-algorithm enhanced indoor navigation system and method based on IEZ framework
CN111141283A (en) Method for judging advancing direction through geomagnetic data

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