CN113587925B - Inertial navigation system and full-attitude navigation resolving method and device thereof - Google Patents

Inertial navigation system and full-attitude navigation resolving method and device thereof Download PDF

Info

Publication number
CN113587925B
CN113587925B CN202110808144.XA CN202110808144A CN113587925B CN 113587925 B CN113587925 B CN 113587925B CN 202110808144 A CN202110808144 A CN 202110808144A CN 113587925 B CN113587925 B CN 113587925B
Authority
CN
China
Prior art keywords
carrier
conversion matrix
coordinate system
angle
navigation
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
CN202110808144.XA
Other languages
Chinese (zh)
Other versions
CN113587925A (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.)
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Original Assignee
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
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 Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials filed Critical Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Priority to CN202110808144.XA priority Critical patent/CN113587925B/en
Publication of CN113587925A publication Critical patent/CN113587925A/en
Application granted granted Critical
Publication of CN113587925B publication Critical patent/CN113587925B/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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

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

Abstract

The invention discloses an inertial navigation system and a full-gesture navigation resolving method and device thereof, wherein the inertial navigation system comprises initial alignment and calculation of an initial gesture conversion matrix; updating the speed, the position and the gesture of the carrier to obtain an updated gesture conversion matrix; when the absolute value of the 3 rd row and the 2 nd column elements in the updated posture conversion matrix is larger than a set threshold value, adopting full-posture navigation to calculate a pitch angle, a rolling angle and an azimuth angle, and when the absolute value of the 3 rd row and the 2 nd column elements in the updated posture conversion matrix is smaller than or equal to the set threshold value, adopting a conventional method to navigate and calculate the pitch angle, the rolling angle and the azimuth angle; the carrier attitude angle, the motion speed and the motion displacement under the full attitude are calculated through the triaxial accelerometer increment and the gyro increment, the azimuth angle when the absolute value of the pitch angle is more than 89.2 degrees can be tracked, and the problem that the azimuth angle attitude calculation is inaccurate when the absolute value of the pitch angle is more than 89.2 degrees is solved.

Description

