A pedestrian inertial navigation positioning method based on the human lower limb DH model

Abstract

The invention discloses a pedestrian inertial navigation positioning method based on a human lower limb DH model, comprising the following steps: (1) collecting pedestrian foot-bound IMU data and preprocessing it; (2) using the DH criterion to establish a human lower limb model; installing and calibrating sensors for four node positions of the legs; (3) solving the two-node IMU data of the left and right feet in real time and performing zero-speed detection and zero-speed correction; (4) collecting the four-node data of the legs, solving the attitude angle in real time, and then solving the bipedal distance vector in real time through forward kinematics; (5) establishing a bipedal distance equation constraint based on the left and right bipedal state quantities obtained in step (3) and the bipedal distance vector solved in step (4), and correcting the position, speed and attitude in combination with a filtering method to obtain the corrected speed, attitude and position. The invention solves the heading divergence problem in pedestrian inertial navigation, reduces the cumulative error of pedestrian navigation positioning, and improves the navigation positioning accuracy.

Description

Technical Field

[0001] The invention belongs to the technical field of pedestrian inertial navigation systems and relates to a pedestrian inertial navigation positioning method based on a human lower limb DH model. Background Art

[0002] With the continuous improvement of MEMS technology and the increasing popularity of its application, pedestrian inertial navigation technology has developed rapidly. At present, most pedestrian inertial navigation methods use strapdown inertial navigation technology, which connects the inertial sensor to the pedestrian's feet, performs kinematic analysis based on the contact state between the foot and the ground, and uses the zero velocity update (ZUPT) method to achieve a highly autonomous and low-cost pedestrian navigation system.

[0003] The zero-speed correction method establishes a pseudo-observation of zero foot velocity in the support phase, which can periodically correct the inertial navigation system error and is an effective means to constrain the divergence of inertial navigation errors. However, the heading state in the pedestrian inertial navigation system has poor observability, and heading drift can cause large position errors, affecting the navigation positioning accuracy. It is difficult to meet the reliable pedestrian navigation and positioning needs using only the zero-speed correction method.

[0004] The application with patent number 2020108957717 discloses a pedestrian navigation and positioning method based on four-node motion constraints of the human body. According to the IMU data of the pedestrians collected first, the pedestrian's posture, speed, and position information are solved, and a pedestrian motion gait detection model is established. Zero-speed correction is performed and then the pedestrian's heading is corrected according to a filtering method based on bipedal inequality constraints. However, the accuracy of the navigation system using this method is still insufficient. Summary of the invention

[0005] Purpose of the invention: In view of the above shortcomings, the present invention provides a pedestrian inertial navigation and positioning method based on the human lower limb DH model, which solves the heading divergence problem in pedestrian inertial navigation, reduces the cumulative error of pedestrian navigation and positioning, and improves the navigation and positioning accuracy.

[0006] Technical solution: To solve the above problems, the present invention provides a pedestrian inertial navigation positioning method based on the human lower limb DH model, comprising the following steps:

[0007] (1) Collect pedestrian foot-mounted IMU data and pre-process it;

[0008] (2) Using the DH criterion, a human lower limb model is established; when the human lower limb is in a static stage, sensors are installed and calibrated at the positions of the left thigh, right thigh, left calf, and right calf;

[0009] (3) Real-time calculation of the IMU data of the left ankle joint node and the right ankle joint node, a total of two nodes, and zero-speed detection is performed. When the detection result is in the zero-speed state, zero-speed correction is performed, and the state quantities of the left and right feet are obtained during the correction process;

[0010] (4) collecting data of four nodes of the leg through the sensor of step (2) and performing real-time posture calculation to obtain a real-time updated posture angle, and solving the bipedal distance vector in real time through forward kinematics; the four nodes are respectively a left leg hip joint node, a right leg hip joint node, a left leg knee joint node, and a right leg knee joint node;

[0011] (5) Based on the state quantities of the left and right feet obtained in step (3), the bipedal distance vector calculated in step (4) is combined to establish a bipedal distance equation constraint, and the position, velocity and posture are corrected in combination with a filtering method to obtain the corrected velocity, posture and position.

[0012] Furthermore, step (4) is specifically as follows:

[0013] (4.1) Collect data from four nodes of the leg, and use the strapdown inertial navigation algorithm to solve the IMU attitude angles of the four nodes of the leg in real time. The attitude angles include the heading angle ψ, the pitch angle θ, and the roll angle γ; the four nodes are the left leg hip joint node, the right leg hip joint node, the left leg knee joint node, and the right leg knee joint node;

[0014] (4.2) Using forward kinematics to solve the bipedal distance vector

