+

CN111157984B - A Pedestrian Autonomous Navigation Method Based on Millimeter Wave Radar and Inertial Measurement Unit - Google Patents

A Pedestrian Autonomous Navigation Method Based on Millimeter Wave Radar and Inertial Measurement Unit Download PDF

Info

Publication number
CN111157984B
CN111157984B CN202010016854.4A CN202010016854A CN111157984B CN 111157984 B CN111157984 B CN 111157984B CN 202010016854 A CN202010016854 A CN 202010016854A CN 111157984 B CN111157984 B CN 111157984B
Authority
CN
China
Prior art keywords
foot
matrix
measurement unit
moment
wave radar
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.)
Expired - Fee Related
Application number
CN202010016854.4A
Other languages
Chinese (zh)
Other versions
CN111157984A (en
Inventor
阎波
李柯蒙
魏震益
肖卓凌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202010016854.4A priority Critical patent/CN111157984B/en
Publication of CN111157984A publication Critical patent/CN111157984A/en
Application granted granted Critical
Publication of CN111157984B publication Critical patent/CN111157984B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明公开了一种基于毫米波雷达及惯性测量单元的一种行人自主导航方法，其包括以下步骤：S1、在每只脚上安装一个惯性测量单元，并在任一只脚上安装毫米波雷达；S2、获取用户的步长信息和每只脚的模糊位置信息；S3、对用户双脚的模糊位置信息进行修正与融合，得到融合后的位置信息；S4、将融合后的信息作为用户的具体位置信息，判断是否继续导航，若是则返回步骤S2，否则结束导航。本方法通过毫米波雷达对双脚间的位姿信息进行测量，并将其作为融合卡尔曼滤波器的水平管测量，将累计误差的增长率的数量级从平方降低到了常数级，有效的降低了累积误差为整体定位精度产生的不良影响，对长距离、长时间的定位导航精度有显著的提高。

Figure 202010016854

The invention discloses a pedestrian autonomous navigation method based on a millimeter-wave radar and an inertial measurement unit, which includes the following steps: S1. Install an inertial measurement unit on each foot, and install a millimeter-wave radar on any foot ; S2, obtain the step length information of the user and the fuzzy position information of each foot; S3, correct and fuse the fuzzy position information of the user's feet to obtain the fused position information; S4, take the fused information as the user's For specific location information, determine whether to continue the navigation, if so, return to step S2, otherwise end the navigation. This method measures the pose information between the feet by millimeter wave radar, and uses it as the horizontal tube measurement of the fusion Kalman filter, which reduces the magnitude of the cumulative error growth rate from the square to the constant level, effectively reducing the The cumulative error is an adverse effect on the overall positioning accuracy, which significantly improves the long-distance and long-term positioning and navigation accuracy.

Figure 202010016854

Description

Pedestrian autonomous navigation method based on millimeter wave radar and inertial measurement unit
Technical Field
The invention relates to the field of navigation, in particular to a pedestrian autonomous navigation method based on a millimeter wave radar and an inertia measurement unit.
Background
With the rapid development of society, navigation technology is widely applied in both military and civil fields, and has been developed in recent years. In outdoor positioning, the global navigation positioning system has excellent performance, however, because the satellite signal penetrability is poor, the performance of the satellite positioning system is not satisfactory in indoor scenes such as high buildings, underground buildings and the like. The inertial navigation technology is a navigation technology which is not easily interfered by external environment, does not need to be deployed in advance, does not depend on the navigation technology, is low in cost, and has unique advantages in positioning under the condition without a GPS.
The inertial navigation system performs quadratic integration on the acceleration and the angular acceleration measured by an inertial sensor (an accelerometer and a gyroscope) and accumulates the acceleration and the angular acceleration at a known starting point to obtain a positioning and navigation track. In the process, noise is inevitably integrated and superposed, so that the influence of the noise on the positioning quality is increased, and a serious track divergence phenomenon occurs in long-distance and long-time positioning, so that the requirement of long-time positioning is difficult to meet.
Disclosure of Invention
Aiming at the defects in the prior art, the pedestrian autonomous navigation method based on the millimeter wave radar and the inertia measurement unit solves the problem that the existing inertia navigation system can generate accumulated errors.
In order to achieve the purpose of the invention, the invention adopts the technical scheme that:
the method for autonomous navigation of the pedestrian based on the millimeter wave radar and the inertial measurement unit comprises the following steps:
s1, installing an inertia measurement unit on each foot, and installing a millimeter wave radar on any foot;
s2, acquiring step length information of the pedestrian through a millimeter wave radar; acquiring fuzzy position information of each foot of the pedestrian by using the current position information as a reference through an inertial measurement unit;
s3, correcting and fusing the fuzzy position information of the two feet of the pedestrian by taking the step length information as the horizontal observation quantity of a Kalman filter for multi-sensor fusion to obtain fused position information;
and S4, taking the fused information as the specific position information of the pedestrian, judging whether to continue navigation, if so, returning to the step S2, and if not, ending the navigation.
Further, the specific method for acquiring the step size information of the pedestrian by the millimeter wave radar in step S2 includes the following sub-steps:
s2-1-1, transmitting signals through a millimeter wave radar and receiving the reflected signals;
s2-1-2, traversing each detected target according to the reflected signal, and acquiring the speed, the distance and the echo power of each detected target;
s2-1-3, removing the target with the speed of 0, removing the target with the distance greater than a first threshold value, and removing the target with the echo power less than a second threshold value;
s2-1-4, in keeping with the goal, according to the formula:
Figure GDA0003176856350000021
taking the normalized echo power as weight, and carrying out weighted average on each detected target at present to obtain step length information R of the pedestrian; wherein
Figure GDA0003176856350000022
The distance of the retained ith target;
Figure GDA0003176856350000023
the echo power of the ith target is retained.
Further, the specific method for acquiring the speed and the distance of each detected target in S2-1-2 is as follows:
and mixing the reflected signals to obtain the frequency Fs of the intermediate frequency signals, according to a formula:
Figure GDA0003176856350000024
obtaining the distance d of a target distance radar; arranging the obtained distances d according to rows, performing fast Fourier transform on each row of the arranged matrix, taking the rows of the matrix obtained after the transform as distance values, and taking the rows of the matrix obtained after the transform as speed values; wherein c is the speed of light; s is the rate of change of the signal frequency with time.
Further, the first threshold is 0.8 times the height of the pedestrian; the second threshold value is one thousandth of the transmitting power of the millimeter wave radar.
Further, the specific method for acquiring the fuzzy position information of each foot of the pedestrian through the inertial measurement unit in the step S2 includes the following sub-steps:
s2-2-1, establishing a body coordinate system of each foot by taking the position of the inertial measurement unit installed in each foot as a coordinate origin, reading data of a gyroscope in the inertial measurement unit, and according to a formula:
Ct+1＝Ct(2I3×3+ΩtΔt)(2I3×3-ΩtΔt)-1
Figure GDA0003176856350000031
acquiring an attitude matrix of each foot at the moment of t + 1; wherein C istIs the attitude matrix of the foot at time t; ct+1The attitude matrix of the foot at time t + 1; i is3×3Is a 3 × 3 identity matrix; omegatAn antisymmetric matrix of the foot angular rate at time t; delta t is the interval duration of adjacent moments; omegax、ωyAnd ωzThe angular rates in the x-axis direction, the y-axis direction and the z-axis direction read by the foot gyroscope at the moment t are respectively;
s2-2-2, establishing an inertial navigation coordinate system by taking the human body center of the pedestrian as the origin of coordinates, acquiring data of an accelerometer in an inertial measurement unit, and according to a formula:
Figure GDA0003176856350000032
Figure GDA0003176856350000033
pt+1＝pt+vt+1Δt
acquiring fuzzy position information of each foot at the moment of t + 1; wherein
Figure GDA0003176856350000034
The acceleration of the foot at the moment t under the inertial navigation coordinate system;
Figure GDA0003176856350000035
the acceleration of the foot in the body coordinate system at the moment t under the system is obtained; v. oftThe velocity of the foot at time t; v. oft+1The velocity of the foot at time t + 1; p is a radical oftPosition information of the foot at time t; p is a radical oft+1The fuzzy position information of the foot at the time t + 1.
Further, the specific method of step S3 includes the following sub-steps:
s3-1, according to the formula
Xa0＝(Ca0,pa0,va0)T,Xb0＝(Cb0,pb0,vb0)T
Respectively obtaining state vectors X of two feet of a pedestrian at t +1 momenta0And Xb0(ii) a Wherein C isa0、pa0And va0Respectively setting an attitude matrix, position information and speed corresponding to a time t +1 without setting a millimeter wave radar; cb0、pb0And vb0Respectively setting an attitude matrix, position information and speed corresponding to a foot t +1 moment of the millimeter wave radar;
s3-2, according to the formula:
Pt+1＝FtPtFt T+Q
Figure GDA0003176856350000041
Figure GDA0003176856350000042
updating an error covariance matrix corresponding to the t +1 moment of each foot; wherein P ist+1The error covariance matrix of the foot at time t + 1; ptAn error covariance matrix of the foot at time t; ftA state transition matrix at time t; q is state transition noise; (.)TIs the transposition of the matrix; stThe acceleration of the inertia measurement unit at the moment t;
Figure GDA0003176856350000043
the acceleration of the inertial measurement unit in the x-axis direction at the moment t is measured;
Figure GDA0003176856350000044
the acceleration of the inertia measurement unit in the y-axis direction at the moment t;
Figure GDA0003176856350000045
the acceleration of the inertia measurement unit in the z-axis direction at the moment t; 03×3A zero matrix of 3 × 3;
s3-3, judging whether the current speed is 0, if so, entering the step S3-4, otherwise, entering the step S3-7;
s3-4, according to the formula:
Kt+1＝Pt+1HT(HPt+1HT+M)-1
H＝{03×3 03×3 I3×3}
respectively obtaining Kalman gains K of two feet at t +1 momentt+1(ii) a The method comprises the following steps that H is an observation matrix, the positions of two zero matrixes in H respectively correspond to the positions of an attitude matrix and position information in an observed state vector at the moment of t +1 of a foot, and a unit matrix in H corresponds to the position information of speed in the observed state vector at the moment of t +1 of the foot; m is observation noise;
s3-5, according to the formula:
P’t+1＝(I9×9-Kt+1H)Pt+1
correcting the error covariance matrix of each foot at the moment of t +1 to obtain a corrected error covariance matrix P'; wherein P't+1The corrected error covariance matrix corresponding to the foot at the time t + 1; i is9×9Is a 9 × 9 identity matrix;
s3-6, according to the formula:
ε＝(εc,εp,εv)T＝Kt+1·vt+1
C't+1＝(2I3×3+ε’cΔt)(2I3×3-ε’cΔt)-1Ct+1
p’t+1＝pt+1-εp
v’t+1＝vt+1-εv
Xa1＝(Ca1,pa1,va1)T,Xb1＝(Cb1,pb1,vb1)T
correcting the attitude matrix C of the foot where the inertial measurement unit is located at the moment t +1t+1Position information p of the foot where the inertial measurement unit is locatedt+1And the velocity v of the foot on which the inertial measurement unit is locatedt+1And updating the state vector of the foot at the moment t +1 to respectively obtain a posture matrix C 'of the foot where the inertial measurement unit is located after correction't+1And position information p 'of the foot where the corrected inertia measurement unit is located't+1And the velocity v 'of the foot where the corrected inertia measurement unit is located't+1And an updated state vector Xa1And Xb1(ii) a Wherein epsilon'cIs a parameter epsiloncAn antisymmetric matrix of (a); epsilon is the correction data; epsilonpAnd εvAre all intermediate parameters; parameter epsilonc、εpAnd εvIs given a value of Kt+1·vt+1Determining; ca1、pa1And va1Respectively corresponding to the corrected attitude matrix, position information and speed at the time of a pin t +1 without the millimeter wave radar; cb1、pb1And vb1Respectively setting a posture matrix, position information and speed which are corrected correspondingly to the time t +1 of the millimeter wave radar;
s3-7, according to the formula:
Zradar＝Crn·(R+ρr2i)+Cln·ρa2i
obtaining relative position information Z between two body coordinate systems at t +1 momentradar(ii) a Wherein C isrnA rotation matrix from a body coordinate system corresponding to a foot of the millimeter wave radar to an inertial navigation coordinate system is set at the moment t + 1; clnA rotation matrix from a body coordinate system corresponding to the other foot to an inertial navigation coordinate system at the moment of t + 1; r is step length information; rhor2iThe direction vector of the millimeter wave radar to the inertia measurement unit of the foot is shown; rhoa2iIs the inertia from the reflection point of the millimeter wave radar to the foot where the reflection point is positionedA direction vector of the sexual measurement unit;
s3-8, according to the formula:
Figure GDA0003176856350000061
X＝(Ca,pa,va,Cb,pb,vb)T
splicing the latest error covariance matrixes respectively corresponding to the left foot and the right foot into a covariance matrix PfusionSplicing the latest state vectors corresponding to the left foot and the right foot into a state vector X; wherein P isaSetting a latest error covariance matrix corresponding to a pin without the millimeter wave radar; pbSetting a latest error covariance matrix corresponding to a foot of the millimeter wave radar; ca、paAnd vaThe latest attitude matrix, the position information and the speed corresponding to the foot without the millimeter wave radar are respectively set; cb、pbAnd vbThe latest attitude matrix, the position information and the speed corresponding to the foot provided with the millimeter wave radar are respectively set;
s3-9, according to the formula:
Figure GDA0003176856350000062
obtaining a fused Kalman gain Kfusion(ii) a Wherein the observation matrix Hfusion＝(03×3I3×303×303×3-I3×303×3)；RfusionThe current observation noise;
s3-10, according to the formula:
P'fusion＝(I18×18-KfusionHfusion)Pfusion
X'＝X+Kfusion(Zradar-HfusionX)
fusion update covariance matrix PfusionAnd the state vector X is obtained to obtain a covariance matrix P 'after fusion updating'fusionAnd the fused state vector X', namely the real covariance matrix and state vector of the pedestrian at the moment of t + 1; the fused state vector X' includes fused position information.
The invention has the beneficial effects that: the method measures the pose information between the two feet through the millimeter wave radar, and takes the pose information as the horizontal tube measurement of the fusion Kalman filter, so that the magnitude order of the increase rate of the accumulated error is reduced from square to constant, the adverse effect of the accumulated error on the integral positioning precision is effectively reduced, and the positioning navigation precision in a long distance and a long time is obviously improved.
Drawings
FIG. 1 is a schematic flow diagram of the present invention;
FIG. 2 is a diagram of the positioning effect of a conventional inertial navigation system when a pedestrian walks;
FIG. 3 is a diagram showing the positioning effect of the method when pedestrians walk on the same path;
FIG. 4 is a diagram of the error accumulation distribution function of the present method and the conventional inertial navigation method.
Detailed Description
The following description of the embodiments of the present invention is provided to facilitate the understanding of the present invention by those skilled in the art, but it should be understood that the present invention is not limited to the scope of the embodiments, and it will be apparent to those skilled in the art that various changes may be made without departing from the spirit and scope of the invention as defined and defined in the appended claims, and all matters produced by the invention using the inventive concept are protected.
As shown in fig. 1, the method for autonomous navigation of a pedestrian based on a millimeter wave radar and an inertial measurement unit includes the following steps:
s1, installing an inertia measurement unit on each foot, and installing a millimeter wave radar on any foot;
s2, acquiring step length information of the pedestrian through a millimeter wave radar; acquiring fuzzy position information of each foot of the pedestrian by using the current position information as a reference through an inertial measurement unit;
s3, correcting and fusing the fuzzy position information of the two feet of the pedestrian by taking the step length information as the horizontal observation quantity of a Kalman filter for multi-sensor fusion to obtain fused position information;
and S4, taking the fused information as the specific position information of the pedestrian, judging whether to continue navigation, if so, returning to the step S2, and if not, ending the navigation.
The specific method for acquiring the step length information of the pedestrian through the millimeter wave radar in the step S2 includes the following substeps:
s2-1-1, transmitting signals through a millimeter wave radar and receiving the reflected signals;
s2-1-2, traversing each detected target according to the reflected signal, and acquiring the speed, the distance and the echo power of each detected target;
s2-1-3, removing the target with the speed of 0, removing the target with the distance greater than a first threshold value, and removing the target with the echo power less than a second threshold value; the first threshold value is 0.8 times of the height of the pedestrian; the second threshold value is one thousandth of the transmitting power of the millimeter wave radar;
s2-1-4, in keeping with the goal, according to the formula:
Figure GDA0003176856350000081
taking the normalized echo power as weight, and carrying out weighted average on each detected target at present to obtain step length information R of the pedestrian; wherein
Figure GDA0003176856350000082
The distance of the retained ith target;
Figure GDA0003176856350000083
the echo power of the ith target is retained.
The specific method for acquiring the speed and distance of each detected target in the step S2-1-2 is as follows: and mixing the reflected signals to obtain the frequency Fs of the intermediate frequency signals, according to a formula:
Figure GDA0003176856350000084
obtaining the distance d of a target distance radar; arranging the obtained distances d according to rows, performing fast Fourier transform on each row of the arranged matrix, taking the rows of the matrix obtained after the transform as distance values, and taking the rows of the matrix obtained after the transform as speed values; wherein c is the speed of light; s is the rate of change of the signal frequency with time.
The specific method for acquiring the fuzzy position information of each foot of the pedestrian through the inertial measurement unit in the step S2 comprises the following sub-steps:
s2-2-1, establishing a body coordinate system of each foot by taking the position of the inertial measurement unit installed in each foot as a coordinate origin, reading data of a gyroscope in the inertial measurement unit, and according to a formula:
Ct+1＝Ct(2I3×3+ΩtΔt)(2I3×3-ΩtΔt)-1
Figure GDA0003176856350000091
acquiring an attitude matrix of each foot at the moment of t + 1; wherein C istIs the attitude matrix of the foot at time t; ct+1The attitude matrix of the foot at time t + 1; i is3×3Is a 3 × 3 identity matrix; omegatAn antisymmetric matrix of the foot angular rate at time t; delta t is the interval duration of adjacent moments; omegax、ωyAnd ωzThe angular rates in the x-axis direction, the y-axis direction and the z-axis direction read by the foot gyroscope at the moment t are respectively;
s2-2-2, establishing an inertial navigation coordinate system by taking the human body center of the pedestrian as the origin of coordinates, acquiring data of an accelerometer in an inertial measurement unit, and according to a formula:
Figure GDA0003176856350000092
Figure GDA0003176856350000093
pt+1＝pt+vt+1Δt
acquiring fuzzy position information of each foot at the moment of t + 1; wherein
Figure GDA0003176856350000094
The acceleration of the foot at the moment t under the inertial navigation coordinate system;
Figure GDA0003176856350000095
the acceleration of the foot in the body coordinate system at the moment t under the system is obtained; v. oftThe velocity of the foot at time t; v. oft+1The velocity of the foot at time t + 1; p is a radical oftPosition information of the foot at time t; p is a radical oft+1The fuzzy position information of the foot at the time t + 1.
The specific method of step S3 includes the following substeps:
s3-1, according to the formula
Xa0＝(Ca0,pa0,va0)T,Xb0＝(Cb0,pb0,vb0)T
Respectively obtaining state vectors X of two feet of a pedestrian at t +1 momenta0And Xb0(ii) a Wherein C isa0、pa0And va0Respectively setting an attitude matrix, position information and speed corresponding to a time t +1 without setting a millimeter wave radar; cb0、pb0And vb0Respectively setting an attitude matrix, position information and speed corresponding to a foot t +1 moment of the millimeter wave radar;
s3-2, according to the formula:
Pt+1＝FtPtFt T+Q
Figure GDA0003176856350000101
Figure GDA0003176856350000102
updating an error covariance matrix corresponding to the t +1 moment of each foot; wherein P ist+1The error covariance matrix of the foot at time t + 1; ptAn error covariance matrix of the foot at time t; ftA state transition matrix at time t; q is state transition noise; (.)TIs the transposition of the matrix; stThe acceleration of the inertia measurement unit at the moment t;
Figure GDA0003176856350000103
the acceleration of the inertial measurement unit in the x-axis direction at the moment t is measured;
Figure GDA0003176856350000104
the acceleration of the inertia measurement unit in the y-axis direction at the moment t;
Figure GDA0003176856350000105
the acceleration of the inertia measurement unit in the z-axis direction at the moment t; 03×3A zero matrix of 3 × 3;
s3-3, judging whether the current speed is 0, if so, entering the step S3-4, otherwise, entering the step S3-7;
s3-4, according to the formula:
Kt+1＝Pt+1HT(HPt+1HT+M)-1
H＝{03×3 03×3 I3×3}
respectively obtaining Kalman gains K of two feet at t +1 momentt+1(ii) a The method comprises the following steps that H is an observation matrix, the positions of two zero matrixes in H respectively correspond to the positions of an attitude matrix and position information in an observed state vector at the moment of t +1 of a foot, and a unit matrix in H corresponds to the position information of speed in the observed state vector at the moment of t +1 of the foot; m is observation noise;
s3-5, according to the formula:
P’t+1＝(I9×9-Kt+1H)Pt+1
correcting the error covariance matrix of each foot at the moment of t +1 to obtain a corrected error covariance matrix P'; wherein P't+1The corrected error covariance matrix corresponding to the foot at the time t + 1; i is9×9Is a 9 × 9 identity matrix;
s3-6, according to the formula:
ε＝(εc,εp,εv)T＝Kt+1·vt+1
C't+1＝(2I3×3+ε’cΔt)(2I3×3-ε’cΔt)-1Ct+1
p’t+1＝pt+1-εp
v’t+1＝vt+1-εv
Xa1＝(Ca1,pa1,va1)T,Xb1＝(Cb1,pb1,vb1)T
correcting the attitude matrix C of the foot where the inertial measurement unit is located at the moment t +1t+1Position information p of the foot where the inertial measurement unit is locatedt+1And the velocity v of the foot on which the inertial measurement unit is locatedt+1And updating the state vector of the foot at the moment t +1 to respectively obtain a posture matrix C 'of the foot where the inertial measurement unit is located after correction't+1And position information p 'of the foot where the corrected inertia measurement unit is located't+1And the velocity v 'of the foot where the corrected inertia measurement unit is located't+1And an updated state vector Xa1And Xb1(ii) a Wherein epsilon'cIs a parameter epsiloncAn antisymmetric matrix of (a); epsilon is the correction data; epsilonpAnd εvAre all intermediate parameters; parameter epsilonc、εpAnd εvIs given a value of Kt+1·vt+1Determining; ca1、pa1And va1Respectively corresponding to the corrected attitude matrix, position information and speed at the time of a pin t +1 without the millimeter wave radar; cb1、pb1And vb1Respectively is provided with a hairThe attitude matrix, the position information and the speed after correction corresponding to the moment t +1 of the meter-wave radar are obtained;
s3-7, according to the formula:
Zradar＝Crn·(R+ρr2i)+Cln·ρa2i
obtaining relative position information Z between two body coordinate systems at t +1 momentradar(ii) a Wherein C isrnA rotation matrix from a body coordinate system corresponding to a foot of the millimeter wave radar to an inertial navigation coordinate system is set at the moment t + 1; clnA rotation matrix from a body coordinate system corresponding to the other foot to an inertial navigation coordinate system at the moment of t + 1; r is step length information; rhor2iThe direction vector of the millimeter wave radar to the inertia measurement unit of the foot is shown; rhoa2iThe direction vector from the reflection point of the millimeter wave radar to the inertial measurement unit of the foot where the reflection point is located;
s3-8, according to the formula:
Figure GDA0003176856350000121
X＝(Ca,pa,va,Cb,pb,vb)T
splicing the latest error covariance matrixes respectively corresponding to the left foot and the right foot into a covariance matrix PfusionSplicing the latest state vectors corresponding to the left foot and the right foot into a state vector X; wherein P isaSetting a latest error covariance matrix corresponding to a pin without the millimeter wave radar; pbSetting a latest error covariance matrix corresponding to a foot of the millimeter wave radar; ca、paAnd vaThe latest attitude matrix, the position information and the speed corresponding to the foot without the millimeter wave radar are respectively set; cb、pbAnd vbThe latest attitude matrix, the position information and the speed corresponding to the foot provided with the millimeter wave radar are respectively set;
s3-9, according to the formula:
Figure GDA0003176856350000122
obtaining a fused Kalman gain Kfusion(ii) a Wherein the observation matrix Hfusion＝(03×3I3×303×303×3-I3×303×3)；RfusionThe current observation noise;
s3-10, according to the formula:
P'fusion＝(I18×18-KfusionHfusion)Pfusion
X'＝X+Kfusion(Zradar-HfusionX)
fusion update covariance matrix PfusionAnd the state vector X is obtained to obtain a covariance matrix P 'after fusion updating'fusionAnd the fused state vector X', namely the real covariance matrix and state vector of the pedestrian at the moment of t + 1; the fused state vector X' includes fused position information.
In the specific implementation process, when a person normally walks, one foot is stepped forwards to contact the ground to support the body, and the foot with the speed of 0 is the foot on the ground.
In an embodiment of the invention, the same pedestrian repeatedly walks in the same path by using the traditional inertial navigation system and the method at the same time, as shown in fig. 2, 3 and 4, the positioning effect of the method is more fit with the actual walking path, the generated error accumulation is smaller, the positioning result drift is smaller, and the positioning precision is higher.
In conclusion, the method measures the pose information between the two feet through the millimeter wave radar, and measures the pose information as the horizontal tube of the fusion Kalman filter, so that the magnitude order of the increase rate of the accumulated error is reduced from square to a constant level, the adverse effect of the accumulated error on the integral positioning precision is effectively reduced, and the long-distance and long-time positioning navigation precision is remarkably improved.

Claims (5)

1. A pedestrian autonomous navigation method based on a millimeter wave radar and an inertia measurement unit is characterized by comprising the following steps:
s1, installing an inertia measurement unit on each foot, and installing a millimeter wave radar on any foot;
s2, acquiring step length information of the pedestrian through a millimeter wave radar; acquiring fuzzy position information of each foot of the pedestrian by using the current position information as a reference through an inertial measurement unit;
s3, correcting and fusing the fuzzy position information of the two feet of the pedestrian by taking the step length information as the horizontal observation quantity of a Kalman filter for multi-sensor fusion to obtain fused position information;
s4, taking the fused information as the specific position information of the pedestrian, judging whether to continue navigation, if so, returning to the step S2, otherwise, ending the navigation;
the specific method for acquiring the step length information of the pedestrian through the millimeter wave radar in the step S2 includes the following substeps:
s2-1-1, transmitting signals through a millimeter wave radar and receiving the reflected signals;
s2-1-2, traversing each detected target according to the reflected signal, and acquiring the speed, the distance and the echo power of each detected target;
s2-1-3, removing the target with the speed of 0, removing the target with the distance greater than a first threshold value, and removing the target with the echo power less than a second threshold value;
s2-1-4, in keeping with the goal, according to the formula:
Figure FDA0003214596170000011
taking the normalized echo power as weight, and carrying out weighted average on each detected target at present to obtain step length information R of the pedestrian; wherein
Figure FDA0003214596170000012
The distance of the retained ith target;
Figure FDA0003214596170000013
the echo power of the ith target is retained.
2. The method for pedestrian autonomous navigation based on millimeter wave radar and inertial measurement unit according to claim 1, wherein the specific method for obtaining the speed and distance of each detected target in S2-1-2 is as follows:
and mixing the reflected signals to obtain the frequency Fs of the intermediate frequency signals, according to a formula:
Figure FDA0003214596170000021
obtaining the distance d of a target distance radar; arranging the obtained distances d according to rows, performing fast Fourier transform on each row of the arranged matrix, taking the rows of the matrix obtained after the transform as distance values, and taking the rows of the matrix obtained after the transform as speed values; wherein c is the speed of light; s is the rate of change of the signal frequency with time.
3. The method for pedestrian autonomous navigation based on millimeter wave radar and inertial measurement unit of claim 1, wherein the first threshold is 0.8 times the height of the pedestrian; the second threshold value is one thousandth of the transmitting power of the millimeter wave radar.
4. The method for pedestrian autonomous navigation based on millimeter wave radar and inertial measurement unit of claim 1, wherein the specific method for obtaining the fuzzy position information of each foot of the pedestrian through the inertial measurement unit in step S2 comprises the following sub-steps:
s2-2-1, establishing a body coordinate system of each foot by taking the position of the inertial measurement unit installed in each foot as a coordinate origin, reading data of a gyroscope in the inertial measurement unit, and according to a formula:
Ct+1＝Ct(2I3×3+ΩtΔt)(2I3×3-ΩtΔt)-1
Figure FDA0003214596170000022
acquiring an attitude matrix of each foot at the moment of t + 1; wherein C istIs the attitude matrix of the foot at time t; ct+1The attitude matrix of the foot at time t + 1; i is3×3Is a 3 × 3 identity matrix; omegatAn antisymmetric matrix of the foot angular rate at time t; delta t is the interval duration of adjacent moments; omegax、ωyAnd ωzThe angular rates in the x-axis direction, the y-axis direction and the z-axis direction read by the foot gyroscope at the moment t are respectively;
s2-2-2, establishing an inertial navigation coordinate system by taking the human body center of the pedestrian as the origin of coordinates, acquiring data of an accelerometer in an inertial measurement unit, and according to a formula:
Figure FDA0003214596170000031
Figure FDA0003214596170000032
pt+1＝pt+vt+1Δt
acquiring fuzzy position information of each foot at the moment of t + 1; wherein
Figure FDA0003214596170000033
The acceleration of the foot at the moment t under the inertial navigation coordinate system;
Figure FDA0003214596170000034
the acceleration of the foot in the body coordinate system at the moment t under the system is obtained; v. oftThe velocity of the foot at time t; v. oft+1The velocity of the foot at time t + 1; p is a radical oftPosition information of the foot at time t; p is a radical oft+1The fuzzy position information of the foot at the time t + 1.
5. The method for pedestrian autonomous navigation based on millimeter wave radar and inertial measurement unit according to claim 4, wherein the specific method of step S3 comprises the following sub-steps:
s3-1, according to the formula
Xa0＝(Ca0,pa0,va0)T,Xb0＝(Cb0,pb0,vb0)T
Respectively obtaining state vectors X of two feet of a pedestrian at t +1 momenta0And Xb0(ii) a Wherein C isa0、pa0And va0Respectively setting an attitude matrix, position information and speed corresponding to a time t +1 without setting a millimeter wave radar; cb0、pb0And vb0Respectively setting an attitude matrix, position information and speed corresponding to a foot t +1 moment of the millimeter wave radar;
s3-2, according to the formula:
Pt+1＝FtPtFt T+Q
Figure FDA0003214596170000035
Figure FDA0003214596170000036
updating an error covariance matrix corresponding to the t +1 moment of each foot; wherein P ist+1The error covariance matrix of the foot at time t + 1; ptAn error covariance matrix of the foot at time t; ftA state transition matrix at time t; q is state transition noise; (.)TIs the transposition of the matrix; stThe acceleration of the inertia measurement unit at the moment t;
Figure FDA0003214596170000041
the acceleration of the inertial measurement unit in the x-axis direction at the moment t is measured;
Figure FDA0003214596170000042
the acceleration of the inertia measurement unit in the y-axis direction at the moment t;
Figure FDA0003214596170000043
the acceleration of the inertia measurement unit in the z-axis direction at the moment t; 03×3A zero matrix of 3 × 3;
s3-3, judging whether the current speed is 0, if so, entering the step S3-4, otherwise, entering the step S3-7;
s3-4, according to the formula:
Kt+1＝Pt+1HT(HPt+1HT+M)-1
H＝{03×3 03×3 I3×3}
respectively obtaining Kalman gains K of two feet at t +1 momentt+1(ii) a The method comprises the following steps that H is an observation matrix, the positions of two zero matrixes in H respectively correspond to the positions of an attitude matrix and position information in an observed state vector at the moment of t +1 of a foot, and a unit matrix in H corresponds to the position information of speed in the observed state vector at the moment of t +1 of the foot; m is observation noise;
s3-5, according to the formula:
P′t+1＝(I9×9-Kt+1H)Pt+1
correcting the error covariance matrix of each foot at the moment of t +1 to obtain a corrected error covariance matrix P'; wherein P't+1The corrected error covariance matrix corresponding to the foot at the time t + 1; i is9×9Is a 9 × 9 identity matrix;
s3-6, according to the formula:
ε＝(εc,εp,εv)T＝Kt+1·vt+1
C't+1＝(2I3×3+ε′cΔt)(2I3×3-ε′cΔt)-1Ct+1
p′t+1＝pt+1-εp
v′t+1＝vt+1-εv
Xa1＝(Ca1,pa1,va1)T,Xb1＝(Cb1,pb1,vb1)T
correcting the attitude matrix C of the foot where the inertial measurement unit is located at the moment t +1t+1Position information p of the foot where the inertial measurement unit is locatedt+1And the velocity v of the foot on which the inertial measurement unit is locatedt+1And updating the state vector of the foot at the moment t +1 to respectively obtain a posture matrix C 'of the foot where the inertial measurement unit is located after correction't+1And position information p 'of the foot where the corrected inertia measurement unit is located't+1And the velocity v 'of the foot where the corrected inertia measurement unit is located't+1And an updated state vector Xa1And Xb1(ii) a Wherein epsilon'cIs a parameter epsiloncAn antisymmetric matrix of (a); epsilon is the correction data; epsilonpAnd εvAre all intermediate parameters; parameter epsilonc、εpAnd εvIs given a value of Kt+1·vt+1Determining; ca1、pa1And va1Respectively corresponding to the corrected attitude matrix, position information and speed at the time of a pin t +1 without the millimeter wave radar; cb1、pb1And vb1Respectively setting a posture matrix, position information and speed which are corrected correspondingly to the time t +1 of the millimeter wave radar;
s3-7, according to the formula:
Zradar＝Crn·(R+ρr2i)+Cln·ρa2i
obtaining relative position information Z between two body coordinate systems at t +1 momentradar(ii) a Wherein C isrnA rotation matrix from a body coordinate system corresponding to a foot of the millimeter wave radar to an inertial navigation coordinate system is set at the moment t + 1; clnA rotation matrix from a body coordinate system corresponding to the other foot to an inertial navigation coordinate system at the moment of t + 1; r is step length information; rhor2iIs the inertia of the millimeter wave radar to the footMeasuring a directional vector of the cell; rhoa2iThe direction vector from the reflection point of the millimeter wave radar to the inertial measurement unit of the foot where the reflection point is located;
s3-8, according to the formula:
Figure FDA0003214596170000051
X＝(Ca,pa,va,Cb,pb,vb)T
splicing the latest error covariance matrixes respectively corresponding to the left foot and the right foot into a covariance matrix PfusionSplicing the latest state vectors corresponding to the left foot and the right foot into a state vector X; wherein P isaSetting a latest error covariance matrix corresponding to a pin without the millimeter wave radar; pbSetting a latest error covariance matrix corresponding to a foot of the millimeter wave radar; ca、paAnd vaThe latest attitude matrix, the position information and the speed corresponding to the foot without the millimeter wave radar are respectively set; cb、pbAnd vbThe latest attitude matrix, the position information and the speed corresponding to the foot provided with the millimeter wave radar are respectively set;
s3-9, according to the formula:
Figure FDA0003214596170000061
obtaining a fused Kalman gain Kfusion(ii) a Wherein the observation matrix Hfusion＝(03×3 I3×3 03×3 03×3 -I3×3 03×3)；RfusionThe current observation noise;
s3-10, according to the formula:
P'fusion＝(I18×18-KfusionHfusion)Pfusion
X'＝X+Kfusion(Zradar-HfusionX)
fusion update covariance matrix PfusionAnd the state vector X is obtained to obtain a covariance matrix P 'after fusion updating'fusionAnd the fused state vector X', namely the real covariance matrix and state vector of the pedestrian at the moment of t + 1; the fused state vector X' includes fused position information.
CN202010016854.4A 2020-01-08 2020-01-08 A Pedestrian Autonomous Navigation Method Based on Millimeter Wave Radar and Inertial Measurement Unit Expired - Fee Related CN111157984B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010016854.4A CN111157984B (en) 2020-01-08 2020-01-08 A Pedestrian Autonomous Navigation Method Based on Millimeter Wave Radar and Inertial Measurement Unit

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010016854.4A CN111157984B (en) 2020-01-08 2020-01-08 A Pedestrian Autonomous Navigation Method Based on Millimeter Wave Radar and Inertial Measurement Unit

Publications (2)

Publication Number Publication Date
CN111157984A CN111157984A (en) 2020-05-15
CN111157984B true CN111157984B (en) 2021-10-22

Family

ID=70561909

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010016854.4A Expired - Fee Related CN111157984B (en) 2020-01-08 2020-01-08 A Pedestrian Autonomous Navigation Method Based on Millimeter Wave Radar and Inertial Measurement Unit

Country Status (1)

Country Link
CN (1) CN111157984B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112379344B (en) * 2020-11-09 2024-04-02 中国科学院电子学研究所 Signal compensation method and device, equipment and storage medium
CN113091733A (en) * 2021-03-15 2021-07-09 武汉大学 A real-time positioning device and method based on the fusion of millimeter wave radar and IMU
CN113848545B (en) * 2021-09-01 2023-04-14 电子科技大学 A Fusion Target Detection and Tracking Method Based on Vision and Millimeter Wave Radar
CN115355907A (en) * 2022-10-19 2022-11-18 东南大学 Autonomous navigation system and method based on millimeter wave sensing integrated unmanned equipment
CN116700319B (en) * 2023-08-04 2023-10-20 西安交通大学 Aerial robot autonomous takeoff and landing system and method based on micro radar array
CN119334350A (en) * 2024-09-26 2025-01-21 清华大学 A pedestrian positioning method based on the fusion of millimeter wave radar and inertial measurement unit

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2555806A (en) * 2016-11-09 2018-05-16 Atlantic Inertial Systems Ltd A navigation system
CN109282806B (en) * 2017-07-20 2024-03-22 罗伯特·博世有限公司 Method, apparatus and storage medium for determining pedestrian position
US10788570B2 (en) * 2017-09-29 2020-09-29 The Boeing Company Radar system for mobile platform and method of use
CN108180923B (en) * 2017-12-08 2020-10-20 北京理工大学 An Inertial Navigation and Positioning Method Based on Human Odometer
CN108836346A (en) * 2018-04-16 2018-11-20 大连理工大学 A kind of Human Body Gait Analysis method and system based on inertial sensor

Also Published As

Publication number Publication date
CN111157984A (en) 2020-05-15

Similar Documents

Publication Publication Date Title
CN111157984B (en) A Pedestrian Autonomous Navigation Method Based on Millimeter Wave Radar and Inertial Measurement Unit
US11506512B2 (en) Method and system using tightly coupled radar positioning to improve map performance
CN111077549B (en) Position data correction method, apparatus and computer readable storage medium
CN114396943B (en) Fusion positioning method and terminal
US11525682B2 (en) Host vehicle position estimation device
CN110057356B (en) Method and device for locating vehicle in tunnel
CN114166221B (en) Auxiliary transportation robot positioning method and system in dynamic complex mine environment
CN110702091A (en) High-precision positioning method for moving robot along subway rail
CN114234969B (en) Navigation positioning method and device and electronic equipment
CN113237482B (en) Robust vehicle positioning method in urban canyon environment based on factor graph
JP2024103654A (en) Measurement accuracy calculation device, self-position estimation device, control method, program, and storage medium
CN109945864B (en) Indoor driving positioning fusion method, device, storage medium and terminal device
CN115343738A (en) Integrated navigation method and device based on GNSS-RTK and IMU
Qian et al. A LiDAR aiding ambiguity resolution method using fuzzy one-to-many feature matching
JP7418196B2 (en) Travel trajectory estimation method and travel trajectory estimation device
JP2016206149A (en) Gradient estimation apparatus and program
RU2556286C1 (en) Measurement of aircraft heading
Xu et al. An innovation-based cycle-slip, multipath estimation, detection and mitigation method for tightly coupled GNSS/INS/Vision navigation in urban areas
TWI635302B (en) Real-time precise positioning system of vehicle
JP2023002082A (en) MAP UPDATE DEVICE, MAP UPDATE METHOD AND MAP UPDATE COMPUTER PROGRAM
CN118746841A (en) Mobile laser radar trajectory correction method and device based on automatic tracking of unmanned aerial vehicles
JP2019203795A (en) Dynamic coordinate management device, dynamic coordinate management method, and program
CN112923934A (en) Laser SLAM technology suitable for combining inertial navigation in unstructured scene
CN117685933A (en) A method and system for relative coordinate positioning of inspection equipment in large-span steel box beams
CN111256708A (en) Vehicle-mounted integrated navigation method based on radio frequency identification

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20211022

点击 这是indexloc提供的php浏览器服务，不要输入任何密码和下载