Inertial navigation system and full-attitude navigation resolving method and device thereof
Technical Field
The invention belongs to the technical field of inertial navigation, and particularly relates to an inertial navigation system and a full-attitude navigation resolving method and device thereof.
Background
In the inertial navigation attitude calculation process, when the pitch angle approaches +/-90 degrees (for example, the absolute value of the pitch angle is larger than 89.2 degrees), the calculation of the azimuth angle and the roll angle can generate coupling, so that the azimuth angle and the roll angle which are directly calculated are inconsistent with the actual situation. Although the speed and displacement navigation results are normal at this time, in some systems needing to monitor the navigation posture, the posture early warning system can be false-alarmed due to the fact that the calculated azimuth angle and the calculated roll angle are not in line with the reality.
Disclosure of Invention
The invention aims to provide an inertial navigation system and a full-attitude navigation resolving method and device thereof, which are used for solving the problem that the azimuth angle and the rolling angle which are directly resolved are inconsistent with the actual situation due to coupling when the pitch angle is close to +/-90 degrees.
The invention solves the technical problems by the following technical scheme: a full-attitude navigation resolving method of an inertial navigation system comprises the following steps:
step 1: carrying out initial alignment on the speed, the position and the gesture of a carrier carrying an inertial navigation system, and calculating an initial gesture conversion matrix;
step 2: updating the speed and the position of the carrier at the current moment k according to the initial posture conversion matrix in the step 1;
step 3: updating the posture of the carrier at the current moment k according to the initial posture conversion matrix in the step 1 and the updated speed and position in the step 2 to obtain an updated posture conversion matrix;
step 4: judging whether the absolute value of the 3 rd row and 2 nd column elements in the updated posture conversion matrix in the step 3 is larger than a set threshold value, if so, calculating the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
Figure GDA0004255606470000011
Figure GDA0004255606470000012
Figure GDA0004255606470000013
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,
Figure GDA0004255606470000014
azimuth angle ψ for current time k k Differential of->
Figure GDA0004255606470000015
Posture conversion matrix for current time k>
Figure GDA0004255606470000016
Element of row 3, column 2, of->
Figure GDA0004255606470000017
Posture conversion matrix for current time k>
Figure GDA0004255606470000021
Elements of row 1 and column 3 in ∈1,>
Figure GDA0004255606470000022
posture conversion matrix for current time k>
Figure GDA0004255606470000023
Elements of row 1, column 1, ">
Figure GDA0004255606470000024
For the angular increment output of the Y-axis gyro at the current moment k,/->
Figure GDA0004255606470000025
Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, calculating the pitch angle, the rolling angle and the azimuth angle of the carrier at the current moment k according to a conventional method;
step 5: and (2) repeating the steps (2-4) to calculate the pitch angle, the rolling angle and the azimuth angle of the carrier at the next moment k+1 until the inertial navigation is finished.
Further, in the step 1, the calculation process of the initial posture conversion matrix is as follows:
calculating the local gravity acceleration g and the earth rotation angular velocity omega ie The projection on the navigation coordinate system n is specifically:
g n ＝[0 0 g] T
Figure GDA0004255606470000026
wherein g n For the projection of the local gravitational acceleration g on the navigation coordinate system n, L is the latitude of the current position of the carrier,
Figure GDA0004255606470000027
is the rotation angular velocity omega of the earth ie Projection on a navigation coordinate system n;
calculating the local gravity acceleration g and the earth rotation angular velocity omega ie In the coordinates of the carrierProjection on line b;
based on the local gravitational acceleration g and the rotational angular velocity omega of the earth ie The coordinate conversion relation between the projection on the navigation coordinate system n and the projection on the carrier coordinate system b calculates an initial posture conversion matrix, and the expression of the initial posture conversion matrix is as follows:
Figure GDA0004255606470000028
wherein,,
Figure GDA0004255606470000029
for initial pose transformation matrix g b For the projection of the local gravitational acceleration g on the carrier coordinate system b,
Figure GDA00042556064700000210
is the rotation angular velocity omega of the earth ie Projection on the carrier coordinate system b.
Further, in the step 2, the updated velocity differential equation of the carrier is:
Figure GDA00042556064700000211
Figure GDA00042556064700000212
Figure GDA00042556064700000213
Figure GDA0004255606470000031
wherein R is the radius of the earth,
Figure GDA0004255606470000032
and->
Figure GDA0004255606470000033
Differentiation of east, north and sky speeds, V E(k) 、V N(k) And V U(k) The east speed, the north speed and the sky speed of the carrier at the current moment k are respectively L (k-1) And h (k-1) Respectively the latitude and the height f of the carrier at the last moment k-1 E(k) 、f N(k) And f U(k) The east-to-north and the heaven-to-earth forces, ω, of the carrier at the current moment k, respectively ie The rotation angular velocity of the earth, g is the local gravity acceleration; a, a x(k) 、a y(k) 、a z(k) The output of the triaxial accelerometer in X direction, Y direction and Z direction at the current moment k is respectively; />
Figure GDA0004255606470000034
The gesture conversion matrix is the gesture conversion matrix at the current moment;
the updated position differential equation of the carrier is:
Figure GDA0004255606470000035
Figure GDA0004255606470000036
Figure GDA0004255606470000037
wherein,,
Figure GDA0004255606470000038
for the longitude lambda of the carrier at the current moment k (k) Differential of->
Figure GDA0004255606470000039
And->
Figure GDA00042556064700000310
The latitude L of the carrier at the current moment k (k) And height h (k) Is a derivative of (a).
Further, in the step 3, the updated differential equation of the posture transformation matrix is:
Figure GDA00042556064700000311
Figure GDA00042556064700000312
Figure GDA00042556064700000313
Figure GDA00042556064700000314
wherein,,
Figure GDA00042556064700000315
for the posture conversion matrix of the carrier coordinate system b relative to the navigation coordinate system n at the current moment k, +.>
Figure GDA00042556064700000316
For the gesture conversion matrix->
Figure GDA00042556064700000317
Differential of->
Figure GDA00042556064700000318
For the projection vector of the angular velocity output by the current moment k carrier coordinate system b relative to the inertial coordinate system i on the carrier coordinate system b,/>
Figure GDA00042556064700000319
The angular velocity output for the current moment k navigation coordinate system n relative to the inertial coordinate system i is derivedProjection vector on the aerial coordinate system n, +.>
Figure GDA0004255606470000041
For projection vector +.>
Figure GDA0004255606470000042
Is an antisymmetric matrix of>
Figure GDA0004255606470000043
For the current moment k the earth rotation angular velocity omega ie Projection on navigation coordinate system n, +.>
Figure GDA0004255606470000044
For the projection vector of the angular velocity output by the navigation coordinate system n relative to the geographic system e at the current moment k on the navigation coordinate system n, L (k-1) And h (k-1) The latitude and the height of the carrier at the last moment k-1 are respectively V E(k-1) 、V N(k-1) And V U(k-1) The east direction speed, the north direction speed and the heaven direction speed of the carrier at the last moment k-1 are respectively R M 、R N Is the radius of curvature of the earth.
Further, in the step 4, when the absolute value of the 3 rd row and the 2 nd column elements in the posture conversion matrix at the current time k is greater than the set threshold value and the absolute value of the pitch angle is greater than 89.2 °, the calculation formula of the azimuth angle at the current time k is:
Figure GDA0004255606470000045
wherein, psi is k Azimuth angle, ψ ', of current time k calculated for step 4' k An azimuth angle, a, of the current moment k when the absolute value of the pitch angle is larger than 89.2 DEG z(k) For the output of the Z-axis accelerometer at the current time k, a z(k-1) The output of the Z-axis accelerometer at the last time k-1.
Further, in the step 4, formulas for calculating the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to a conventional method are respectively as follows:
Figure GDA0004255606470000046
Figure GDA0004255606470000047
Figure GDA0004255606470000048
wherein,,
Figure GDA0004255606470000049
posture conversion matrix for current time k>
Figure GDA00042556064700000410
Element of row 3, column 1, respectively->
Figure GDA00042556064700000411
Posture conversion matrix for current time k>
Figure GDA00042556064700000412
Element of row 3, column 3, of->
Figure GDA00042556064700000413
Posture conversion matrix for current time k>
Figure GDA00042556064700000414
Elements of row 1, column 2, ">
Figure GDA00042556064700000415
Posture conversion matrix for current time k>
Figure GDA00042556064700000416
The elements of row 2 and column 2 in (a).
The invention also provides a full-attitude navigation solution device of the inertial navigation system, comprising:
the initial alignment unit is used for carrying out initial alignment on the speed, the position and the gesture of the carrier carrying the inertial navigation system and calculating an initial gesture conversion matrix;
a speed and position updating unit, configured to update the speed and position of the carrier at the current time k according to the initial posture conversion matrix;
the gesture updating unit is used for updating the gesture of the carrier at the current moment k according to the initial gesture conversion matrix, the updated speed and the updated position to obtain an updated gesture conversion matrix;
the resolving unit is used for judging whether the absolute value of the 3 rd row and the 2 nd column elements in the updated gesture conversion matrix is larger than a set threshold value, if so, resolving the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
Figure GDA0004255606470000051
Figure GDA0004255606470000052
Figure GDA0004255606470000053
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,
Figure GDA0004255606470000054
azimuth angle ψ for current time k k Differential of->
Figure GDA0004255606470000055
Posture conversion matrix for current time k>
Figure GDA0004255606470000056
Element of row 3, column 2, of->
Figure GDA0004255606470000057
Posture conversion matrix for current time k>
Figure GDA0004255606470000058
Elements of row 1 and column 3 in ∈1,>
Figure GDA0004255606470000059
posture conversion matrix for current time k>
Figure GDA00042556064700000510
Elements of row 1, column 1, ">
Figure GDA00042556064700000511
For the angular increment output of the Y-axis gyro at the current moment k,/->
Figure GDA00042556064700000512
Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k are calculated according to a conventional method.
The invention also provides an inertial navigation system, which comprises the full-attitude navigation solving device.
Advantageous effects
Compared with the prior art, the invention has the advantages that:
according to the inertial navigation system, the attitude angle, the movement speed and the movement displacement under the carrier inertial navigation algorithm under the full attitude are calculated through the triaxial accelerometer increment and the gyro increment, and the azimuth angle when the absolute value of the pitch angle is larger than 89.2 degrees can be tracked, so that the problem that in the traditional attitude calculation, the solution error is overlarge due to the fact that the denominator of a rolling angle calculation formula approaches 0 when the absolute value of the pitch angle approaches 90 degrees, and the problem that the attitude angle (or azimuth angle) calculation is inconsistent with the actual situation is solved; when the absolute value of the pitch angle is greater than 89.2 degrees, the pitch angle is calculated by means of the output of the actual gyro rate, a false alarm phenomenon cannot occur, and the problem that the azimuth attitude calculation is inaccurate when the absolute value of the pitch angle is greater than 89.2 degrees can be solved.
Drawings
In order to more clearly illustrate the technical solutions of the present invention, the drawings that are needed in the description of the embodiments will be briefly described below, it being obvious that the drawing in the description below is only one embodiment of the present invention, and that other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a full-pose navigation solution method in an embodiment of the invention;
FIG. 2 is a software interface diagram of a full-pose navigation solution in an embodiment of the invention.
Detailed Description
The following description of the embodiments of the present invention will be made more apparent and fully by reference to the accompanying drawings, in which it is shown, however, only some, but not all embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The technical scheme of the present application is described in detail below with specific examples. The following embodiments may be combined with each other, and some embodiments may not be repeated for the same or similar concepts or processes.
As shown in fig. 1, the full-pose navigation resolving method of the inertial navigation system provided in the present embodiment includes the following steps:
step 1: and (3) carrying out initial alignment on the speed, the position and the posture of the carrier carrying the inertial navigation system, and calculating an initial posture conversion matrix.
Inputting three-axis accelerometer increment data and gyro increment data, wherein the data format requires a pure data text stored in a 6-column form, the first to three columns are respectively XYZ-axis acceleration increment data, and the fourth to six columns are respectively XYZ-axis gyro increment data; the local gravity acceleration g is input, the local longitude lambda (in degrees) is input, and the local latitude L (in degrees) is input; inputting a navigation start time, generally defaulting to 300s, the first 300s must be static data for initial alignment; the navigation solution result is output from 301 s.
The initial alignment is carried out by using static data of 300s before the carrier moves, and the coordinate conversion relation of the space vector alpha between the navigation coordinate system n and the carrier coordinate system b is as follows:
Figure GDA0004255606470000061
wherein alpha is n 、α b The space vector alpha is respectively represented by coordinates of a navigation coordinate system n and a carrier coordinate system b,
Figure GDA0004255606470000062
is a gesture conversion matrix from the carrier coordinate system b to the navigation coordinate system n.
In the present embodiment, the local gravitational acceleration g and the earth rotation angular velocity ω are used ie The initial posture conversion matrix is calculated according to the coordinate conversion relation between the projection on the navigation coordinate system n and the projection on the carrier coordinate system b, and the specific calculation process is as follows:
1.1 calculating the local gravity acceleration g and the Earth rotation angular velocity omega ie The projection on the navigation coordinate system n is specifically:
g n ＝[0 0 g] T (2)
Figure GDA0004255606470000063
wherein g n For the projection of the local gravitational acceleration g on the navigation coordinate system n, L is the latitude of the current position of the carrier,
Figure GDA0004255606470000071
is the rotation angular velocity omega of the earth ie Projection on the navigation coordinate system n.
1.2 calculating the local gravity acceleration g and the Earth rotation angular velocity omega ie Projection g on carrier coordinate system b b And
Figure GDA0004255606470000072
wherein g b For the mean value of the tri-axial accelerometer output in 300s static data,/o>
Figure GDA0004255606470000073
Is the average value of the gyroscope output in the 300s static data.
Step 1.1 and step 1.2 may be performed simultaneously, without any sequence.
1.3 according to the local gravity acceleration g and the earth rotation angular velocity omega ie The coordinate conversion relation between the projection on the navigation coordinate system n and the projection on the carrier coordinate system b (i.e., expression (1)) calculates an initial posture conversion matrix whose expression is:
Figure GDA0004255606470000074
wherein,,
Figure GDA0004255606470000075
for initial pose transformation matrix g b For the projection of the local gravitational acceleration g on the carrier coordinate system b,
Figure GDA0004255606470000076
is the rotation angular velocity omega of the earth ie Projection on the carrier coordinate system b.
Step 2: and updating the speed and the position of the carrier at the current moment k according to the initial posture conversion matrix in the step 1.
After the initial gesture conversion matrix is obtained, the speed and the position at any moment can be updated. In this embodiment, the navigation coordinate system is the northeast coordinate system, and the updated velocity differential equation of the carrier at the current time k is:
Figure GDA0004255606470000077
wherein R is the radius of the earth,
Figure GDA0004255606470000078
and->
Figure GDA0004255606470000079
Respectively east velocity V E(k) North velocity V N(k) Sum of the upward velocity V U(k) Is the derivative of V E(k) 、V N(k) And V U(k) The east speed, the north speed and the sky speed of the carrier at the current moment k are respectively L (k-1) And h (k-1) Respectively the latitude and the height f of the carrier at the last moment k-1 E(k) 、f N(k) And f U(k) The east-to-north and the heaven-to-earth forces, ω, of the carrier at the current moment k, respectively ie The rotation angular velocity of the earth, g is the local gravitational acceleration.
The east-direction specific force, the north-direction specific force and the sky-direction specific force at the current moment k are obtained by an output left-hand gesture conversion matrix of the triaxial accelerometer, and specifically are as follows:
Figure GDA0004255606470000081
wherein a is x(k) 、a y(k) 、a z(k) The output of the triaxial accelerometer in X direction, Y direction and Z direction at the current moment k is respectively;
Figure GDA0004255606470000082
the gesture conversion matrix is the gesture conversion matrix at the current moment.
Solving the velocity differential equation (5) to obtain the east velocity V of the carrier at the current moment k E(k) North velocity V N(k) Sum of the upward velocity V U(k) . The position update at the current instant k is based on a velocity update, in particular：
Figure GDA0004255606470000083
Wherein,,
Figure GDA0004255606470000084
for the longitude lambda of the carrier at the current moment k (k) Differential of->
Figure GDA0004255606470000085
And->
Figure GDA0004255606470000086
The latitude L of the carrier at the current moment k (k) And height h (k) Is a derivative of (a). Solving the position differential equation (7) to obtain the longitude lambda of the carrier at the current moment k (k) Latitude L (k) And height h (k) 。
Step 3: and updating the posture of the carrier at the current moment k according to the initial posture conversion matrix in the step 1 and the updated speed and position in the step 2 to obtain an updated posture conversion matrix.
After the initial attitude conversion matrix and the updated speed and position are obtained, the attitude conversion matrix can be updated according to the output of the gyroscope measured by each frame, and the differential equation of the updated attitude conversion matrix is as follows:
Figure GDA0004255606470000087
wherein,,
Figure GDA0004255606470000088
for the posture conversion matrix of the carrier coordinate system b relative to the navigation coordinate system n at the current moment k, +.>
Figure GDA0004255606470000089
For the gesture conversion matrix->
Figure GDA00042556064700000810
Differential of->
Figure GDA00042556064700000811
For the projection vector of the angular velocity output by the current moment k carrier coordinate system b relative to the navigation coordinate system n on the carrier coordinate system b,/>
Figure GDA00042556064700000812
X is projection vector +.>
Figure GDA00042556064700000813
Is an anti-symmetric matrix of (a).
Since the gyroscope outputs the angular velocity of the carrier coordinate system b relative to the inertial coordinate system i
Figure GDA00042556064700000814
And angular velocity
Figure GDA00042556064700000815
Cannot be obtained by direct measurement, and the differential equation of formula (8) needs to be transformed as follows:
Figure GDA0004255606470000091
wherein,,
Figure GDA0004255606470000092
for the projection vector of the angular velocity output by the current moment k carrier coordinate system b relative to the inertial coordinate system i on the carrier coordinate system b,/>
Figure GDA0004255606470000093
For the projection vector of the angular velocity output by the navigation coordinate system n relative to the inertial coordinate system i at the current moment k on the carrier coordinate system b,/>
Figure GDA0004255606470000094
Navigating the coordinate system n relative to inertia for the current moment kProjection vector of angular velocity output by coordinate system i on navigation coordinate system n, +.>
Figure GDA0004255606470000095
X is projection vector +.>
Figure GDA0004255606470000096
Is an antisymmetric matrix of>
Figure GDA0004255606470000097
For the gesture conversion matrix of the navigation coordinate system n relative to the carrier coordinate system b at the current moment k, it is known from its definition, +.>
Figure GDA0004255606470000098
Is->
Figure GDA0004255606470000099
Transposed matrix of (i.e.)>
Figure GDA00042556064700000910
Projection vector
Figure GDA00042556064700000911
I.e. the rotation of the navigation coordinate system n relative to the inertial coordinate system i, comprising two parts: navigation coordinate system n rotation caused by earth rotation>
Figure GDA00042556064700000912
And the inertial navigation system moves the navigation coordinate system n rotation caused by the surface curvature of the earth near the surface of the earth +.>
Figure GDA00042556064700000913
There is->
Figure GDA00042556064700000914
Figure GDA00042556064700000915
Figure GDA00042556064700000916
Wherein,,
Figure GDA00042556064700000917
for the current moment k the earth rotation angular velocity omega ie Projection vector on navigation coordinate system n, < >>
Figure GDA00042556064700000918
A projection vector of the angular velocity output by the navigation coordinate system n relative to the geographic system e at the current moment k on the navigation coordinate system n; v (V) E(k-1) 、V N(k-1) And V U(k-1) The east speed, the north speed and the sky speed of the carrier at the last moment k-1 are updated by the accelerometer output at the last moment k-1; r is R M 、R N Is the radius of curvature of the earth.
The attitude transformation matrix at the current moment k can be obtained by solving the differential equation of (9)
Figure GDA00042556064700000919
In this embodiment, navigation gesture calculation is performed based on a northeast navigation coordinate system, and a gesture conversion matrix is defined as follows:
Figure GDA00042556064700000920
according to the definition of the gesture conversion matrix, the specific value of each element in the gesture conversion matrix can be obtained by combining the solving result of the differential equation (9).
Step 4: judging whether the absolute value of the 3 rd row and 2 nd column elements in the updated posture conversion matrix in the step 3 is larger than a set threshold value, if so, calculating the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formulas (12) and (13), wherein the specific formulas are as follows:
Figure GDA0004255606470000101
Figure GDA0004255606470000102
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,
Figure GDA0004255606470000103
azimuth angle ψ for current time k k Differential of->
Figure GDA0004255606470000104
Posture conversion matrix for current time k>
Figure GDA0004255606470000105
Element of row 3, column 2, of->
Figure GDA0004255606470000106
Posture conversion matrix for current time k>
Figure GDA0004255606470000107
Elements of row 1 and column 3 in ∈1,>
Figure GDA0004255606470000108
posture conversion matrix for current time k>
Figure GDA0004255606470000109
Elements of row 1, column 1, ">
Figure GDA00042556064700001010
For the angular increment output of the Y-axis gyro at the current moment k,/->
Figure GDA00042556064700001011
For Z-axis gyro at presentAnd outputting the angle increment of the etching k.
Otherwise, the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k are calculated according to a conventional method, and the specific formula is as follows:
Figure GDA00042556064700001012
wherein,,
Figure GDA00042556064700001013
posture conversion matrix for current time k>
Figure GDA00042556064700001014
Element of row 3, column 1, respectively->
Figure GDA00042556064700001015
Posture conversion matrix for current time k>
Figure GDA00042556064700001016
Element of row 3, column 3, of->
Figure GDA00042556064700001017
Posture conversion matrix for current time k>
Figure GDA00042556064700001018
Elements of row 1, column 2, ">
Figure GDA00042556064700001019
Posture conversion matrix for current time k>
Figure GDA00042556064700001020
The elements of row 2 and column 2 in (a).
That is, when the calculated pitch angle does not fall within the threshold range of ±90°, the pitch angle, roll angle and azimuth angle are calculated according to equation (14), and when the calculated pitch angle approaches the threshold range of ±90°, the pitch angle, roll angle and azimuth angle are calculated according to equations (12) and (13).
According to engineering experience, in the embodiment, the threshold value is set to be 0.9999, namely
Figure GDA00042556064700001021
In this case, the pitch angle, roll angle and azimuth angle are calculated according to equations (12) and (13), and the azimuth angle can be calculated by solving the differential equation of equation (13).
In the process of gesture resolving, attention is required to be paid to the value range of resolving angles, theta k The value range is [ -90 degrees, 90 degrees), gamma k The value range of (a) is [ -180 degrees, 180 degrees), and (b) phi k Is [0 ], 360 °). When the carrier moves around the rolling axis or azimuth axis, the attitude resolution cannot track the azimuth angle of the carrier in real time. In this embodiment, under the initial attitude, the rotation of the carrier X axis corresponds to the change of the pitch angle, the rotation of the carrier Y axis corresponds to the change of the roll angle, and the rotation of the carrier Z axis corresponds to the change of the azimuth angle, and if the absolute value of the pitch angle is greater than 89.2 °, the change of the azimuth angle is affected by the change of angles in both the axial directions of the Y axis and the Z axis, and the specific calculation is as shown in formula (13).
It is noted that when the pitch angle is increased from 89.5 to 90 and then further increased by 0.5, the output of the pitch angle is still 89.5 due to the range limit of the pitch angle, but the azimuth angle is changed by 180. Therefore, in the azimuth angle solution when the absolute value of the pitch angle is greater than 89.2 °, a judgment formula (15) is added, namely
Figure GDA0004255606470000111
And when the absolute value of the pitch angle is larger than 89.2 degrees, the calculation formula of the azimuth angle at the current moment k is as follows:
Figure GDA0004255606470000112
wherein, psi is k Azimuth angle, ψ ', of current time k calculated for equation (13)' k Azimuth angle, a, of current time k calculated for equation (15) z(k) For the output of the Z-axis accelerometer at the current time k, a z(k-1) On Z-axis accelerometerAn output at time k-1.
Step 5: and (2) repeating the steps (2-4), and calculating the pitch angle, the rolling angle and the azimuth angle of the carrier at the next moment k+1 until the inertial navigation is finished or the data are output by the accelerometer and the gyroscope at the last frame.
According to the full-attitude navigation resolving method of the inertial navigation system, after the initial attitude conversion matrix of a moving body (carrier for short) carrying the inertial navigation system is obtained through initial alignment, the attitude conversion matrix at each moment is updated through the output of the triaxial accelerometer and the output of the gyroscope, so that the full-attitude navigation resolving problem can be solved pertinently, the practicability is high, the problem that the azimuth attitude resolving is inaccurate when the absolute value of the pitch angle is larger than 89.2 degrees is solved, and the problem that the attitude monitoring software is misreported is avoided.
The embodiment also provides a full-attitude navigation solution device of an inertial navigation system, including:
and the initial alignment unit is used for carrying out initial alignment on the speed, the position and the gesture of the carrier carrying the inertial navigation system, and calculating an initial gesture conversion matrix as shown in a formula (4).
Inputting three-axis accelerometer increment data and gyro increment data, wherein the data format requires a pure data text stored in a 6-column form, the first to three columns are respectively XYZ-axis acceleration increment data, and the fourth to six columns are respectively XYZ-axis gyro increment data; the local gravity acceleration g is input, the local longitude lambda (in degrees) is input, and the local latitude L (in degrees) is input; inputting a navigation start time, generally defaulting to 300s, the first 300s must be static data for initial alignment; the navigation solution result is output from 301 s. As shown in fig. 2, the tri-axial accelerometer incremental data and gyro incremental data required for navigation solution can be input by clicking "import navigation data".
And the speed and position updating unit is used for updating the speed and position of the carrier at the current moment k according to the initial posture conversion matrix, as shown in formulas (5) - (7).
And the gesture updating unit is used for updating the gesture of the carrier at the current moment k according to the initial gesture conversion matrix, the updated speed and the updated position to obtain an updated gesture conversion matrix, as shown in a formula (9).
The resolving unit is used for judging whether the absolute value of the 3 rd row and the 2 nd column elements in the updated gesture conversion matrix is larger than a set threshold value, if so, resolving the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
Figure GDA0004255606470000121
Figure GDA0004255606470000122
Figure GDA0004255606470000123
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,
Figure GDA0004255606470000124
azimuth angle ψ for current time k k Differential of->
Figure GDA0004255606470000125
Posture conversion matrix for current time k>
Figure GDA0004255606470000126
Element of row 3, column 2, of->
Figure GDA0004255606470000127
Posture conversion matrix for current time k>
Figure GDA0004255606470000128
Elements of row 1 and column 3 in ∈1,>
Figure GDA0004255606470000129
posture conversion matrix for current time k>
Figure GDA00042556064700001210
Elements of row 1, column 1, ">
Figure GDA00042556064700001211
For the angular increment output of the Y-axis gyro at the current moment k,/->
Figure GDA00042556064700001212
Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k are calculated according to a conventional method, as shown in a formula (14).
As shown in fig. 2, clicking the "navigation calculation" and waiting for popping up the "navigation calculation completed" window, a file named result. Txt can be found in the navigation data folder, that is, the navigation calculation result file is composed of nine columns of data, and the first column to the third column are respectively the navigation calculation pitch angle, the rolling angle and the azimuth angle; the fourth column to the sixth column are respectively an east speed error, a north speed error and an sky speed error which are calculated by navigation; the seventh to ninth columns are the east displacement error, the north displacement error and the sky displacement error of the navigation solution, respectively.
The foregoing disclosure is merely illustrative of specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art will readily recognize that changes and modifications are possible within the scope of the present invention.