[0015] (4.2.1) According to the established DH model parameters, the homogeneous transformation matrix between the joints of a single leg node is obtained The joint i includes joint 1, joint 2, joint 3, and joint 4, wherein joint 1, joint 2, and joint 3 correspond to three degrees of freedom of the hip joint, and joint 4 corresponds to one degree of freedom of the knee joint; The formula is:

[0016]

[0017] Among them, θ i represents the rotation angle of joint i, i is 1, 2, 3, 4; θ1 is the IMU heading angle of the hip joint node of a single leg; θ2 is the IMU roll angle of the hip joint node of a single leg, θ3 is the IMU pitch angle of the hip joint node of a single leg; θ4 is the IMU pitch angle of the knee joint node of a single leg, and the values ​​of θ1, θ2, θ3, and θ4 are all taken from the values ​​of the node attitude angles of the same leg; α i represents the torsion angle of joint i, d i represents the offset of the link between joint i and joint i-1;

[0018] (4.2.2) Get the homogeneous transformation matrix of the hip joint and the ankle joint of a single leg node The formula is:

[0019]

[0020] (4.2.3) Extraction The first three elements in the fourth column obtain the relative position of the hip joint and the ankle joint of a single leg node.

[0021] (4.2.4) Relative position relationship of the two-node IMU of the foot It is expressed as:

[0022]

[0023] in, Indicates the relative position of the left foot-mounted IMU and the left hip joint. Indicates the relative position between the right foot-mounted IMU and the right hip joint; T = [-d 0 0] T It represents the relative position of the left and right hip joints, and d represents the distance between the left and right hip joints.

[0024] Furthermore, step (5) specifically includes:

[0025] (5.1) Combine the two-legged state variables and construct the system equation:

[0026] X=[X L X R ]

[0027] Among them, X L Indicates the navigation error state of the left foot IMU, X R Represents the navigation error state of the right foot IMU;

[0028] (5.2) According to X L and X R The calculated relative distance modulus of the bipedal IMU is:

[0029] d c =f(L·X)

[0030] Among them, f( ) is the vector Euclidean distance solution function, L = [0 3×6 I 3×3 0 3×15 -I 3×3 0 3×9 ];

[0031] (5.3) The relative distance modulus between the two-node IMU of the foot is obtained according to the bipedal distance vector:

[0032]

[0033] (5.4) Establish the bipedal equality constraint and obtain the measurement equation:

[0034] Z dis =δd=d c -d DH =f(L·X)-d DH

[0035] (5.5) After applying the extended Kalman filter to obtain the optimal estimated error state according to the system equation and measurement equation, the position, velocity, attitude and IMU device errors are corrected again.

[0036] Furthermore, the calibration formula of the sensor in step (2) is:

[0037]

[0038] Among them, R trans represents the initial rotation matrix, θ s represents the initial attitude pitch angle, γ s represents the roll angle of the initial attitude, ψ s The heading angle representing the initial attitude.

[0039] Furthermore, the preprocessing in step (1) specifically includes the following steps:

[0040] (1.1) Calculate and deduct the gyroscope zero bias from the IMU static data b :

[0041]

[0042] Wherein, s0 represents the start time of the static interval, s1 represents the end time of the static interval, and W represents the gyroscope output;

[0043] (1.2) The initial attitude angle is calculated using the specific force output of the static accelerometer, including the foot pitch angle θ and foot roll angle γ:

[0044]

[0045]

[0046] Among them, g x g is the X-axis specific force output of the foot IMU; y g is the specific force output of the Y axis of the foot IMU; z It is the specific force output of the Z-axis of the foot IMU.

[0047] Furthermore, the real-time solutions in step (2) and step (3) both adopt the strapdown inertial navigation method, specifically:

[0048] The posture update algorithm formula is as follows:

[0049]

[0050] Where Q represents the quaternion matrix, t k-1 represents the k-1 moment, t k represents the k moment, i.e. the next sampling moment after k-1 moment; ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle. To express a gesture;

[0051] The speed update algorithm formula is as follows:

[0052]

[0053] in, represents the differential velocity of the carrier in the geographic frame at time k+1, v n (t k+1 ) is the velocity of the carrier in the geographic system at time k+1; Represents the coordinate transformation matrix from the aircraft system to the navigation system; represents the specific force output of the accelerometer; is the projection of the earth's rotation angular velocity in the navigation coordinate system, It represents the projection of the angular velocity of the navigation system relative to the earth system in the navigation coordinate system; g n represents the local gravitational acceleration;

[0054] The location update algorithm formula is as follows:

[0055]

