CN118225086B - Method and system for precisely integrating and calculating pose under inertial system - Google Patents
Method and system for precisely integrating and calculating pose under inertial system Download PDFInfo
- Publication number
- CN118225086B CN118225086B CN202410643551.3A CN202410643551A CN118225086B CN 118225086 B CN118225086 B CN 118225086B CN 202410643551 A CN202410643551 A CN 202410643551A CN 118225086 B CN118225086 B CN 118225086B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- carrier
- inertial
- initial
- under
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 238000004364 calculation method Methods 0.000 claims abstract description 37
- 230000001133 acceleration Effects 0.000 claims abstract description 17
- 239000011159 matrix material Substances 0.000 claims description 48
- 230000009466 transformation Effects 0.000 claims description 35
- 230000008859 change Effects 0.000 claims description 30
- 238000003860 storage Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 230000005484 gravity Effects 0.000 claims description 8
- 206010034719 Personality change Diseases 0.000 claims description 7
- 238000005259 measurement Methods 0.000 claims description 7
- 238000004458 analytical method Methods 0.000 claims description 4
- 230000010354 integration Effects 0.000 description 8
- 238000004891 communication Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 7
- 230000005641 tunneling Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000012937 correction Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000013433 optimization analysis Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
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/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Analysis (AREA)
- Computational Mathematics (AREA)
- Operations Research (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention discloses a method and a system for precisely integrating and calculating pose under an inertial system, wherein the method comprises the following steps: s100, setting a geocentric inertial coordinate system as a navigation resolving coordinate system, and setting the geocentric inertial coordinate system at the initial moment to coincide with a geocentric fixed coordinate system; s200, acquiring initial state information of the carrier including an initial position, an initial speed and an initial posture according to a geocentric inertial coordinate system at an initial moment; s300, measuring acceleration and angular velocity of the carrier at the middle moment by using a gyroscope and an accelerometer, and integrating the acceleration and the angular velocity under a geocentric inertial coordinate system to obtain a coordinate of the carrier at the termination moment; s400, subtracting the motion quantity of the geocentric geodetic fixed coordinate system under the geodetic inertial coordinate system from the coordinate of the carrier under the termination time to obtain new carrier pose information; s500, correcting the new carrier pose information by utilizing an error equation to obtain final carrier pose information. The calculation efficiency is improved, and errors caused by the inertial positioning and attitude determination calculation method are reduced.
Description
Technical Field
The invention belongs to the technical field of mobile measurement, and particularly relates to a method and a system for precisely integrating and calculating pose under an inertial system.
Background
The inertial navigation system is based on Newton mechanics principle, and uses a gyroscope to measure the attitude change information of a carrier relative to an inertial space, and uses an accelerometer to measure the acceleration information of the carrier relative to the inertial space, and by giving an initial state, the system can establish a corresponding attitude matrix according to the output of the gyroscope, and integrate the measured value of the accelerometer under a corresponding coordinate system according to the established attitude matrix, so as to obtain the three-dimensional speed, position and attitude of the carrier. Because the inertial navigation system adopts the way of integrating and resolving to resolve the state information such as the position and the attitude of the carrier, the errors in the inertial system resolving are continuously accumulated, and finally the accuracy and the reliability of the navigation resolving are affected. To improve the resolution accuracy and performance, errors in such hardware itself can be reduced by improving the performance of gyroscopes and accelerometers in inertial navigation systems. However, since the inertial sensor is a very precise instrument, the manufacturing process is extremely complex, and a high cost is required for slightly improving the performance of the inertial instrument. The problem is solved from the theoretical method level, so that the cost is low, and the universality is wide.
The integral operation is a fundamental and indispensable link in the practical application process of the inertial navigation system, and the characteristics and the performance of the integral calculation model greatly influence the accuracy and the correctness of the position and the posture. The traditional method is based on a geocentric fixed coordinate system, the corresponding inertial navigation updating algorithm is essentially bound to the curved earth surface, and the linear distance is replaced by an angle at each moment of updating. The Chinese patent with publication number CN117236072B discloses a shield target pose solving method based on a tunnel design axis, which comprises the following steps: giving a parameterized equation of a tunnel design axis; carrying out vector calculation on the absolute target position of the shield tunneling to obtain the absolute target position of the shield tunneling; based on the absolute target position of the shield tunneling, carrying out the matrix calculation of the absolute target attitude of the shield tunneling to obtain the absolute daily standard attitude of the shield tunneling; and carrying out the parameter calculation of the position and the posture of the absolute target of the shield tunneling based on the absolute daily standard position and the absolute target posture of the shield tunneling.
The chinese patent with publication number CN117236072B has many data parameters introduced during the calculation process, and has the problems of complex calculation, precision loss, interleaving of parameters inside the model, and difficulty in establishing a direct relationship between the parameters to be solved and the observed quantity, and cannot ensure the accuracy and efficiency of pose calculation when the complex earth structure parameters are repeatedly converted.
Disclosure of Invention
Aiming at the defects or improvement demands of the prior art, the invention provides a method and a system for resolving the precise pose under an inertial system, which start from the bottom layer theory resolved by an inertial navigation system, directly resolving the pose under a geocentric inertial coordinate system, deduct the influence caused by the related parameters of the earth, omit the conversion, integration and other correction operations related to the parameters of the earth in the middle process, effectively improve the computing efficiency and reduce the error caused by the current inertial positioning pose resolving method.
In order to achieve the above object, according to a first aspect of the embodiments of the present invention, there is provided a method for precisely integrating and calculating a pose under an inertial system, including:
S100, setting a geocentric inertial coordinate system as a navigation resolving coordinate system, and setting the geocentric inertial coordinate system at the initial moment to coincide with a geocentric fixed coordinate system;
s200, acquiring initial state information of the carrier including an initial position, an initial speed and an initial posture according to a geocentric inertial coordinate system at an initial moment;
S300, measuring acceleration and angular velocity of the carrier at the middle moment, and integrating the acceleration and the angular velocity under a geocentric inertial coordinate system to obtain a coordinate of the carrier at the termination moment;
S400, calculating to obtain the change angle of the geocentric geodetic coordinate system under the geodetic inertial coordinate system, rotating the geodetic coordinate system around the z axis to obtain the motion quantity of the geodetic coordinate system under the geodetic inertial coordinate system, and subtracting the motion quantity of the geodetic coordinate system under the geodetic inertial coordinate system from the coordinate of the carrier at the termination moment to obtain new carrier pose information;
S500, calculating and obtaining an error equation through the information in the step S300, and correcting the new carrier pose information by using the error equation to obtain final carrier pose information.
Further, in step S200, the obtaining of the initial state information includes:
s201, converting the coordinate transformation matrix from the carrier coordinate system at the initial moment to the geocentric and geodetic fixed coordinate system And a coordinate transformation matrix of the carrier coordinate system to the navigation coordinate systemAcquiring a coordinate transformation matrix (initial posture) from a carrier coordinate system at the initial moment of a carrier to a geocentric inertial coordinate system；
S202, determining the initial three-dimensional speed of the carrier under the geocentric inertial coordinate system according to the speed and the position of the carrier in each direction in the navigation coordinate system and the earth parameters;
s203, determining the initial position of the carrier according to the geocentric fixed coordinate system.
Further, in step S201, the coordinate transformation matrix (initial posture) from the carrier coordinate system to the geocentric inertial coordinate system at the initial time is obtainedThe method comprises the following steps:
；
wherein, Is a coordinate transformation matrix from a carrier coordinate system to a geocentric fixed coordinate system in an initial state.
Further, the coordinate transformation matrix from the carrier coordinate system to the geocentric and geodetic coordinate system at the initial momentThe method comprises the following steps:
；
wherein, As the longitude and latitude of the initial time,
Is the latitude at the initial time.
Further, the coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system at the initial momentThe method comprises the following steps:
；
wherein, For the heading attitude angle at the initial time,
For the pitch attitude angle at the initial moment,
Is the roll attitude angle at the initial time.
Further, in step S202, the initial three-dimensional speed of the carrier in the geocentric inertial coordinate system is:
；
wherein, For the initial three-dimensional velocity of the carrier in the geocentric inertial coordinate system,
For the velocity of the carrier in the x-axis direction in the navigational coordinate system,
For the velocity of the carrier in the y-axis direction in the navigational coordinate system,
For the velocity of the carrier in the z-axis direction in the navigational coordinate system,
Is the curvature radius of the earth's mortise circle,
For the elevation of the carrier from the ground,
Is the rotational angular velocity of the earth,
Is the latitude of the place where the carrier is located.
Further, in step S203, the initial position of the carrier is:
；
wherein, Is the initial three-dimensional position vector of the carrier,
The coordinates of the carrier in the geocentric fixed coordinate system at the initial moment.
Further, in step S300, the termination time is a calculated target time, and the pose information of the carrier at the termination time is updated by a state update formula, including:
S301, updating the position:
；
wherein, Is the position vector at the moment m,
Is the position vector at the time of m-1,
For position increments from m-1 to m times,
The coordinate transformation matrix is m-1 to m middle moments.
Further, step S300 further includes:
s302, updating the gesture:
；
；
；
wherein, For the coordinate transformation matrix from the m-moment carrier coordinate system to the navigation coordinate system,
The coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system at the m-1 moment,
Is a coordinate transformation matrix from an m-1 moment carrier coordinate system to an m moment carrier coordinate system,
Is a three-dimensional rotation vector under a carrier coordinate system at m time,
Is a three-dimensional angle change vector at m time,
Is a three-dimensional angle change vector at m-1 time.
Further, step S300 further includes:
s303, updating the speed:
；
wherein, Is a velocity change vector in the inertial frame,
Is a coordinate rotation matrix of the carrier system relative to the inertial system,
For the specific force information measured under the carrier system,
Is the earth gravity vector under the inertial system.
Further, in step S500, the error equation includes a position error equation, an attitude error equation, and a velocity error equation.
Further, the velocity error equation is:
；
wherein, In order to change the speed error, the speed error is changed,
In order to measure the resulting specific force information,
For the purpose of an attitude error,
Is the accelerometer zero offset error.
The attitude error equation is:
，
wherein, In order for the attitude error to vary,
For the purpose of an attitude error,
For the attitude change and the error of the carrier coordinate system measured under the inertial system relative to the inertial system,
Is zero offset error of the gyro under the inertial system.
The position error equation is:
；
wherein, Is the position error change in the X-axis direction under the inertial coordinate system,
Is the position error change in the Y-axis direction under the inertial coordinate system,
Is the position error change in the Z-axis direction under the inertial coordinate system,
Is the velocity error in the X-axis direction under the inertial coordinate system,
Is the velocity error in the Y-axis direction under the inertial coordinate system,
Is the speed error in the Z-axis direction under the inertial coordinate system.
According to another aspect of the embodiment of the present invention, there is provided an inertial system pose accurate integration calculation system, including:
the inertial measurement module is used for measuring the instantaneous specific force of the moving carrier in three directions and the instantaneous angular speed relative to an inertial coordinate system;
the pose conversion module is used for obtaining a rotation matrix of the carrier coordinate system corresponding to the calculation coordinate system through calculation of the diagonal speed, and converting the specific force into the calculation coordinate system through the rotation matrix;
the pose resolving module is used for calculating the speed in a calculation coordinate system through contrast force to finally obtain new pose information of the carrier including the position, the speed and the pose;
and the error analysis module is used for optimally compensating the obtained position, speed and posture information through an error formula to obtain final carrier posture information.
In general, the above technical solutions conceived by the present invention, compared with the prior art, enable the following beneficial effects to be obtained:
1. the invention relates to a precise integration and calculation method for pose under an inertial system, which is characterized in that the observation values of a gyroscope and an accelerometer are directly integrated and calculated under an inertial coordinate system, meanwhile, the influence caused by the related parameters of the earth is independently integrated, and the position, speed and pose variation caused by the rotation of the earth is deducted from the final position, speed and pose calculation result. The method for correcting the position and posture errors by inertial product decomposition under the inertial coordinate system is researched, conversion, integration and other correction operations related to the earth parameters in the middle process are omitted, the calculation efficiency is effectively improved, and errors caused by the current inertial positioning and posture determination calculation method are reduced.
2. The invention discloses a precision product resolving method for pose under an inertial system, which solves the problems that the complex process of resolving an existing inertial navigation system product resolving model, the interleaving of earth rotation, centrifugal force, attraction force, a radius of curvature of a mortise ring, a radius of curvature of a meridian ring, various errors and the like, causes complex calculation, precision loss and interleaving of parameters in the model, and is difficult to establish direct relation between parameters to be solved and observed quantity.
3. According to the pose accurate integrating and calculating system under the inertial system, the three-axis accelerometer and the gyroscope are utilized to detect the instantaneous acceleration and the instantaneous angular velocity in the three directions of XYZ, linear integration calculation is directly carried out under the inertial system to obtain pose information of a carrier, complex earth structure parameter repeated conversion is not needed, and the accuracy and the efficiency of pose calculation are ensured.
Drawings
FIG. 1 is a flow chart of a method for resolving a pose precise product under an inertial frame according to an embodiment of the invention;
FIG. 2 is a schematic diagram of the steps of a method for resolving the pose precise integration under the inertial system according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of step S200 of a method for resolving a pose precise product under an inertial frame according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a connection relationship of a system for resolving pose precise integration under an inertial system according to an embodiment of the present invention;
fig. 5 is a schematic structural diagram of a computing device according to an embodiment of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention. In addition, the technical features of the embodiments of the present invention described below may be combined with each other as long as they do not collide with each other.
The generation of acceleration includes two parts, one caused by the presence of a mass, and the other caused by a change in the state of motion relative to the inertial space. The change in angular velocity is caused by a change in attitude with respect to the inertial space.
Example 1
The embodiment provides a method for precisely integrating and calculating pose under an inertial system, which specifically comprises the following steps:
S100, setting a geocentric inertial coordinate system as a navigation resolving coordinate system, and setting the geocentric inertial coordinate system at the initial moment to coincide with a geocentric fixed coordinate system;
s200, acquiring initial state information of the carrier including an initial position, an initial speed and an initial posture according to a geocentric inertial coordinate system at an initial moment;
S300, measuring acceleration and angular velocity of the carrier at the middle moment by using a gyroscope and an accelerometer, and integrating the acceleration and the angular velocity under a geocentric inertial coordinate system to obtain a coordinate of the carrier at the termination moment;
s400, obtaining a change angle of the geocentric geodetic coordinate system under the geodetic inertial coordinate system according to the movement time multiplied by the angular velocity, rotating the geodetic coordinate system around the z axis by the change angle to obtain the movement quantity of the geodetic coordinate system under the geodetic inertial coordinate system, subtracting the movement quantity of the geodetic coordinate system under the geodetic inertial coordinate system from the coordinate of the carrier at the termination moment, and obtaining new carrier pose information.
In step S200, the obtaining of the initial state information includes:
s201, converting the coordinate transformation matrix from the carrier coordinate system at the initial moment to the geocentric and geodetic fixed coordinate system And a coordinate transformation matrix of the carrier coordinate system to the navigation coordinate systemAcquiring a coordinate transformation matrix (initial posture) from a carrier coordinate system at the initial moment of a carrier to a geocentric inertial coordinate system；
S202, determining the initial three-dimensional speed of the carrier under the geocentric inertial coordinate system according to the speed and the position of the carrier in each direction in the navigation coordinate system and the earth parameters;
s203, determining the initial position of the carrier according to the geocentric fixed coordinate system.
In step S201, the coordinate transformation matrix (initial posture) from the carrier coordinate system to the geocentric inertial coordinate system at the initial timeThe method comprises the following steps:
；
wherein, The coordinate transformation matrix is a coordinate transformation matrix from a carrier coordinate system to a geocentric ground fixed coordinate system in an initial state;
coordinate transformation matrix from carrier coordinate system to geocentric and geodetic fixed coordinate system at initial moment The method comprises the following steps:
；
wherein, As the longitude and latitude of the initial time,
The latitude at the initial moment;
coordinate transformation matrix from carrier coordinate system to navigation coordinate system at initial moment The method comprises the following steps:
；
wherein, For the heading attitude angle at the initial time,
For the pitch attitude angle at the initial moment,
Is the roll attitude angle at the initial time.
In step S202, the initial three-dimensional speed of the carrier in the geocentric inertial coordinate system is:
；
wherein, For the initial three-dimensional velocity of the carrier in the geocentric inertial coordinate system,
For the velocity of the carrier in the x-axis direction in the navigational coordinate system,
For the velocity of the carrier in the y-axis direction in the navigational coordinate system,
For the velocity of the carrier in the z-axis direction in the navigational coordinate system,
Is the curvature radius of the earth's mortise circle,
For the elevation of the carrier from the ground,
Is the rotational angular velocity of the earth,
Is the latitude of the place where the carrier is located.
In step S203, the initial position of the carrier is:
；
wherein, Is the initial three-dimensional position vector of the carrier,
The coordinates of the carrier in the geocentric fixed coordinate system at the initial moment.
The coordinates of the initial moment carrier under the geocentric earth fixed coordinate systemPositioning is performed by positioning means including satellite positioning, vision sensor positioning, bluetooth positioning, ultrasonic positioning, and Wi-Fi positioning.
In step S300, the termination time is a calculated target time, and the pose information of the carrier at the termination time is updated by a state update formula, including:
S301, updating the position:
；
wherein, Is the position vector at the moment m,
Is the position vector at the time of m-1,
For position increments from m-1 to m times,
A coordinate transformation matrix from m-1 to m middle time;
s302, updating the gesture:
；
；
；
wherein, For the coordinate transformation matrix from the m-moment carrier coordinate system to the navigation coordinate system,
The coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system at the m-1 moment,
Is a coordinate transformation matrix from an m-1 moment carrier coordinate system to an m moment carrier coordinate system,
Is a three-dimensional rotation vector under a carrier coordinate system at m time,
Is a three-dimensional angle change vector at m time,
The three-dimensional angle change vector is m-1 moment;
coordinate transformation matrix from m-1 moment carrier coordinate system to navigation coordinate system The method comprises the following steps:
，
wherein, Is the included angle between the m-1 moment carrier coordinate system and the initial moment inertial coordinate system in the X-axis direction,
Is the included angle between the m-1 moment carrier coordinate system and the initial moment inertial coordinate system in the Y-axis direction,
The included angle between the m-1 moment carrier coordinate system and the initial moment inertial coordinate system in the Z-axis direction;
The said 、AndObtained by multiplying the gyro observations by the corresponding time intervals.
The coordinate transformation matrix from the m-1 moment carrier coordinate system to the m moment carrier coordinate systemThe method comprises the following steps:
；
wherein, The attitude change angle of the carrier coordinate system in the X-axis direction from m-1 time to m time,
The attitude change angle of the carrier coordinate system in the Y-axis direction from m-1 time to m time,
The attitude change angle of the carrier coordinate system in the Z-axis direction from m-1 time to m time;
The said 、AndAnd is also obtained by multiplying the gyro observations by the corresponding time intervals.
S303, updating the speed:
；
wherein, Is a velocity change vector in the inertial frame,
Is a coordinate rotation matrix of the carrier system relative to the inertial system,
For the specific force information measured under the carrier system,
Is the earth gravity vector under the inertial system.
Wherein the velocity change vector under the inertial systemThe method comprises the following steps:
；
Is the speed variation along the X-axis direction under the inertia system,
Is the speed variation along the Y-axis direction under the inertia system,
Is the speed variation along the Z axis direction under the inertial system;
Specific force information measured under the carrier system The method comprises the following steps:
；
Is specific force information along the X-axis direction under the inertia system,
Is specific force information along the Y-axis direction under the inertial system,
Is specific force information along the X-axis direction under an inertial system;
gravity vector of earth under inertial system The method comprises the following steps:
；
Is the gravitational acceleration along the X-axis direction under the inertial system,
Is the gravity acceleration along the Y-axis direction under the inertia system,
Is the gravitational acceleration along the Z-axis direction under the inertial system.
In step S400, the geocentric geodetic coordinate system and the geodetic inertial coordinate system are the same z-axis, and the amount of motion of the geodetic coordinate system in the geodetic inertial coordinate system is obtained by rotating the change angle around the z-axis.
After step S400, the method further comprises the steps of:
S500, deriving the position, speed and posture updating equation to obtain an error equation, and correcting the new carrier posture information by using the error equation to obtain final carrier posture information.
In step S500, the error equation includes a position error equation, an attitude error equation, and a velocity error equation, where the velocity error equation is:
；
wherein, In order to change the speed error, the speed error is changed,
In order to measure the resulting specific force information,
For the purpose of an attitude error,
Zero offset error for accelerometer;
The attitude error equation is:
；
wherein, In order for the attitude error to vary,
For the purpose of an attitude error,
For the attitude change and the error of the carrier coordinate system measured under the inertial system relative to the inertial system,
Zero offset error of the gyro under the inertial system;
The position error equation is:
；
wherein, Is the position error change in the X-axis direction under the inertial coordinate system,
Is the position error change in the Y-axis direction under the inertial coordinate system,
Is the position error change in the Z-axis direction under the inertial coordinate system,
Is the velocity error in the X-axis direction under the inertial coordinate system,
Is the velocity error in the Y-axis direction under the inertial coordinate system,
Is the speed error in the Z-axis direction under the inertial coordinate system.
Preferably, the invention starts from the underlying theory solved by the inertial navigation system, directly integrating and resolving under the geocentric inertial coordinate system, deducting the influence caused by the related parameters of the earth, the conversion, integration and other correction operations related to the earth parameters in the middle process are omitted, so that the calculation efficiency can be effectively improved, and errors caused by the current inertial positioning and attitude determination solution method can be reduced.
In the invention, m is 1 … N, and N is a positive integer. The origin of the geocentric inertial coordinate system is selected at the center of the earth, the z-axis of the coordinate system points to the north pole along the direction of the earth's rotation axis, the plane formed by the x-axis and the y-axis is in the equatorial plane and is perpendicular to the earth's rotation axis, wherein the x-axis is the intersection line of the equatorial plane and the equatorial plane, and xyz forms the right-hand coordinate system. The geocentric coordinate system is fixed in the inertial space and is a measurement reference of a measurement sensor of the inertial navigation system; the geocentric geodetic coordinate system is fixed relative to the earth body, rotates relative to the inertial coordinate system at the earth rotation speed, the origin of the geodetic coordinate system is positioned at the earth center, the z axis coincides with the earth rotation axis as the z axis of the geodetic inertial coordinate system, the oxy plane is positioned in the equatorial plane, the x axis points to the intersection point of the equator and the initial meridian plane on the equatorial plane, and the y axis points to the east 90 DEG direction; the navigation coordinate system is a coordinate system determined according to navigation requirements, and a geographic coordinate system taking the center of gravity of the carrier as an origin is selected as the navigation coordinate system; the origin of the geographic coordinate system can be selected at the gravity center of the carrier, or at any point on the surface of the earth, and the x, y and z axes respectively represent the east, north and sky directions; the carrier coordinate system is a coordinate system fixedly connected with the carrier, an origin is selected at the center of gravity of the carrier, a Y axis points to the advancing direction, an X axis points to the right, a Z axis, the X axis and the Y axis form a right-hand rectangular coordinate system, and pitch angles, roll angles and course angles are corresponding to the X axis, the Y axis and the Z axis.
Example 2
The present embodiment provides a system for precisely integrating and calculating pose under inertial system, comprising:
the inertial measurement module is used for measuring the instantaneous specific force of the moving carrier in three directions and the instantaneous angular speed relative to an inertial coordinate system;
the pose conversion module is used for obtaining a rotation matrix of the carrier coordinate system corresponding to the calculation coordinate system through calculation of the diagonal speed, and converting the specific force into the calculation coordinate system through the rotation matrix;
the pose resolving module is used for calculating the speed in a calculation coordinate system through contrast force to finally obtain new pose information of the carrier including the position, the speed and the pose;
and the error analysis module is used for optimally compensating the obtained position, speed and posture information through an error formula to obtain final carrier posture information.
The inertial measurement module (IMU) comprises a triaxial accelerometer, a gyroscope, a digital coil and a CPU, wherein the triaxial accelerometer is used for detecting instantaneous acceleration of the moving carrier in three directions of XYZ, and the gyroscope is used for detecting instantaneous angular velocity of the moving carrier relative to an inertial coordinate system in the three directions of XYZ.
The data of the triaxial accelerometer and the gyroscope are compensated and optimized to be a linear acceleration rate and an angular acceleration rate, and the data are integrated into the inertial navigator to generate information including position, attitude and speed relative to an earth coordinate system.
Example 3
The present application also provides a computing device, please refer to fig. 5, fig. 5 is a schematic structural diagram of a computing device according to an embodiment of the present application, the computing device may include:
a memory for storing a computer program;
and the processor is used for executing the computer program to realize the steps of any automatic solving method of the computer program based on the data pool.
As shown in fig. 5, which is a schematic diagram of a composition structure of a computing device, the computing device may include: a processor 10, a memory 11, a communication interface 12 and a communication bus 13. The processor 10, the memory 11 and the communication interface 12 all complete communication with each other through a communication bus 13.
In an embodiment of the present application, the processor 10 may be a central processing unit (Central Processing Unit, CPU), an asic, a dsp, a field programmable gate array, or other programmable logic device, etc.
Processor 10 may call a program stored in memory 11, and in particular, processor 10 may perform operations in an embodiment of an abnormal IP identification method.
The memory 11 is used for storing one or more programs, and the programs may include program codes including computer operation instructions, and in the embodiment of the present application, at least the programs for implementing the following functions are stored in the memory 11:
Adding the corresponding flow elements into the canvas based on the received drag operation information; the process elements comprise text analysis elements, execution programs and optimization algorithms;
Inputting information to the corresponding flow elements in the canvas based on the input operation information;
Connecting all the flow elements based on the directed line segments, the calling information of each flow element and the logic flow information to obtain logic flows;
And executing optimization analysis processing on the logic flow based on the variable data in the data pool to obtain an optimal result.
In one possible implementation, the memory 11 may include a storage program area and a storage data area, where the storage program area may store an operating system, and at least one application program required for functions, etc.; the storage data area may store data created during use.
In addition, the memory 11 may include high-speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device or other volatile solid-state storage device.
The communication interface 12 may be an interface of a communication module for interfacing with other devices or systems.
Of course, it should be noted that the structure shown in fig. 5 does not limit the computing device in the embodiment of the present application, and the computing device may include more or fewer components than shown in fig. 5 or may combine some components in practical applications.
Example 4
The application also provides a computer readable storage medium, the computer readable storage medium stores a computer program, and the computer program can realize the steps of any automatic solving method of the computer program based on the data pool when being executed by a processor.
The computer readable storage medium may include: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
For the description of the computer-readable storage medium provided by the present application, refer to the above method embodiments, and the disclosure is not repeated here.
In the description, each embodiment is described in a progressive manner, and each embodiment is mainly described by the differences from other embodiments, so that the same similar parts among the embodiments are mutually referred. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
Those of skill would further appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative elements and steps are described above generally in terms of functionality in order to clearly illustrate the interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. The software modules may be disposed in Random Access Memory (RAM), memory, read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
It will be readily appreciated by those skilled in the art that the foregoing description is merely a preferred embodiment of the invention and is not intended to limit the invention, but any modifications, equivalents, improvements or alternatives falling within the spirit and principles of the invention are intended to be included within the scope of the invention.
Claims (17)
1. The method for precisely integrating and calculating the pose under the inertial system is characterized by comprising the following steps of:
S100, setting a geocentric inertial coordinate system as a navigation resolving coordinate system, and setting the geocentric inertial coordinate system at the initial moment to coincide with a geocentric fixed coordinate system;
s200, acquiring initial state information of the carrier including an initial position, an initial speed and an initial posture according to a geocentric inertial coordinate system at an initial moment;
S300, measuring acceleration and angular velocity of the carrier at the middle moment, and integrating the acceleration and the angular velocity under a geocentric inertial coordinate system to obtain a coordinate of the carrier at the termination moment;
S400, calculating to obtain the change angle of the geocentric geodetic coordinate system under the geodetic inertial coordinate system, rotating the geodetic coordinate system around the z axis to obtain the motion quantity of the geodetic coordinate system under the geodetic inertial coordinate system, and subtracting the motion quantity of the geodetic coordinate system under the geodetic inertial coordinate system from the coordinate of the carrier at the termination moment to obtain new carrier pose information;
S500, calculating and obtaining an error equation through the information in the step S300, and correcting the new carrier pose information by using the error equation to obtain final carrier pose information.
2. The method for precisely integrating and calculating the pose under inertial system according to claim 1, wherein in step S200, the obtaining of the initial state information includes:
s201, converting the coordinate transformation matrix from the carrier coordinate system at the initial moment to the geocentric and geodetic fixed coordinate system And a coordinate transformation matrix of the carrier coordinate system to the navigation coordinate systemAcquiring a coordinate conversion matrix from a carrier coordinate system at the initial moment of a carrier to a geocentric inertial coordinate system；
S202, determining the initial three-dimensional speed of the carrier under the geocentric inertial coordinate system according to the speed and the position of the carrier in each direction in the navigation coordinate system and the earth parameters;
s203, determining the initial position of the carrier according to the geocentric fixed coordinate system.
3. The method of claim 2, wherein in step S201, the coordinate transformation matrix from the carrier coordinate system to the geocentric inertial coordinate system at the initial time is the sameThe method comprises the following steps:
wherein, Is a coordinate transformation matrix from a carrier coordinate system to a geocentric fixed coordinate system in an initial state.
4. A method for precisely integrating and calculating pose under inertial system according to claim 3, wherein said initial moment is a coordinate transformation matrix from carrier coordinate system to geocentric earth fixed coordinate systemThe method comprises the following steps:
wherein, As the longitude of the initial moment in time,
Is the latitude at the initial time.
5. A method for precisely integrating and calculating pose under inertial system according to claim 3, wherein said coordinate transformation matrix from carrier coordinate system to navigation coordinate system at initial timeThe method comprises the following steps:
wherein, For the heading attitude angle at the initial time,
For the pitch attitude angle at the initial moment,
Is the roll attitude angle at the initial time.
6. The method for precisely integrating and calculating the pose under the inertial system according to claim 2, wherein in step S202, the initial three-dimensional velocity of the carrier under the geocentric inertial coordinate system is:
wherein, For the initial three-dimensional velocity of the carrier in the geocentric inertial coordinate system,
For the velocity of the carrier in the x-axis direction in the navigational coordinate system,
For the velocity of the carrier in the y-axis direction in the navigational coordinate system,
For the velocity of the carrier in the z-axis direction in the navigational coordinate system,
Is the curvature radius of the earth's mortise circle,
For the elevation of the carrier from the ground,
Is the rotational angular velocity of the earth,
Is the latitude of the place where the carrier is located.
7. The method for precisely integrating and calculating the pose under inertial system according to claim 2, wherein in step S203, the initial position of the carrier is:
wherein, Is the initial three-dimensional position vector of the carrier,
The coordinates of the carrier in the geocentric fixed coordinate system at the initial moment.
8. The method of any one of claims 1 to 7, wherein in step S300, the termination time is a calculated target time, and the pose information of the carrier at the termination time is updated by a state update formula, including:
S301, updating the position:
wherein, Is the position vector at the moment m,
Is the position vector at the time of m-1,
For position increments from m-1 to m times,
The coordinate transformation matrix is m-1 to m middle moments.
9. The method of inertial system pose accurate product calculation according to claim 8, wherein step S300 further comprises:
s302, updating the gesture:
wherein, For the coordinate transformation matrix from the m-moment carrier coordinate system to the navigation coordinate system,
The coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system at the m-1 moment,
Is a coordinate transformation matrix from an m-1 moment carrier coordinate system to an m moment carrier coordinate system,
Is a three-dimensional rotation vector under a carrier coordinate system at m time,
Is a three-dimensional angle change vector at m time,
Is a three-dimensional angle change vector at m-1 time.
10. The method of inertial system pose accurate product calculation according to claim 8, wherein step S300 further comprises:
s303, updating the speed:
wherein, Is a velocity change vector in the inertial frame,
Is a coordinate rotation matrix of the carrier system relative to the inertial system,
For the specific force information measured under the carrier system,
Is the earth gravity vector under the inertial system.
11. The method according to any one of claims 1 to 7, wherein in step S500, the error equations include a position error equation, an attitude error equation, and a velocity error equation.
12. The inertial system pose accurate product calculation method according to claim 11, wherein said velocity error equation is:
wherein, In order to change the speed error, the speed error is changed,
In order to measure the resulting specific force information,
For the purpose of an attitude error,
Is the accelerometer zero offset error.
13. The inertial system pose accurate product calculation method according to claim 11, wherein said pose error equation is:
wherein, In order for the attitude error to vary,
For the purpose of an attitude error,
For the attitude change and the error of the carrier coordinate system measured under the inertial system relative to the inertial system,
Is zero offset error of the gyro under the inertial system.
14. The inertial system pose accurate product calculation method according to claim 11, wherein said position error equation is:
wherein, Is the position error change in the X-axis direction under the inertial coordinate system,
Is the position error change in the Y-axis direction under the inertial coordinate system,
Is the position error change in the Z-axis direction under the inertial coordinate system,
Is the velocity error in the X-axis direction under the inertial coordinate system,
Is the velocity error in the Y-axis direction under the inertial coordinate system,
Is the speed error in the Z-axis direction under the inertial coordinate system.
15. An inertial system pose accurate product calculation system for implementing the inertial system pose accurate product calculation method according to any one of claims 1to 4, comprising:
the inertial measurement module is used for measuring the instantaneous specific force of the moving carrier in three directions and the instantaneous angular speed relative to an inertial coordinate system;
the pose conversion module is used for obtaining a rotation matrix of the carrier coordinate system corresponding to the calculation coordinate system through calculation of the diagonal speed, and converting the specific force into the calculation coordinate system through the rotation matrix;
the pose resolving module is used for calculating the speed in a calculation coordinate system through contrast force to finally obtain new pose information of the carrier including the position, the speed and the pose;
and the error analysis module is used for optimally compensating the obtained position, speed and posture information through an error formula to obtain final carrier posture information.
16. A computing device, comprising:
a memory for storing a computer program;
a processor for implementing the steps of the method according to any one of claims 1 to 14 when said computer program is executed.
17. A computer readable storage medium, characterized in that it has stored thereon a computer program which, when executed by a processor, implements the steps of the method according to any of claims 1 to 14.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410643551.3A CN118225086B (en) | 2024-05-23 | 2024-05-23 | Method and system for precisely integrating and calculating pose under inertial system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410643551.3A CN118225086B (en) | 2024-05-23 | 2024-05-23 | Method and system for precisely integrating and calculating pose under inertial system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118225086A CN118225086A (en) | 2024-06-21 |
| CN118225086B true CN118225086B (en) | 2024-08-02 |
Family
ID=91510517
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410643551.3A Active CN118225086B (en) | 2024-05-23 | 2024-05-23 | Method and system for precisely integrating and calculating pose under inertial system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118225086B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106643726A (en) * | 2016-11-23 | 2017-05-10 | 北京航天控制仪器研究所 | Unified inertial navigation calculation method |
| CN113203415A (en) * | 2021-04-12 | 2021-08-03 | 北京航空航天大学 | Atomic gyro navigation system and navigation resolving method thereof |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8781737B2 (en) * | 2009-11-20 | 2014-07-15 | Qualcomm Incorporated | Spatial alignment determination for an inertial measurement unit (IMU) |
-
2024
- 2024-05-23 CN CN202410643551.3A patent/CN118225086B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106643726A (en) * | 2016-11-23 | 2017-05-10 | 北京航天控制仪器研究所 | Unified inertial navigation calculation method |
| CN113203415A (en) * | 2021-04-12 | 2021-08-03 | 北京航空航天大学 | Atomic gyro navigation system and navigation resolving method thereof |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118225086A (en) | 2024-06-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112595350B (en) | Automatic calibration method and terminal for inertial navigation system | |
| CN107063254B (en) | Gesture resolving method for gyros and geomagnetic combination | |
| CN110954102B (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN111678538A (en) | Dynamic level meter error compensation method based on speed matching | |
| CN107202578B (en) | MEMS technology-based strapdown vertical gyroscope resolving method | |
| CN109211269A (en) | A kind of dual-axis rotation inertial navigation system attitude error scaling method | |
| CN107677292B (en) | Vertical line deviation compensation method based on gravity field model | |
| CN111537002B (en) | Calibration method and orientation method for laser strapdown inertial measurement unit installation error | |
| CN113587925B (en) | Inertial navigation system and full-attitude navigation resolving method and device thereof | |
| CN116448145A (en) | Navigation attitude determination method based on polarization vector space difference | |
| CN109631939A (en) | A kind of rapid alignment method based on magnetometer and accelerometer | |
| CN113551669B (en) | Combined navigation positioning method and device based on short base line | |
| CN113790737B (en) | On-site rapid calibration method of array sensor | |
| CN109029499B (en) | Accelerometer zero-bias iterative optimization estimation method based on gravity apparent motion model | |
| CN107063181A (en) | The measuring method and device of the level inclination of Multifunctional adjustment table under complex environment | |
| CN110514201B (en) | Inertial navigation system and navigation method suitable for high-rotation-speed rotating body | |
| CN118225086B (en) | Method and system for precisely integrating and calculating pose under inertial system | |
| JP2001141507A (en) | Inertial navigation system | |
| CN115931001A (en) | Inertial measurement unit calibration method and device, computer equipment and storage medium | |
| CN113670297A (en) | Off-line positioning method based on MEMS and wheel type odometer | |
| CN113551688A (en) | Quick non-support disassembly-free calibration method and device for vehicle-mounted positioning and directional navigation equipment | |
| CN112649001A (en) | Method for resolving attitude and position of small unmanned aerial vehicle | |
| CN118424241B (en) | Short-time double-position north-seeking method and system of north-seeking instrument | |
| CN117076824B (en) | State prediction method and device of mobile terminal, electronic equipment and storage medium | |
| CN116839634B (en) | Method for calibrating gyroscope by mechanical arm and mechanical arm |
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 |