Claims (8)

1. The full-attitude navigation resolving method of the inertial navigation system is characterized by comprising the following steps of:
step 1: carrying out initial alignment on the speed, the position and the gesture of a carrier carrying an inertial navigation system, and calculating an initial gesture conversion matrix;
step 2: updating the speed and the position of the carrier at the current moment k according to the initial posture conversion matrix in the step 1;
step 3: updating the posture of the carrier at the current moment k according to the initial posture conversion matrix in the step 1 and the updated speed and position in the step 2 to obtain an updated posture conversion matrix;
step 4: judging whether the absolute value of the 3 rd row and 2 nd column elements in the updated posture conversion matrix in the step 3 is larger than a set threshold value, if so, calculating the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
Figure FDA0004255606460000011
Figure FDA0004255606460000012
Figure FDA0004255606460000013
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,
Figure FDA0004255606460000014
azimuth angle ψ for current time k k Differential of->
Figure FDA0004255606460000015
Posture conversion matrix for current time k>
Figure FDA0004255606460000016
Element of row 3, column 2, of->
Figure FDA0004255606460000017
For the current time kIs>
Figure FDA0004255606460000018
Elements of row 1 and column 3 in ∈1,>
Figure FDA0004255606460000019
posture conversion matrix for current time k>
Figure FDA00042556064600000110
Elements of row 1, column 1, ">
Figure FDA00042556064600000111
For the angular increment output of the Y-axis gyro at the current moment k,/->
Figure FDA00042556064600000112
Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, calculating the pitch angle, the rolling angle and the azimuth angle of the carrier at the current moment k according to a conventional method;
step 5: and (2) repeating the steps (2-4) to calculate the pitch angle, the rolling angle and the azimuth angle of the carrier at the next moment k+1 until the inertial navigation is finished.
2. The full-pose navigation solution method according to claim 1, wherein in the step 1, the calculation process of the initial pose conversion matrix is:
calculating the local gravity acceleration g and the earth rotation angular velocity omega ie The projection on the navigation coordinate system n is specifically:
g n ＝[00g] T
Figure FDA00042556064600000113
wherein g n For the projection of the local gravitational acceleration g on the navigation coordinate system n, L is the latitude of the current position of the carrierThe degree of the heat dissipation,
Figure FDA0004255606460000021
is the rotation angular velocity omega of the earth ie Projection on a navigation coordinate system n;
calculating the local gravity acceleration g and the earth rotation angular velocity omega ie Projection on the carrier coordinate system b;
based on the local gravitational acceleration g and the rotational angular velocity omega of the earth ie The coordinate conversion relation between the projection on the navigation coordinate system n and the projection on the carrier coordinate system b calculates an initial posture conversion matrix, and the expression of the initial posture conversion matrix is as follows:
Figure FDA0004255606460000022
wherein,,
Figure FDA0004255606460000023
for initial pose transformation matrix g b For the projection of the local gravitational acceleration g on the carrier coordinate system b, < >>
Figure FDA0004255606460000024
Is the rotation angular velocity omega of the earth ie Projection on the carrier coordinate system b.
3. The full-pose navigation solution method according to claim 1, wherein in the step 2, the updated velocity differential equation of the carrier is:
Figure FDA0004255606460000025
Figure FDA0004255606460000026
Figure FDA0004255606460000027
Figure FDA0004255606460000028
wherein R is the radius of the earth,
Figure FDA0004255606460000029
and->
Figure FDA00042556064600000210
Differentiation of east, north and sky speeds, V E(k) 、V N(k) And V U(k) The east speed, the north speed and the sky speed of the carrier at the current moment k are respectively L (k-1) And h (k-1) Respectively the latitude and the height f of the carrier at the last moment k-1 E(k) 、f N(k) And f U(k) The east-to-north and the heaven-to-earth forces, ω, of the carrier at the current moment k, respectively ie The rotation angular velocity of the earth, g is the local gravity acceleration; a, a x(k) 、a y(k) 、a z(k) The output of the triaxial accelerometer in X direction, Y direction and Z direction at the current moment k is respectively; />
Figure FDA00042556064600000211
The gesture conversion matrix is the gesture conversion matrix at the current moment;
the updated position differential equation of the carrier is:
Figure FDA0004255606460000031
Figure FDA0004255606460000032
Figure FDA0004255606460000033
wherein,,
Figure FDA0004255606460000034
for the longitude lambda of the carrier at the current moment k (k) Differential of->
Figure FDA0004255606460000035
And->
Figure FDA0004255606460000036
The latitude L of the carrier at the current moment k (k) And height h (k) Is a derivative of (a).
4. The full-pose navigation solution method according to claim 1, wherein in the step 3, the updated pose conversion matrix differential equation is:
Figure FDA0004255606460000037
Figure FDA0004255606460000038
Figure FDA0004255606460000039
Figure FDA00042556064600000310
wherein,,
Figure FDA00042556064600000311
for the posture conversion matrix of the carrier coordinate system b relative to the navigation coordinate system n at the current moment k, +.>
Figure FDA00042556064600000312
For the gesture conversion matrix->
Figure FDA00042556064600000313
Differential of->
Figure FDA00042556064600000314
For the projection vector of the angular velocity output by the current moment k carrier coordinate system b relative to the inertial coordinate system i on the carrier coordinate system b,/>
Figure FDA00042556064600000315
For the projection vector of the angular velocity output by the navigation coordinate system n relative to the inertial coordinate system i at the current moment k on the navigation coordinate system n,/>
Figure FDA00042556064600000316
For projection vector +.>
Figure FDA00042556064600000317
Is an antisymmetric matrix of>
Figure FDA00042556064600000318
For the current moment k the earth rotation angular velocity omega ie Projection on navigation coordinate system n, +.>
Figure FDA00042556064600000319
For the projection vector of the angular velocity output by the navigation coordinate system n relative to the geographic system e at the current moment k on the navigation coordinate system n, L (k-1) And h (k-1) The latitude and the height of the carrier at the last moment k-1 are respectively V E(k-1) 、V N(k-1) And V U(k-1) The east direction speed, the north direction speed and the heaven direction speed of the carrier at the last moment k-1 are respectively R M 、R N Is the radius of curvature of the earth.
5. The full-pose navigation solution according to any one of claims 1 to 4, wherein in the step 4, when the absolute value of the 3 rd row and 2 nd column elements in the pose conversion matrix at the current time k is greater than a set threshold value and the absolute value of the pitch angle is greater than 89.2 °, the calculation formula of the azimuth angle at the current time k is:
Figure FDA0004255606460000041
wherein, psi is k Azimuth angle, ψ ', of current time k calculated for step 4' k An azimuth angle, a, of the current moment k when the absolute value of the pitch angle is larger than 89.2 DEG z(k) For the output of the Z-axis accelerometer at the current time k, a z(k-1) The output of the Z-axis accelerometer at the last time k-1.
6. The full-pose navigation solution according to any one of claims 1 to 4, wherein in the step 4, formulas for solving the pitch angle, the roll angle and the azimuth angle of the carrier at the current time k according to a conventional method are respectively:
Figure FDA0004255606460000042
Figure FDA0004255606460000043
Figure FDA0004255606460000044
wherein,,
Figure FDA0004255606460000045
for the current time kIs>
Figure FDA0004255606460000046
Element of row 3, column 1, respectively->
Figure FDA0004255606460000047
Posture conversion matrix for current time k>
Figure FDA0004255606460000048
Element of row 3, column 3, of->
Figure FDA0004255606460000049
Posture conversion matrix for current time k>
Figure FDA00042556064600000410
Elements of row 1, column 2, ">
Figure FDA00042556064600000411
Posture conversion matrix for current time k>
Figure FDA00042556064600000412
The elements of row 2 and column 2 in (a).
7. A full-pose navigation solution device of an inertial navigation system, comprising:
the initial alignment unit is used for carrying out initial alignment on the speed, the position and the gesture of the carrier carrying the inertial navigation system and calculating an initial gesture conversion matrix;
a speed and position updating unit, configured to update the speed and position of the carrier at the current time k according to the initial posture conversion matrix;
the gesture updating unit is used for updating the gesture of the carrier at the current moment k according to the initial gesture conversion matrix, the updated speed and the updated position to obtain an updated gesture conversion matrix;
the resolving unit is used for judging whether the absolute value of the 3 rd row and the 2 nd column elements in the updated gesture conversion matrix is larger than a set threshold value, if so, resolving the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k according to the following formula:
Figure FDA00042556064600000413
Figure FDA00042556064600000414
Figure FDA00042556064600000415
wherein θ k For pitch angle at current time k, γ k For the roll angle at the current instant k,
Figure FDA0004255606460000051
azimuth angle ψ for current time k k Differential of->
Figure FDA0004255606460000052
Posture conversion matrix for current time k>
Figure FDA0004255606460000053
Element of row 3, column 2, of->
Figure FDA0004255606460000054
Posture conversion matrix for current time k>
Figure FDA0004255606460000055
Elements of row 1 and column 3 in ∈1,>
Figure FDA0004255606460000056
posture conversion matrix for current time k>
Figure FDA0004255606460000057
Elements of row 1, column 1, ">
Figure FDA0004255606460000058
For the angular increment output of the Y-axis gyro at the current moment k,/->
Figure FDA0004255606460000059
Outputting the angle increment of the Z-axis gyroscope at the current moment k;
otherwise, the pitch angle, the roll angle and the azimuth angle of the carrier at the current moment k are calculated according to a conventional method.
8. An inertial navigation system, characterized by: comprising the full-pose navigation solution according to claim 7.
CN202110808144.XA 2021-07-16 2021-07-16 Inertial navigation system and full-attitude navigation resolving method and device thereof Active CN113587925B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110808144.XA CN113587925B (en) 2021-07-16 2021-07-16 Inertial navigation system and full-attitude navigation resolving method and device thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110808144.XA CN113587925B (en) 2021-07-16 2021-07-16 Inertial navigation system and full-attitude navigation resolving method and device thereof