[0056] Among them, L represents longitude, λ represents latitude, and h represents altitude; R m Represents the radius of curvature of the Earth's meridian plane, R n V represents the radius of curvature of the Earth's umbo plane; E represents the velocity vector in the east direction, V N represents the velocity vector in the north direction, V U represents the velocity vector in the celestial direction; Δt represents the sampling time interval.

[0057] Furthermore, the zero speed detection in step (3) is specifically as follows:

[0058]

[0059]

[0060]

[0061]

[0062] ZVD(k)=S1(k)&S2(k)&S3(k)&S4(k)

[0063] in, represents the mean value of acceleration from time k to time k+N, σ A,k:k+n represents the standard deviation of acceleration from time k to time k+N, represents the mean value of the angular velocity from time k to time k+N, σ W,k:k+N represents the standard deviation of the angular velocity from the kth to k+Nth time; T A,min is the minimum threshold of acceleration, T A,max is the maximum threshold of acceleration, T σA is the threshold value of the acceleration standard deviation, T W is the threshold of angular velocity, is the threshold of the angular velocity standard deviation; when S1(k), S2(k), S3(k), and S4(k) are all 1 at the same time, ZVD(k)=1, indicating that the current state is zero speed.

[0064] Furthermore, the zero speed correction algorithm in step (3) is specifically:

[0065] When the foot is detected to be in a zero-speed state, the Kalman filter is used to calculate the error and construct an 18-dimensional Kalman filter state quantity X:

[0066]

[0067] in, represents the platform error angle in the northeast direction, δV represents the velocity error in the northeast direction, and δP represents the position error; ε b represents the gyro bias error, ε r represents the first-order Markov drift error of the gyro, ▽ a represents the first-order Markov drift error of the accelerometer;

[0068] Establish the state equation:

[0069]

[0070] Where A represents the state transfer matrix, G represents the system noise transfer matrix, and W represents the system noise;

[0071] Establish the measurement equation:

[0072] Z zupt =δV=Vn -V ZUPT =HX+v

[0073] Among them, Z zupt Represents the observed value, which is the IMU velocity error δV, V n is the IMU velocity vector obtained by strapdown inertial navigation, V ZUPT The theoretical value is [0 0 0] T , H is the observation matrix, v is the observation noise matrix, which represents the uncertainty of velocity observation;

[0074] Construct a Kalman filter based on the state equation and measurement equation to obtain the optimal estimated state quantity

[0075] Furthermore, in step (5.5), the extended Kalman filter is applied using the system equation and the measurement equation as follows:

[0076]

[0077]

[0078]

[0079]

[0080] Among them, A represents the state transfer matrix, Q represents the state transfer covariance matrix, represents the prior estimated covariance at time k, P k-1 represents the posterior estimated covariance at time k-1, K k represents the Kalman filter gain, J represents the Jacobian matrix, R represents the measurement noise covariance, Z represents the observed value, represents the prior estimate of the state at time k obtained from the system equation, X k The optimal estimate obtained by solving the Kalman filter, represents the posterior estimated covariance at time k, and I represents the identity matrix.

[0081] Furthermore, after obtaining the measurement equation in step (5.4), Taylor series expansion is performed and the linear terms are retained to obtain the linear error measurement equation:

[0082] Z dis =δd=d c -d DH =J·X+n d

[0083] Among them, n d is the observation noise matrix, and J represents the Jacobian matrix of the nonlinear function relative to the system state vector X.

[0084] Beneficial effect: Compared with the prior art, the significant advantages of the present invention are: by establishing a DH model and using the real-time updated attitude angles of the four nodes of the legs to solve the bipedal distance vector, combining the state quantities obtained from the bipedal to construct a bipedal equality constraint for error correction, effectively compensating for the problem that the zero-speed correction algorithm is difficult to constrain the heading drift, and effectively suppressing the heading error divergence of the inertial navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] Figure 1 Shown is a flow chart of the present invention;

[0086] Figure 2 Shown is the DH model constructed in the present invention;

[0087] Figure 3 Shown is a schematic diagram of the joint degrees of freedom of the four nodes of the leg in the DH model constructed by the present invention;

[0088] Figure 4 The figure shows the installation and calibration diagram of the four-node IMU of the leg in the present invention;

[0089] Figure 5 The figure shows a comparison between the positioning results of the method of the present invention and the results of the traditional method. DETAILED DESCRIPTION

[0090] The technical solution of the present invention is further described below in conjunction with the accompanying drawings.

[0091] like Figure 1 As shown, a pedestrian inertial navigation positioning method based on the human lower limb DH model includes the following steps:

[0092] Step 1: Collect the pedestrian's foot-bound IMU data and use the 1000 static data output to perform gyroscope zero bias correction and initial attitude solution.

