CN110763229A - Portable inertial navigation positioning rod and positioning and attitude determining method thereof - Google Patents
Portable inertial navigation positioning rod and positioning and attitude determining method thereof Download PDFInfo
- Publication number
- CN110763229A CN110763229A CN201911099403.5A CN201911099403A CN110763229A CN 110763229 A CN110763229 A CN 110763229A CN 201911099403 A CN201911099403 A CN 201911099403A CN 110763229 A CN110763229 A CN 110763229A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- zero
- speed
- positioning rod
- navigation positioning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- 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/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
The invention discloses a portable inertial navigation positioning rod and a positioning and attitude determining method thereof, based on a zero-speed correction/inertial navigation system, the inertial navigation positioning rod comprises a carrier rod, a measurement sensor and a data processing unit; detecting and identifying the time interval of the zero-speed correction opportunity by using a zero-speed detection sensor; and the zero-speed observation value is used for measurement updating and is subjected to data fusion with inertial navigation solution, so that the combined positioning and attitude determination solution is realized. The invention solves the problem of precision divergence caused by rapid accumulation of navigation errors of a low-cost micro-electro-mechanical system (MEMS) Inertial Navigation System (INS) along with time, can realize zero-speed correction in an extremely easy and convenient mode under the condition of not interrupting/interfering the motion state of the system, inhibits the error divergence of the inertial navigation system, and realizes the accurate positioning of portable navigation positioning or mapping equipment.
Description
Technical Field
The invention relates to the field of inertial navigation and inertial integrated navigation positioning, in particular to a portable inertial navigation positioning rod and a positioning and attitude determining method thereof.
Background
Inertial navigation is a technology for obtaining a positioning and attitude-fixing result by performing projection and integral operation on data of an inertial sensor (including a gyroscope and an accelerometer), and navigation and positioning errors of the inertial navigation system accumulate over time, particularly the navigation errors of a low-cost micro-electro-mechanical system (MEMS) Inertial Navigation System (INS) accumulate rapidly over time. The inertial navigation system is often combined with other navigation positioning means to form a combined navigation system to solve the problem of accuracy divergence. The integrated navigation generally requires additional sensors to acquire external auxiliary positioning information and correct inertial navigation errors.
Zero-speed correction is a commonly used, effective, "zero-cost" inertial navigation aid. When the inertial navigation system and the carrier are in a static state, the zero speed is used as an external observation to correct the inertial navigation speed. The zero-speed correction can effectively control the speed error of the inertial navigation system, further slow down the divergence of the position error of the inertial navigation system, generally does not need to add an additional physical sensor in the implementation process of the zero-speed correction, and is effective low-cost auxiliary information. The use of zero speed correction information, however, requires that the carrier be stationary, interfering with and disrupting the normal motion of the carrier.
How to realize zero-speed correction under the condition of not interfering or interfering the motion state of the carrier as little as possible obviously has great significance. Especially for some portable navigation positioning or mapping devices, the method has extremely important practical value.
The invention provides a method for realizing zero-speed correction under the condition of not interrupting/interfering the motion state of a system, in particular to a portable positioning rod based on a zero-speed correction/inertial navigation system.
Disclosure of Invention
The invention aims to solve the technical problem of providing a portable inertial navigation positioning rod and a positioning and attitude determining method thereof aiming at the defects in the prior art.
The technical scheme adopted by the invention for solving the technical problems is as follows:
the invention provides a portable inertial navigation positioning rod, which comprises a carrier rod, a measurement sensor and a data processing unit, wherein the carrier rod is connected with the measurement sensor; wherein:
measuring transducer fixed mounting is on the carrier pole, and measuring transducer includes: an inertia measurement unit and a zero-speed detection sensor; the inertia measurement unit is used for measuring three-dimensional specific force and three-dimensional angular velocity information; the zero-speed detection sensor is fixedly arranged on the carrier rod and carries out zero-speed detection by judging whether the output of the zero-speed detection sensor exceeds a set threshold value or not;
the data processing unit is used for processing the measurement information transmitted by the measurement sensor in real time, and performing data fusion and positioning and attitude determination calculation by combining zero-speed detection information through an inertial navigation mechanical arrangement algorithm and Kalman filtering.
Further, the shape of the carrier rod of the present invention includes: rod-shaped carriers, spheres, cylinders or cuboids.
Further, the zero-speed detection sensor of the present invention includes: pressure sensor, inertial sensor, elastic switch, user manual switch.
Further, the inertial measurement unit of the present invention includes a three-axis accelerometer and a three-axis gyroscope.
The invention provides a positioning and attitude determining method of a portable inertial navigation positioning rod, which comprises the following steps:
s1, positioning and initializing the inertial navigation positioning rod;
s2, in the positioning process, the inertial navigation positioning rod intermittently obtains a zero-speed correction opportunity;
s3, detecting the zero-speed time period of the inertial navigation positioning rod, judging whether one end of the inertial navigation positioning rod is in contact with the ground of the detection point and keeps still, and if so, recording the ground contact time period as the zero-speed time period;
and S4, acquiring a zero-speed observation value of the inertial navigation positioning rod in a touchdown time period, and performing data fusion with an inertial navigation system to realize combined positioning and attitude determination calculation.
Further, the specific method for initializing the inertial navigation positioning lever positioning in step S1 of the present invention is as follows:
s11, performing positioning or positioning algorithm including resolving inertial navigation position, speed and attitude;
and S12, the positioning initialization of the inertial navigation positioning rod comprises position initialization, speed initialization and posture initialization.
Further, the specific method for intermittently obtaining the zero-velocity correction opportunity by the inertial navigation positioning lever in step S2 of the present invention is as follows:
s21, operating the inertial navigation positioning rod to enable the speed of a certain mass point of the inertial navigation positioning rod relative to the earth or ground fixed point to be zero;
s22, the inertial measurement unit itself or the entire inertial navigation positioning lever is not required to have zero velocity, and if the velocity of a certain part of the inertial navigation positioning lever fixedly connected to the inertial measurement unit is zero, the inertial navigation positioning lever may be regarded as a zero velocity correction opportunity.
Further, the method for detecting the zero-speed time interval of the inertial navigation positioning lever in the step S3 of the present invention includes:
installing a pressure sensor at the contact point of the inertial navigation positioning rod and the ground object: sensing whether the pressure value of the point is in contact with a fixed point on the ground or not and keeping the point static by detecting the change of the pressure value of the point; when the pressure value of the pressure sensor exceeds a certain preset threshold value, judging that the part of the inertial navigation positioning rod is in contact with a ground fixed point, and recording a time period when the corresponding pressure is greater than the threshold value as a zero-speed correction time period;
an inertia measurement unit is arranged in the inertia navigation positioning rod: judging whether the inertial navigation positioning rod and the ground contact point are in a zero-speed state or not through the change of the original data of the inertial measurement unit;
an elastic switch is arranged at the contact point of the inertial navigation positioning rod and the ground object: when the inertial navigation positioning rod touches the ground, the elastic force borne by the elastic switch is increased, when the inertial navigation positioning rod touches the ground, the elastic force borne by the switch is reduced, and whether the inertial navigation positioning rod touches the ground or not is judged by judging the elastic force change of the switch to be in a zero-speed state;
installing a manual switch at the contact point of the inertial navigation positioning rod and the ground object: when the inertial navigation positioning rod touches the ground, a user turns on the manual switch, the inertial navigation positioning rod turns off the manual switch at the ground-off moment, and the ground-contact time period of the positioning rod is recorded.
Further, the specific method for implementing the combined positioning and attitude determination calculation by data fusion in step S4 of the present invention is as follows:
s41, when the inertial navigation positioning rod is in the zero-speed time period, recording a zero-speed observation value in the zero-speed time period, realizing data fusion of the zero-speed observation value and the inertial navigation system in a filtering mode, and performing combined positioning calculation;
s42, constructing an observation equation of zero-speed correction; the speed relation between the speed of the inertial measurement unit and the contact point of the inertial navigation positioning rod is expressed as follows:
wherein b represents an inertial measurement unit carrier coordinate system, namely a system b; n represents a local horizontal coordinate system, also called a navigation coordinate system, n system; lbProjecting a three-dimensional vector of the center of the inertial measurement unit pointing to the contact point between the positioning rod and the ground under a system b;is a projection of the rotation angular velocity of b relative to n under b;
is a coordinate transformation matrix between an n system and a b system;the projection of the velocity of the inertial measurement unit under the n system;
for locating the position of the rod in contact withSpeed;
the velocity of the zero velocity point is calculated according to the navigation result of the inertial navigation system as follows:
the actual observations at the zero velocity point are:
wherein n represents the speed of the zero-speed point, and the value of n is not absolutely zero;
writing an observation equation of the zero-speed correction of the inertial navigation positioning rod:
s43, when the inertial navigation positioning rod is in a zero-speed state, the position of the touchdown point of the inertial navigation positioning rod is kept unchanged, the position information of the touchdown point with the unchanged position is used as the external observation of the inertial navigation, and the position divergence of an inertial navigation system is restrained;
s44, performing data fusion by taking zero-speed information as external assistance and inertial navigation solution; the data fusion method comprises Kalman filtering, extended Kalman filtering, unscented Kalman filtering, particle filtering, sequential least square method, nonlinear optimization algorithm or artificial neural network method.
Further, the specific method of using the position information of the touchdown point as the external observation of the inertial navigation in step S43 of the present invention is:
s431, taking the position of the zero-speed starting moment as an external position observation and carrying out data fusion with an inertial navigation system;
s432, regarding the inertial navigation positioning rod as a rigid body rotating around the touch point, and combining the posture of the inertial navigation system and the lever arm lbAnd the speed and the position of the inertial measurement unit are calculated, and the speed and the position of the inertial navigation are updated.
The invention has the following beneficial effects: the portable inertial navigation positioning rod and the positioning and attitude determining method thereof adopt an extremely easy and convenient form to realize zero-speed correction, do not require the whole carrier rod or the IMU to be in a static state, only require a certain part of the positioning rod to be in a zero-speed state, establish the relationship between the IMU and a contact through lever arm transmission, greatly reduce the cost and the cost for realizing the zero-speed correction, realize frequent zero-speed correction, reduce the cost for obtaining the opportunity of the zero-speed correction, and are beneficial to improving the positioning and attitude determining accuracy of an inertial navigation system.
Drawings
The invention will be further described with reference to the accompanying drawings and examples, in which:
FIG. 1 is a portable inertial positioning wand of an embodiment of the present invention;
in the figure, 1 — carrier rod; 2-an inertial measurement unit; 3-a pressure sensor; 4-a data processing unit;
FIG. 2 is a schematic zero-speed diagram of a positioning rod according to an embodiment of the present invention; in the figure, p represents the contact point of the positioning rod with the ground;
fig. 3 is a schematic diagram illustrating the movement and positioning of the inertial positioning rod according to the embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
As shown in fig. 1, the portable inertial positioning rod of the embodiment of the present invention includes a carrier rod 1, an inertial measurement unit 2, and a data processing unit 4, including but not limited to a pressure sensor 3. The sensors are fixedly connected and installed on the carrier rod 1; the sensor data is transmitted to the data processing unit 4 in real time for positioning and attitude determination calculation.
An Inertial Measurement Unit (IMU)2 includes a three-axis accelerometer and a three-axis gyroscope for measuring three-dimensional specific forces and three-dimensional angular velocities. The pressure sensor 3 is fixedly arranged at one end of the positioning rod and used for sensing the pressure applied to the figure 2p at the tail end of the positioning rod, and judging whether p is in contact with the ground or not by measuring the pressure change and used for assisting zero-speed detection.
The embodiment of the invention adopts an extremely easy and convenient form to realize zero-speed correction, and the inertial navigation can be realized by only needing a certain part of the positioning rod to be in a zero-speed state without requiring the whole carrier rod or the IMU to be in a static state. The specific implementation process includes but is not limited to the following modes: referring to fig. 2, when one end p of the positioning rod 1, which is provided with a pressure sensor, is in contact with the ground (or other ground immobile objects), and the pressure increase sensed by the pressure sensor exceeds a certain threshold, it is determined that the ground contact section of the positioning rod is in a stationary state, and at this time, the whole positioning rod is not required to be in a stationary state, that is, when the positioning rod can rotate freely around the contact point, the speed of the contact point is still zero; when p is off the ground, the pressure sensed by the pressure sensor is reduced, the positioning rod is judged to be in a motion state when contacting the ground, and the zero-speed correction condition is not met.
In the embodiment of the invention, the implementation process of the method for positioning and attitude determination by using the portable inertial positioning rod comprises the following steps:
(1) and initializing the positioning of the inertia positioning rod.
(2) In the positioning process, the positioning rod intermittently obtains zero-speed correction opportunities.
(3) Detecting and identifying the time interval when the positioning rod obtains the zero-speed correction opportunity.
(4) And the zero-speed observation and the inertial navigation solution are subjected to data fusion, so that the combined positioning and attitude determination solution is realized, and the position, speed and attitude calculation accuracy of the inertial navigation system is improved and maintained.
The initialization of the inertial positioning rod in the step (1) is specifically as follows:
1.1, initializing the inertial positioning rod comprises position initialization, speed initialization and posture initialization.
1.1.1, the position initialization method includes, but is not limited to, placing the positioning rod on a control point with known coordinates to obtain initial position coordinates.
1.1.2, the velocity initialization method includes, but is not limited to, setting the entire positioning rod in a stationary state and initializing the velocity to zero.
1.1.3, the attitude initialization method includes but is not limited to the steps of carrying out accelerometer leveling through data of a static period of a positioning rod to determine a pitching angle and a rolling angle, and carrying out course angle initialization through the coordinate back calculation of two control points on the orientation of the positioning rod.
1.2, after the initialization of the inertial positioning rod is completed, independent and autonomous inertial navigation calculation is carried out, and the calculation process is not interrupted along with the change of the motion state of the positioning rod.
The intermittent zero-speed correction opportunity in the step (2) includes, but is not limited to: as shown in fig. 3, the user of the portable inertial positioning rod holds the positioning rod by hand and advances in a walking stick manner, and during the exercise, the positioning rod is switched between two states, namely an exercise state and a zero-speed state.
2.1, zero speed state, that is, when one end p of the positioning rod, which is provided with a pressure sensor, is in contact with the ground or a static object on the ground, the speed of the p end relative to the ground is zero at the moment, that is, the positioning rod is in zero speed state.
2.2, "zero velocity state" does not require that the entire locating lever and IMU be at zero velocity, at which point even if the locating lever is rotated about the touchdown point p, the IMU and locating lever non-touchdown point are not at zero velocity at this time, but the zero velocity state at point p is still true.
2.3, the motion state refers to a motion state except the zero speed state, namely, the speed of the sensing end p of the positioning rod relative to the ground is not zero.
The zero speed state determination method in step (3) includes but is not limited to:
and 3.1, judging the zero-speed state according to the pressure value change measured by the pressure sensor arranged at the sensing end p.
3.3.1, the judging method includes, but is not limited to, setting a pressure threshold, and when the pressure sensed by the pressure sensor in real time is greater than or equal to the set threshold, determining that the positioning rod is in a zero-speed state, and performing zero-speed correction on inertial navigation at the moment; when the pressure sensed by the pressure sensor in real time is smaller than the set threshold value, the positioning rod is judged to be in a motion state, and the zero-speed correction of inertial navigation is unavailable at the moment.
The implementation process of performing the combined solution on the zero-speed information and the inertial navigation in the step (4) includes but is not limited to:
and 4.1, constructing an observed quantity, wherein when the positioning rod is in a zero-speed state, the actual speed of the sensing end p relative to the ground is zero. Meaning that the velocity of the inertial navigation solution projected through the lever arm to the touchdown point p should also be theoretically zero. Namely, zero-speed observation is formed:
z＝[0 0 0]T(5)
the observation matrix corresponding to the zero-speed correction can be obtained by the aforementioned formula (4).
4.2, constructing an observation equation of zero-speed correction; the speed relation between the speed of the inertial measurement unit and the contact point of the inertial navigation positioning rod is expressed as follows:
wherein b represents an inertial measurement unit carrier coordinate system, namely a system b; n represents a local horizontal coordinate system, also called a navigation coordinate system, n system; lbProjecting a three-dimensional vector of the center of the inertial measurement unit pointing to the contact point between the positioning rod and the ground under a system b;is a projection of the rotation angular velocity of b relative to n under b;
is a coordinate transformation matrix between an n system and a b system;
the projection of the velocity of the inertial measurement unit under the n system;
the speed of the location point of the positioning rod;
the velocity of the zero velocity point is calculated according to the navigation result of the inertial navigation system as follows:
the actual observations at the zero velocity point are:
wherein n represents the speed of the zero-speed point, and the value of n is not absolutely zero;
writing an observation equation of the zero-speed correction of the inertial navigation positioning rod:
and 4.3, zero-speed correction inertial navigation resolving, wherein when the positioning rod is in a zero-speed state, the information that the contact point speed is zero can be used as an external speed observed quantity of the combined Kalman filtering, data fusion is carried out with the inertial navigation resolving to obtain the optimal estimation of the position, the speed and the attitude, and meanwhile, the measurement error of an inertial sensor (a gyroscope and an accelerometer) is estimated and feedback correction is carried out.
The specific implementation of the kalman filtering algorithm for positioning and attitude determination by combining zero-speed and inertia is familiar to practitioners in the fields of inertial navigation and combined navigation.
4.4, the combined positioning and attitude determination is to perform data fusion by taking zero-speed information as external assistance and inertial navigation solution; the data fusion method includes but is not limited to Kalman filtering, extended Kalman filtering, unscented Kalman filtering, particle filtering, sequential least squares or artificial neural network.
It will be understood that modifications and variations can be made by persons skilled in the art in light of the above teachings and all such modifications and variations are intended to be included within the scope of the invention as defined in the appended claims.
Claims (10)
1. A portable inertial navigation positioning rod is characterized by comprising a carrier rod, a measurement sensor and a data processing unit; wherein:
measuring transducer fixed mounting is on the carrier pole, and measuring transducer includes: an inertia measurement unit and a zero-speed detection sensor; the inertia measurement unit is used for measuring three-dimensional specific force and three-dimensional angular velocity information; the zero-speed detection sensor is fixedly arranged on the carrier rod and carries out zero-speed detection by judging whether the output of the zero-speed detection sensor exceeds a set threshold value or not;
the data processing unit is used for processing the measurement information transmitted by the measurement sensor in real time, and performing data fusion and positioning and attitude determination calculation by combining zero-speed detection information through an inertial navigation mechanical arrangement algorithm and Kalman filtering.
2. The portable inertial navigation positioning lever of claim 1, wherein the shape of the carrier lever comprises: rod-shaped carriers, spheres, cylinders or cuboids.
3. The portable inertial navigation positioning lever of claim 1, wherein the zero-speed detection sensor comprises: pressure sensor, inertial sensor, elastic switch, user manual switch.
4. The portable inertial navigation positioning lever of claim 1, wherein the inertial measurement unit comprises a three-axis accelerometer and a three-axis gyroscope.
5. A positioning and attitude determination method for a portable inertial navigation positioning rod is characterized by comprising the following steps:
s1, positioning and initializing the inertial navigation positioning rod;
s2, in the positioning process, the inertial navigation positioning rod intermittently obtains a zero-speed correction opportunity;
s3, detecting the zero-speed time period of the inertial navigation positioning rod, judging whether one end of the inertial navigation positioning rod is in contact with the ground of the detection point and keeps still, and if so, recording the ground contact time period as the zero-speed time period;
and S4, acquiring a zero-speed observation value of the inertial navigation positioning rod in a touchdown time period, and performing data fusion with an inertial navigation system to realize combined positioning and attitude determination calculation.
6. The method of claim 1, wherein the step S1 of initializing positioning of the inertial navigation positioning lever comprises:
s11, performing positioning or positioning algorithm including resolving inertial navigation position, speed and attitude;
and S12, the positioning initialization of the inertial navigation positioning rod comprises position initialization, speed initialization and posture initialization.
7. The method for positioning and determining the posture of a portable inertial navigation positioning lever according to claim 1, wherein the inertial navigation positioning lever intermittently obtains the zero-velocity correction opportunity in step S2 by:
s21, operating the inertial navigation positioning rod to enable the speed of a certain mass point of the inertial navigation positioning rod relative to the earth or ground fixed point to be zero;
s22, the inertial measurement unit itself or the entire inertial navigation positioning lever is not required to have zero velocity, and if the velocity of a certain part of the inertial navigation positioning lever fixedly connected to the inertial measurement unit is zero, the inertial navigation positioning lever may be regarded as a zero velocity correction opportunity.
8. The method of claim 1, wherein the step S3 of detecting the zero-velocity time interval of the inertial navigation positioning lever comprises:
installing a pressure sensor at the contact point of the inertial navigation positioning rod and the ground object: sensing whether the pressure value of the point is in contact with a fixed point on the ground or not and keeping the point static by detecting the change of the pressure value of the point; when the pressure value of the pressure sensor exceeds a certain preset threshold value, judging that the part of the inertial navigation positioning rod is in contact with a ground fixed point, and recording a time period when the corresponding pressure is greater than the threshold value as a zero-speed correction time period;
an inertia measurement unit is arranged in the inertia navigation positioning rod: judging whether the inertial navigation positioning rod and the ground contact point are in a zero-speed state or not through the change of the original data of the inertial measurement unit;
an elastic switch is arranged at the contact point of the inertial navigation positioning rod and the ground object: when the inertial navigation positioning rod touches the ground, the elastic force borne by the elastic switch is increased, when the inertial navigation positioning rod touches the ground, the elastic force borne by the switch is reduced, and whether the inertial navigation positioning rod touches the ground or not is judged by judging the elastic force change of the switch to be in a zero-speed state;
installing a manual switch at the contact point of the inertial navigation positioning rod and the ground object: when the inertial navigation positioning rod touches the ground, a user turns on the manual switch, the inertial navigation positioning rod turns off the manual switch at the ground-off moment, and the ground-contact time period of the positioning rod is recorded.
9. The method for positioning and attitude determination of the portable inertial navigation positioning lever according to claim 1, wherein the specific method for implementing the combined positioning and attitude determination solution by data fusion in step S4 is as follows:
s41, when the inertial navigation positioning rod is in the zero-speed time period, recording a zero-speed observation value in the zero-speed time period, realizing data fusion of the zero-speed observation value and the inertial navigation system in a filtering mode, and performing combined positioning calculation;
s42, constructing an observation equation of zero-speed correction; the speed relation between the speed of the inertial measurement unit and the contact point of the inertial navigation positioning rod is expressed as follows:
wherein b represents an inertial measurement unit carrier coordinate system, namely a system b; n represents a local horizontal coordinate system, also called a navigation coordinate system, n system; lbProjecting a three-dimensional vector of the center of the inertial measurement unit pointing to the contact point between the positioning rod and the ground under a system b;
is a projection of the rotation angular velocity of b relative to n under b;
is a coordinate transformation matrix between an n system and a b system;
the projection of the velocity of the inertial measurement unit under the n system;
the speed of the location point of the positioning rod;
the velocity of the zero velocity point is calculated according to the navigation result of the inertial navigation system as follows:
the actual observations at the zero velocity point are:
wherein n represents the speed of the zero-speed point, and the value of n is not absolutely zero;
writing an observation equation of the zero-speed correction of the inertial navigation positioning rod:
s43, when the inertial navigation positioning rod is in a zero-speed state, the position of the touchdown point of the inertial navigation positioning rod is kept unchanged, the position information of the touchdown point with the unchanged position is used as the external observation of the inertial navigation, and the position divergence of an inertial navigation system is restrained;
s44, performing data fusion by taking zero-speed information as external assistance and inertial navigation solution; the data fusion method comprises Kalman filtering, extended Kalman filtering, unscented Kalman filtering, particle filtering, sequential least square method, nonlinear optimization algorithm or artificial neural network method.
10. The method of claim 9, wherein the step S43 of using the position information of the touchdown point as an external observation of the inertial navigation comprises:
s431, taking the position of the zero-speed starting moment as an external position observation and carrying out data fusion with an inertial navigation system;
s432, regarding the inertial navigation positioning rod as a rigid body rotating around the touch point, and combining the posture of the inertial navigation system and the lever arm lbAnd the speed and the position of the inertial measurement unit are calculated, and the speed and the position of the inertial navigation are updated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911099403.5A CN110763229B (en) | 2019-11-12 | 2019-11-12 | Portable inertial navigation positioning rod and positioning and attitude determining method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911099403.5A CN110763229B (en) | 2019-11-12 | 2019-11-12 | Portable inertial navigation positioning rod and positioning and attitude determining method thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110763229A true CN110763229A (en) | 2020-02-07 |
| CN110763229B CN110763229B (en) | 2022-04-15 |
Family
ID=69337217
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911099403.5A Active CN110763229B (en) | 2019-11-12 | 2019-11-12 | Portable inertial navigation positioning rod and positioning and attitude determining method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110763229B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111569124A (en) * | 2020-05-15 | 2020-08-25 | 李军 | Complicated special-shaped surface medical instrument disinfection device |
| CN113551665A (en) * | 2021-06-25 | 2021-10-26 | 中国科学院国家空间科学中心 | High dynamic motion state sensing system and sensing method for motion carrier |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6853909B2 (en) * | 2001-12-03 | 2005-02-08 | Applanix Corporation, Inc | Walking stick navigator for position determination |
| CN101484777A (en) * | 2005-12-15 | 2009-07-15 | 天宝导航有限公司 | Managed traverse system and method to acquire accurate survey data in absence of precise GPS data |
| CN104352039A (en) * | 2014-11-07 | 2015-02-18 | 北京中电华远科技有限公司 | Monitoring walking stick |
| CN104613963A (en) * | 2015-01-23 | 2015-05-13 | 南京师范大学 | Pedestrian navigation system and navigation positioning method based on kinesiology model |
| CN106908759A (en) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | A kind of indoor pedestrian navigation method based on UWB technology |
| CN107655476A (en) * | 2017-08-21 | 2018-02-02 | 南京航空航天大学 | Pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation |
| CN110146111A (en) * | 2019-06-03 | 2019-08-20 | 西安精准测控有限责任公司 | A kind of Initial Alignment Method of centering rod |
| CN110260860A (en) * | 2019-06-20 | 2019-09-20 | 武汉大学 | Indoor moving measurement and positioning method for determining posture and system based on foot inertial sensor |
| CN110346824A (en) * | 2019-07-15 | 2019-10-18 | 广东工业大学 | A kind of automobile navigation method, system, device and readable storage medium storing program for executing |
-
2019
- 2019-11-12 CN CN201911099403.5A patent/CN110763229B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6853909B2 (en) * | 2001-12-03 | 2005-02-08 | Applanix Corporation, Inc | Walking stick navigator for position determination |
| CN101484777A (en) * | 2005-12-15 | 2009-07-15 | 天宝导航有限公司 | Managed traverse system and method to acquire accurate survey data in absence of precise GPS data |
| CN104352039A (en) * | 2014-11-07 | 2015-02-18 | 北京中电华远科技有限公司 | Monitoring walking stick |
| CN104613963A (en) * | 2015-01-23 | 2015-05-13 | 南京师范大学 | Pedestrian navigation system and navigation positioning method based on kinesiology model |
| CN106908759A (en) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | A kind of indoor pedestrian navigation method based on UWB technology |
| CN107655476A (en) * | 2017-08-21 | 2018-02-02 | 南京航空航天大学 | Pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation |
| CN110146111A (en) * | 2019-06-03 | 2019-08-20 | 西安精准测控有限责任公司 | A kind of Initial Alignment Method of centering rod |
| CN110260860A (en) * | 2019-06-20 | 2019-09-20 | 武汉大学 | Indoor moving measurement and positioning method for determining posture and system based on foot inertial sensor |
| CN110346824A (en) * | 2019-07-15 | 2019-10-18 | 广东工业大学 | A kind of automobile navigation method, system, device and readable storage medium storing program for executing |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111569124A (en) * | 2020-05-15 | 2020-08-25 | 李军 | Complicated special-shaped surface medical instrument disinfection device |
| CN113551665A (en) * | 2021-06-25 | 2021-10-26 | 中国科学院国家空间科学中心 | High dynamic motion state sensing system and sensing method for motion carrier |
| CN113551665B (en) * | 2021-06-25 | 2023-08-11 | 中国科学院国家空间科学中心 | High-dynamic motion state sensing system and sensing method for motion carrier |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110763229B (en) | 2022-04-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20210208180A1 (en) | Correction of accumulated errors in inertial measurement units attached to a user | |
| JP6094026B2 (en) | Posture determination method, position calculation method, and posture determination apparatus | |
| CN111089587B (en) | Inclined RTK course initialization method | |
| KR102440358B1 (en) | Inertial-based navigation device and Inertia-based navigation method based on relative preintegration | |
| CN113093256B (en) | GNSS/IMU mapping system and method | |
| CN110986939B (en) | Visual inertia odometer method based on IMU (inertial measurement Unit) pre-integration | |
| CN107490378B (en) | Indoor positioning and navigation method based on MPU6050 and smart phone | |
| JP7023234B2 (en) | How to estimate pedestrian movement | |
| KR101922700B1 (en) | Method and Apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor | |
| WO2018214227A1 (en) | Unmanned vehicle real-time posture measurement method | |
| CN110763229B (en) | Portable inertial navigation positioning rod and positioning and attitude determining method thereof | |
| CN108387236B (en) | Polarized light SLAM method based on extended Kalman filtering | |
| CN112798021B (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
| CN110986941B (en) | Method for estimating installation angle of mobile phone | |
| CN109540135A (en) | The method and device that the detection of paddy field tractor pose and yaw angle are extracted | |
| JP2016033473A (en) | Position calculation method and position calculation device | |
| US7711516B2 (en) | Method for estimating movement of a solid | |
| CN114002725A (en) | Lane line auxiliary positioning method and device, electronic equipment and storage medium | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN108748220A (en) | A kind of foot based on Inertial Measurement Unit-machine follow-up control method and control system | |
| CN111307114B (en) | Water surface ship horizontal attitude measurement method based on motion reference unit | |
| CN110609621A (en) | Posture calibration method and human motion capture system based on micro-sensor | |
| CN108592907A (en) | A kind of quasi real time step-by-step movement pedestrian navigation method based on bidirectional filtering smoothing technique | |
| CN109297484A (en) | A kind of pedestrian's autonomous positioning error correcting method for thering is gait to constrain | |
| CN112904884B (en) | Method and device for tracking trajectory of foot type robot and readable storage medium |
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 |