Publications (2)

Publication Number Publication Date
CN113587925A CN113587925A (en) 2021-11-02
CN113587925B true CN113587925B (en) 2023-07-07

Family

ID=78247834

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110808144.XA Active CN113587925B (en) 2021-07-16 2021-07-16 Inertial navigation system and full-attitude navigation resolving method and device thereof

Country Status (1)

Country Link
CN (1) CN113587925B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114485641B (en) * 2022-01-24 2024-03-26 武汉梦芯科技有限公司 Attitude calculation method and device based on inertial navigation device navigation azimuth fusion
CN115560756B (en) * 2022-08-26 2023-07-04 北京开拓航宇导控科技有限公司 Miniature self-seeking missile strapdown navigation method under emission coordinate system
CN116048146B (en) * 2023-03-31 2023-06-13 中国船舶集团有限公司第七〇七研究所 Angular velocity smooth control method for inertial navigation of rotary fiber-optic gyroscope
CN117091596B (en) * 2023-10-09 2024-06-21 腾讯科技（深圳）有限公司 Gesture information acquisition method and related equipment

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104677358A (en) * 2013-11-29 2015-06-03 哈尔滨恒誉名翔科技有限公司 Micro-strapdown attitude heading reference system all-attitude controller
CN105203098B (en) * 2015-10-13 2018-10-02 上海华测导航技术股份有限公司 Agricultural machinery all-attitude angle update method based on nine axis MEMS sensors
CN106153073B (en) * 2016-06-21 2018-10-12 东南大学 A kind of nonlinear initial alignment method of full posture Strapdown Inertial Navigation System
JP6922641B2 (en) * 2017-10-13 2021-08-18 株式会社Ｊｖｃケンウッド Angular velocity derivation device and angular velocity derivation method
CN109115212B (en) * 2018-10-30 2022-04-12 中国船舶重工集团公司第七0七研究所 Method for extracting full-range attitude angle of inertial navigation system
CN110132269A (en) * 2019-06-10 2019-08-16 西北工业大学 A kind of guided missile high-precision Vertical Launch initial attitude acquisition methods
CN111780749B (en) * 2020-05-26 2022-06-03 北京航天控制仪器研究所 Attitude control method for full-attitude inertial navigation of orbital transfer maneuvering aircraft
CN111721291B (en) * 2020-07-17 2021-12-07 河北斐然科技有限公司 Engineering algorithm for strapdown inertial navigation under launching system
CN112378400A (en) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 Dual-antenna GNSS assisted strapdown inertial navigation integrated navigation method