[0093] (1) Calculate and deduct the gyroscope bias from the IMU static data b :

[0094]

[0095] Wherein, s0 represents the start time of the static interval, s1 represents the end time of the static interval, and W represents the gyroscope output;

[0096] (2) Use the specific force output of the static accelerometer to calculate the initial attitude angle, including the foot pitch angle θ and foot roll angle γ:

[0097]

[0098]

[0099] Among them, g x g is the X-axis specific force output of the foot IMU; y g is the specific force output of the Y axis of the foot IMU; z It is the specific force output of the Z-axis of the foot IMU.

[0100] Step 2: If Figure 2 As shown, the Denavit-Hartenberg (DH) criterion is used to establish a human lower limb model. In the static stage, sensors are installed and calibrated at the positions of the left thigh, right thigh, left calf, and right calf; data of four nodes of the leg are collected by sensors, and the four nodes are respectively the left leg hip joint node, the right leg hip joint node, the left leg knee joint node, and the right leg knee joint node; mainly including:

[0101] (1) Ignore the slight differences between limbs and the slight translation between joints, and determine the degrees of freedom of movement of the lower limb joints: Figure 3 As shown, the hip joint has 3 degrees of freedom and the knee joint has 1 degree of freedom;

[0102] (2) Determine the length of the pedestrian's thigh and calf, with the thigh length being l1 and the calf length being l2, and the width of the left and right hip joints being d;

[0103] (3) The leg DH model parameters are established according to the length of the lower limbs of the human body, as shown in Table 1:

[0104] Table 1

[0105]

[0106] Among them, Figure 3 As shown, θ i Represents the rotation angle of joint i, i is 1, 2, 3, 4; (joint i is not one of the four joint nodes of the leg, joint i corresponds to the degrees of freedom represented by each of the four joint nodes; joint 1, joint 2, and joint 3 correspond to three degrees of freedom of the hip joint respectively, and joint 4 corresponds to one degree of freedom of the knee joint).

[0107] The values ​​of θ1~θ4 are provided by the leg IMU attitude angle. Specifically, the value of θ1 is the IMU heading angle of the hip joint node of a single leg; the value of θ2 is the IMU roll angle of the hip joint node of a single leg; the value of θ3 is the IMU pitch angle of the hip joint node of a single leg; the value of θ4 is the IMU pitch angle of the knee joint node of a single leg, and the values ​​of θ1, θ2, θ3, and θ4 are all taken from the values ​​of the node attitude angles of the same leg; α i represents the torsion angle of joint i, d i represents the offset of the link between joint i and joint i-1;

[0108] d irepresents the offset of the connecting rod between joint i and joint i-1; ignore the slight differences in limbs and slight translations between joints. When the human body stands normally, the lines connecting the hip, knee, and ankle joints are perpendicular to the horizontal plane, so they are all set to 0; a i represents the length of the connecting rod between joint i and joint i-1; α i Represents the torsion angle and offset of joint i i represents the compensation angle of joint i; when the human body model is determined, only θ i It needs to be solved with the help of IMU, and the other parameters have been determined;

[0109] (4) When installing the leg IMU, due to the irregularity of human skin, there are some bumps and inclinations. In order to achieve the consistency of the three-axis coordinate system of the leg four-node IMU, Figure 4 As shown, the sensor installation calibration is required. The sensor installation calibration is specifically performed through the initial rotation matrix R trans To achieve this, in the initial alignment stage, the sensor performs an initial attitude solution to obtain the pitch angle θ s and the roll angle γ s and heading angle ψ s In theory, the lower limbs of the human body are naturally perpendicular to the ground, and the three-axis coordinate system of the lower limbs is consistent with the geographical system. Therefore, the initial rotation matrix R trans The calculation formula is:

[0110]

[0111] Among them, R trans represents the initial rotation matrix, θ s represents the initial attitude pitch angle, γ s represents the roll angle of the initial attitude, ψ s The heading angle representing the initial attitude.

[0112] Step 3: Calculate the two-node IMU data of the left and right feet in real time, and perform zero-speed detection synchronously. Use the zero-speed update algorithm to correct the cumulative error of the two-node navigation of the foot to obtain the corrected speed, posture and position. Specifically, take the center of the foot-bound inertial sensor as the coordinate origin O, the right direction is the X-axis, the Y-axis is perpendicular to the X-axis and points forward, and the Z-axis points upward. The three axes satisfy the right-hand screw rule. Collect 1000 static data output by the foot-bound inertial sensor, and obtain the acceleration and angular velocity thresholds for zero-speed detection based on the data characteristics of the acceleration and angular velocity in the initial static interval.

[0113] (1) Use the strapdown inertial navigation algorithm to calculate the attitude, velocity and position of the two-node IMU of the foot;

[0114] The quaternion attitude update algorithm formula is as follows:

[0115]

[0116]

[0117] Where Q represents the quaternion matrix, t k-1 represents the k-1 moment, t k represents the kth moment, i.e., the next sampling moment after the k-1th moment, Δθ represents the angle increment of the three axes of the gyroscope during the sampling time, ΔΘ represents the antisymmetric matrix composed of Δθ, ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle. To express a gesture;

[0118] The speed update algorithm formula is as follows:

[0119]

[0120] in, represents the differential velocity of the carrier in the geographic frame at time k+1, v n (t k+1 ) is the velocity of the carrier in the geographic system at time k+1; Represents the coordinate transformation matrix from the aircraft system to the navigation system; represents the specific force output of the accelerometer; is the projection of the earth's rotation angular velocity in the navigation coordinate system, It represents the projection of the angular velocity of the navigation system relative to the earth system in the navigation coordinate system; g n represents the local gravitational acceleration;

[0121] The location update algorithm formula is as follows:

[0122]

[0123] Among them, L represents longitude, λ represents latitude, and h represents altitude; R m Represents the radius of curvature of the Earth's meridian plane, R n V represents the radius of curvature of the Earth's umbo plane; E represents the velocity vector in the east direction, V N represents the velocity vector in the north direction, V U represents the velocity vector in the celestial direction; Δt represents the sampling time interval.

[0124] (2) Zero-speed detection is performed using acceleration modulus, angular velocity standard deviation, acceleration modulus, and angular velocity standard deviation as gait detection conditions;

[0125]

[0126]

[0127]

[0128]

[0129] ZVD(k)=S1(k)&S2(k)&S3(k)&S4(k)

[0130] in, represents the mean value of acceleration from time k to time k+N, σ A,k:k+n represents the standard deviation of acceleration from time k to time k+N, represents the mean value of the angular velocity from time k to time k+N, σ W,k:k+N represents the standard deviation of the angular velocity from the kth to k+Nth time; T A,min is the minimum threshold of acceleration, T A,max is the maximum threshold of acceleration, is the threshold value of the acceleration standard deviation, T W is the threshold of angular velocity, is the threshold of the angular velocity standard deviation; when S1(k), S2(k), S3(k), and S4(k) are all 1 at the same time, ZVD(k)=1, indicating that the current state is zero speed.

[0131] (3) When the foot is detected to be in a zero-speed state, the Kalman filter is used to calculate the error estimation to obtain the corrected position and speed, otherwise no correction is performed;

[0132] (3.1) When the foot is detected to be in a zero-speed state, the Kalman filter is used to perform error estimation and construct an 18-dimensional Kalman filter state quantity X:

[0133]

[0134] in, Respectively represent the platform error angles in the east, north, and celestial directions; δV=[δV E δV N δV U ] T are the velocity errors in the east, north and celestial directions respectively; δP=[δL δλ δh] T Represent the errors of longitude, latitude and altitude respectively; Respectively represent the zero bias errors of the gyroscope X-axis, Y-axis, and Z-axis; Represent the first-order Markov drift errors of the gyroscope X-axis, Y-axis, and Z-axis respectively; They represent the first-order Markov drift errors of the accelerometer X-axis, Y-axis, and Z-axis respectively;

[0135] (3.2) Establish the state equation:

[0136]

[0137] Where A represents the state transfer matrix, G represents the system noise transfer matrix, and W represents the system noise;

[0138] (3.3) Establish the measurement equation:

[0139] Z zupt =δV=V n -V ZUPT =HX+v

[0140] Among them, Z zupt Represents the observed value, which is the IMU velocity error δV, V n is the IMU velocity vector obtained by strapdown inertial navigation, V ZUPT The theoretical value is [0 0 0] T , H is the observation matrix, v is the observation noise matrix, which represents the uncertainty of velocity observation;

[0141] (3.4) Construct a Kalman filter based on the state equation and the measurement equation, and solve it to obtain the optimal estimated state quantity That is, the estimated error of attitude, velocity and position; deduct the first nine-dimensional state quantity from the strapdown inertial navigation solution result in step (3.2) Get the corrected attitude, velocity and position, and deduct the nine-dimensional state from the IMU data Correct the zero bias error and first-order Markov drift error of IMU.

[0142] The above method is used to solve the IMU data collected from the left and right feet to obtain the navigation error state quantity X of the left foot IMU. L 、The navigation error state quantity X of the right foot IMU R .