Also Published As

Publication number Publication date
CN113587925A (en) 2021-11-02

Similar Documents

Publication Publication Date Title
CN113587925B (en) Inertial navigation system and full-attitude navigation resolving method and device thereof
KR102454882B1 (en) Dead reckoning method and apparatus for vehicle, device and storage medium
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN105910606A (en) Direction adjustment method based on angular velocity difference
CN106767767A (en) A kind of micro-nano multimode star sensor system and its data fusion method
CN111189442B (en) CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method
CN110174121A (en) A kind of aviation attitude system attitude algorithm method based on earth&#39;s magnetic field adaptive correction
CN114485641A (en) Attitude calculation method and device based on inertial navigation and satellite navigation azimuth fusion
CN106370178A (en) Mobile terminal equipment attitude measurement method and mobile terminal equipment attitude measurement apparatus
CN112880669A (en) Spacecraft starlight refraction and uniaxial rotation modulation inertia combined navigation method
CN108871323B (en) High-precision navigation method of low-cost inertial sensor in locomotive environment
CN108871319B (en) Attitude calculation method based on earth gravity field and earth magnetic field sequential correction
CN115855048A (en) Multi-sensor fusion pose estimation method
CN104121930A (en) Compensation method for MEMS (Micro-electromechanical Systems) gyroscopic drifting errors based on accelerometer coupling
CN109029467A (en) A kind of spacecraft high-precision angular movement measurement method based on rotator type gyro biorthogonal configuration
CN115655271B (en) Large-range attitude angle extraction method under dynamic condition
CN111141283A (en) Method for judging advancing direction through geomagnetic data
CN113447024B (en) Inertial navigation attitude angle resolving method and system based on extended Krafft angle
CN113447025B (en) Inertial navigation high-precision attitude angle resolving method and system based on Krafft angle
CN111649738B (en) Method for calculating initial attitude of accelerometer under microgravity field
CN114526729A (en) Course optimization method of MEMS inertial positioning system based on redundancy technology
WO2021012635A1 (en) Gyroscope information-based inertial navigation method
CN113670297A (en) Off-line positioning method based on MEMS and wheel type odometer
CN113051757B (en) Strapdown inertial navigation generalized PSI angle error model construction method
CN118225086B (en) Method and system for precisely integrating and calculating pose under inertial 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