[0143] Step 4: Use DH model forward kinematics to solve the bipedal distance in real time;

[0144] (4.1) Collect data from the four nodes of the leg, and use the strapdown inertial navigation algorithm to calculate the IMU attitude angles of the four nodes of the leg in real time. The attitude angles are the heading angle ψ, the pitch angle θ, and the roll angle γ;

[0145] (4.2) Using forward kinematics to solve the bipedal distance vector

[0146] (4.2.1) According to the DH model parameters, obtain the homogeneous transformation matrix between the four joint nodes in the left leg or right leg This matrix represents the spatial position and posture relationship of joint i relative to joint i-1, and the formula is:

[0147]

[0148] (4.2.2) Obtain the homogeneous transformation matrix between the left leg hip joint and the left ankle joint or the right leg hip joint and the right ankle joint The formula is:

[0149]

[0150] (4.2.3) Extraction The first three elements in the fourth column get the relative position of the left leg hip joint and the left ankle joint or the right leg hip joint and the right ankle joint

[0151] (4.2.4) Relative position relationship of the two-node IMU of the foot It is expressed as:

[0152]

[0153] in, Indicates the relative position of the left foot-mounted IMU and the left hip joint. Indicates the relative position between the right foot-mounted IMU and the right hip joint; T = [-d 0 0] T It represents the relative position of the left and right hip joints, and d represents the distance between the left and right hip joints.

[0154] Step 5: Calculate the bipedal distance under the strapdown inertial navigation solution at each sampling time, and establish the bipedal distance equality constraint;

[0155] (5.1) Combine the two-legged state variables and construct the system equation:

[0156] X=[X L X R ]

[0157] Among them, X L Indicates the navigation error state of the left foot IMU, X R Represents the navigation error state of the right foot IMU;

[0158] (5.2) According to X L and X R The calculated relative distance modulus of the bipedal IMU is:

[0159] d c =f(L·X)

[0160] Where f() is the vector Euclidean distance solution function, L = [0 3×6 I 3×3 0 3×15 -I 3×3 0 3×9 ];

[0161] (5.3) According to the lower limb DH model and the calculated bipedal distance vector, the relative distance modulus between the two IMU nodes installed on the foot is:

[0162]

[0163] (5.4) Introduce the bipedal equality constraint based on the DH model, establish the bipedal equality constraint equation, and construct the measurement equation:

[0164] Z dis =δd=d c -d DH =f(L·X)-d DH

[0165] After the nonlinear equation above is expanded by Taylor series and the linear terms are retained, the linear error measurement equation is obtained:

[0166] Z dis =δd=d c -d DH =J·X+n d

[0167] Among them, n d is the observation noise matrix, and J represents the Jacobian matrix of the nonlinear function relative to the system state vector X.

[0168]

[0169]

[0170] Among them, x L Indicates the corrected X-axis position of the left foot IMU, y L Indicates the Y-axis position of the corrected left foot IMU, z L Indicates the corrected Z-axis position of the left foot IMU; x R Indicates the corrected X-axis position of the right foot IMU, y R Indicates the corrected Y-axis position of the right foot IMU, z R represents the corrected Z-axis position of the right foot IMU; ρ represents the bipedal distance obtained by the DH model.

[0171] (5.5) After applying the extended Kalman filter to the system equations and measurement equations obtained according to the above steps (5.1) and (5.4) to obtain the optimal estimated error state, the position, velocity, attitude and IMU device errors are corrected again.

[0172] Specifically:

[0173]

[0174]

[0175]

[0176]

[0177] Among them, A represents the state transfer matrix, Q represents the state transfer covariance matrix, represents the prior estimated covariance at time k, P k-1 represents the posterior estimated covariance at time k-1, K k represents the Kalman filter gain, J represents the Jacobian matrix, R represents the measurement noise covariance, Z represents the observed value, represents the prior estimate of the state at time k obtained from the system equation, X k The optimal estimate obtained by solving the Kalman filter, represents the posterior estimated covariance at time k, and I represents the identity matrix;

[0178] Solve the optimal estimated error state quantity X k Finally, the position, velocity, attitude and IMU device errors are corrected again.

[0179] like Figure 5 As shown, the experimental scene is in the teaching laboratory building. The route is a rectangle with a length of 56.8m and a width of 47m. The total length of the route is 622.8m. The tester walks at 1.5m / s. The solution result of the traditional zero-speed correction method is shown as the dotted line in the figure, and the error of the start and end points is greater than 20 meters; the solution result of the pedestrian navigation and positioning method based on the human lower limb DH model of the present invention is shown as the solid line in the figure, the heading error is greatly reduced, the error of the start and end positions is 1.7 meters, and the positioning accuracy is greatly improved.

Claims

1. A pedestrian inertial navigation positioning method based on the human lower limb DH model, characterized in that: The following steps are involved: (1) Collect pedestrian foot-mounted IMU data and pre-process it; The preprocessing includes the following steps: (1.1) Calculate and deduct the gyroscope bias from the IMU static data Wherein, s0 represents the start time of the static interval, s1 represents the end time of the static interval, and W represents the gyroscope output; (1.2) The initial attitude angle is calculated using the specific force output of the static accelerometer, including the foot pitch angle θ and foot roll angle γ: Among them, g x g is the X-axis specific force output of the foot IMU; y g is the specific force output of the Y axis of the foot IMU; z It is the specific force output of the Z axis of the foot IMU; (2) Using the DH criterion, a human lower limb model is established; when the human lower limb is in a static stage, sensors are installed and calibrated at the positions of the left thigh, right thigh, left calf, and right calf; (3) Real-time calculation of the IMU data of the left ankle joint node and the right ankle joint node, a total of two nodes, and zero-speed detection is performed. When the detection result is in the zero-speed state, zero-speed correction is performed, and the state quantities of the left and right feet are obtained during the correction process; (4) collecting data of four nodes of the legs through the sensor of step (2) and performing real-time posture calculation to obtain a real-time updated posture angle, and calculating the bipedal distance vector in real time through forward kinematics; the four nodes are respectively a left leg hip joint node, a right leg hip joint node, a left leg knee joint node, and a right leg knee joint node; comprising the following sub-steps: (4.1) Collect data from the four nodes of the leg and use the strapdown inertial navigation algorithm to calculate the IMU attitude angles of the four nodes of the leg in real time. The attitude angles include the heading angle ψ, the pitch angle θ, and the roll angle γ; (4.2) Using forward kinematics to solve the bipedal distance vector (4.2.1) According to the established DH model parameters, the homogeneous transformation matrix between the joints of a single leg is obtained The joints include joint 1, joint 2, joint 3, and joint 4, wherein joint 1, joint 2, and joint 3 correspond to three degrees of freedom of the hip joint, and joint 4 corresponds to one degree of freedom of the knee joint; The formula is: Among them, θ i represents the rotation angle of joint i, i is 1, 2, 3, 4; θ1 is the IMU heading angle of the hip joint node of a single leg; θ2 is the IMU roll angle of the hip joint node of a single leg, θ3 is the IMU pitch angle of the hip joint node of a single leg; θ4 is the IMU pitch angle of the knee joint node of a single leg, and the values ​​of θ1, θ2, θ3, and θ4 are all taken from the values ​​of the node attitude angles of the same leg; α i represents the torsion angle of joint i, d i represents the offset of the link between joint i and joint i-1; (4.2.2) Get the homogeneous transformation matrix of the hip joint and the ankle joint of a single leg node The formula is: (4.2.3) Extraction The first three elements in the fourth column obtain the relative position of the hip joint and the ankle joint of a single leg node. (4.2.4) Relative position relationship of the two-node IMU of the foot It is expressed as: in, Indicates the relative position of the left foot-mounted IMU and the left hip joint. Indicates the relative position between the right foot-mounted IMU and the right hip joint; T = [-d00] T represents the relative position of the left and right hip joints, and d represents the distance between the left and right hip joints; (5) according to the state quantities of the left and right bipeds obtained in step (3), in combination with the biped distance vector obtained by solving step (4), a biped distance equation constraint is established, and the position, speed and posture are corrected in combination with a filtering method to obtain a corrected speed, posture and position; the method comprises the following sub-steps: (5.1) Combine the two-legged state variables and construct the system equation: X=[X L X R ] Among them, X L Indicates the navigation error state of the left foot IMU, X R Represents the navigation error state of the right foot IMU; (5.2) According to X L and X R The calculated relative distance modulus of the bipedal IMU is: d c =f(L·X) Where f() is the vector Euclidean distance solution function, L = [0 3×6 I 3×3 0 3×15 -I 3×3 0 3×9 ]; (5.3) The relative distance modulus between the two-node IMU of the foot is obtained according to the bipedal distance vector: (5.4) Establish the bipedal equality constraint and obtain the measurement equation: Z dis =δd=d c -d DH =f(L·X)-d DH (5.5) After applying the extended Kalman filter to obtain the optimal estimated error state according to the system equation and measurement equation, the position, velocity, attitude and IMU device errors are corrected again.

2. The pedestrian inertial navigation positioning method based on the human lower limb DH model according to claim 1 is characterized in that: The calibration formula for the sensor in step (2) is: Among them, R trans represents the initial rotation matrix, θ s represents the initial attitude pitch angle, γ s represents the roll angle of the initial attitude, ψ s The heading angle representing the initial attitude.

3. The pedestrian inertial navigation positioning method based on the human lower limb DH model according to claim 1 is characterized in that: The real-time solutions in steps (2) and (3) both adopt the strapdown inertial navigation method, specifically: The posture update algorithm formula is as follows: Where Q represents the quaternion matrix, t k-1 represents the k-1 moment, t k represents the k moment, i.e. the next sampling moment after k-1 moment; ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle. To express a gesture; The speed update algorithm formula is as follows: in, represents the differential velocity of the carrier in the geographic frame at time k+1, v n (t k+1 ) is the velocity of the carrier in the geographic system at time k+1; Represents the coordinate transformation matrix from the aircraft system to the navigation system; represents the specific force output of the accelerometer; is the projection of the earth's rotation angular velocity in the navigation coordinate system, It represents the projection of the angular velocity of the navigation system relative to the earth system in the navigation coordinate system; g n represents the local gravitational acceleration; The location update algorithm formula is as follows: Among them, L represents longitude, λ represents latitude, and h represents altitude; R m Represents the radius of curvature of the Earth's meridian plane, R n V represents the radius of curvature of the Earth's umbo plane; E represents the velocity vector in the east direction, V N represents the velocity vector in the north direction, V U represents the velocity vector in the celestial direction; Δt represents the sampling time interval.

4. The pedestrian inertial navigation positioning method based on the human lower limb DH model according to claim 1 is characterized in that: The zero speed detection in step (3) is specifically as follows: ZVD(k)=S1(k)&S2(k)&S3(k)&S4(k) in, represents the mean value of acceleration from time k to time k+N, σ A,k:k+n represents the standard deviation of acceleration from time k to time k+N, represents the mean value of the angular velocity from time k to time k+N, σ W,k:k+N represents the standard deviation of the angular velocity from the kth to k+Nth time; T A,min is the minimum threshold of acceleration, T A,max is the maximum threshold of acceleration, is the threshold value of the acceleration standard deviation, T W is the threshold of angular velocity, is the threshold of the angular velocity standard deviation; when S1(k), S2(k), S3(k), and S4(k) are all 1 at the same time, ZVD(k)=1, indicating that the current state is zero speed.

5. The pedestrian inertial navigation positioning method based on the human lower limb DH model according to claim 4 is characterized in that: The zero speed correction algorithm in step (3) is specifically: When the foot is detected to be in a zero-speed state, the Kalman filter is used to calculate the error and construct an 18-dimensional Kalman filter state quantity X: in, represents the platform error angle in the northeast direction, δV represents the velocity error in the northeast direction, and δP represents the position error; ε b represents the gyro bias error, ε r represents the first-order Markov drift error of the gyro, represents the first-order Markov drift error of the accelerometer; Establish the state equation: Where A represents the state transfer matrix, G represents the system noise transfer matrix, and W represents the system noise; Establish the measurement equation: Z zupt =δV=V n -V ZUPT =HX+v Among them, Z zupt Represents the observed value, which is the IMU velocity error δV, V n is the IMU velocity vector obtained by strapdown inertial navigation, V ZUPT The theoretical value is [000] T , H is the observation matrix, v is the observation noise matrix, which represents the uncertainty of velocity observation; Construct a Kalman filter based on the state equation and measurement equation to obtain the optimal estimated state quantity 6. The pedestrian inertial navigation positioning method based on the human lower limb DH model according to claim 1 is characterized in that: In step (5.5), the extended Kalman filter is applied using the system equation and the measurement equation as follows: Among them, A represents the state transfer matrix, Q represents the state transfer covariance matrix, represents the prior estimated covariance at time k, P k-1 represents the posterior estimated covariance at time k-1, K k represents the Kalman filter gain, J represents the Jacobian matrix, R represents the measurement noise covariance, Z represents the observed value, represents the prior estimate of the state at time k obtained from the system equation, X k The optimal estimate obtained by solving the Kalman filter, represents the posterior estimated covariance at time k, and I represents the identity matrix.

7. The pedestrian inertial navigation positioning method based on the human lower limb DH model according to claim 1 is characterized in that: After obtaining the measurement equation in step (5.4), perform Taylor series expansion and retain the linear terms to obtain the linear error measurement equation: Z dis =δd=d c -d DH =J·X+n d Among them, n d is the observation noise matrix, and J represents the Jacobian matrix of the nonlinear function relative to the system state vector X.

Images