Human inertial odometry based on kinematic model optimization

Abstract

The application provides a human body inertial odometer based on a kinematic model optimization, MIMU is fixed on a calf, a human body odometer kinematic model with speed information as output is constructed, MIMU installation parameters, odometer model parameters and sampling time of human body odometer output are determined by solving an analytical solution of a human body odometer model error minimization problem, a Kalman filtering method is adopted, inertial navigation information based on MIMU and human body odometer information are fused, a human body inertial odometer is formed, and pose output is realized, so that autonomous navigation positioning and motion state monitoring of a pedestrian are effectively realized.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of pedestrian autonomous navigation positioning, and particularly relates to a human body inertial odometer based on kinematic model optimization. BACKGROUND

[0002] Pedestrian navigation positioning technology based on micro inertial sensor is a key technology for realizing accurate positioning of personnel in the case of satellite denial, and has wide application prospects in individual combat, rescue and disaster relief, indoor positioning, and motion monitoring. The current mature pedestrian positioning method is the zero velocity update algorithm, which can realize effective positioning, but has certain limitations and has problems such as not easy to wear, device easy to damage, and affecting motion. For non-foot wearing mode, the current main method is to realize positioning through pedestrian dead reckoning, but the positioning performance needs to be improved due to the accuracy and robustness of the existing step length model, and therefore how to realize pedestrian autonomous navigation positioning with accuracy and robustness in non-foot wearing mode becomes a problem to be solved. SUMMARY

[0003] To solve the above problems, the present application proposes a wearing mode of fixing a micro inertial measurement unit (MIMU) on the calf, and according to the characteristics of the human body odometer of patent ZL201711291544.8, a human body inertial odometer based on kinematic model optimization is constructed, which has the characteristics of simplicity, convenience, accuracy and robustness, and can effectively and accurately realize pedestrian autonomous navigation positioning and motion state monitoring.

[0004] A human body inertial odometer based on kinematic model optimization, comprising a micro inertial measurement unit MIMU, an inertial navigation calculation module, a human body odometer module, and a Kalman filter module.

[0005] The micro inertial measurement unit MIMU is arranged on the calf of the person to be measured.

[0006] The inertial navigation calculation module calculates the attitude, velocity and position of the person to be measured according to the angular velocity and acceleration output from the MIMU, and combines the attitude, velocity and position error fed back by the Kalman filter module.

[0007] The human body odometer module calculates and outputs the vertical velocity and horizontal velocity based on the pitch angular velocity measured by the MIMU and the pitch angle θ of the calf of the person to be measured output by the inertial navigation calculation module. M1 The specific calculation process is as follows:

[0008] S1: Fix the micro inertial measurement unit MIMU on the calf, and construct a human body odometer kinematic model with velocity information as output:

[0009]

[0010] wherein, is the horizontal direction velocity output by the human body odometry module, is the vertical direction velocity output by the human body odometry module, ω pitch is the angular velocity measured by the MIMU, and l is the human body odometry kinematics model parameter, is the pitch angle of the lower leg;

[0011] S2: subtracting the human body odometry kinematics model shown in formula (1) from the human body lower limb kinematics model to construct an error equation of the human body odometry kinematics model:

[0012]

[0013] wherein, is the pitch angular velocity of the lower leg rotating around the ankle, is the distance from the measuring point to the ankle, is the pitch angular velocity of the ankle rotating around the ground contact point, is the height of the ankle to the ground;

[0014] S3: solving the analytical solution of the model error minimization problem shown in formula (3) to determine the optimal value of l the optimal value of l the optimal value of the odometry model parameter l the time range of sampling the human body odometry:

[0015]

[0016] S4: within the time range of sampling the human body odometry, determining a sampling time of the human body odometry, and obtaining the horizontal direction velocity and the vertical direction velocity calculated by the human body odometry module at the sampling time;

[0017] The Kalman filter module fuses the inertial navigation information of the MIMU and the velocity information output by the human body odometry module to obtain the pose output of the human body motion.

[0018] Preferably, in the step S3, the determined time range of sampling the human body odometry is the foot-ground contact phase. is the height of the MIMU fixed part to the ground in the standing state.

[0019] Preferably, the specific method of the step S4 comprises:

[0020] After the pitch angular velocity ω pitch output by the MIMU is processed by the second-order mean filtering, the peak value detection is performed by using the sliding window method;

[0021] Under the constraint condition of the following formula, the peak sequence S = {s1, s2, s3,..., s n} obtained by peak detection of the sliding window method is divided into several peak subsequences {s i ,...,s j}:

[0022]

[0023] Where T sample is the MIMU sampling frequency, T min is the shortest motion period of the human body;

[0024] Finding the peak point in each peak subsequence is a true peak point, and the time corresponding to the true peak point is a time point determined by the human body odometer sampling.

[0025] Preferably, the Kalman filter module fuses the inertial navigation information of the MIMU and the speed information output by the human body odometer module to obtain the pose output of the human body motion, and the specific process includes:

[0026] The horizontal speed output by the human body odometer is decomposed in the navigation coordinate system to obtain the x and y axis speeds

[0027] Taking the position error, the speed error and the attitude error as the system state variables, a Kalman filter state equation is constructed:

[0028] X k = Φ k,k-1 X k-1 + Γ k-1 W k-1

[0029]

[0030] Where X k is the system state variable at time k, Φ k,k-1 is a one-step transition matrix from time k-1 to time k, Γ k-1 is a system noise driving matrix at time k-1, W k-1 is a system excitation noise sequence at time k-1; f n is the acceleration in the navigation coordinate system, T s is the sampling time, and S(f n ) is a skew-symmetric matrix composed of f n :

[0031]

[0032] In the above formula, f x , f y and fz are the acceleration of three directions in the navigation coordinate system respectively;

[0033] The Kalman filter observation equation is constructed with the height error and the speed error as the observation quantity:

[0034] Z k = H k X k + V k H k = [0 4×2 I 4×4 0 4×3 ]

[0035]

[0036] wherein, Z k is the system observation matrix at k moment; V k is the system observation noise at k moment; I 4×4 is the unit matrix, is the height from the fixed part of the MIMU to the ground in the standing state, is the speed output by the human body odometer module, is the height output by the inertial navigation calculation module, is the three-axis speed output by the inertial navigation calculation module;

[0037] Then the Kalman filter iterative update equation is as follows:

[0038] X k = K k Z k

[0039]

[0040]

[0041] P k = [I-K k H k ]P k,k-1

[0042] wherein, K k is the filter gain matrix, R k is the variance matrix of the measurement noise sequence, Q k-1 is the variance matrix of the system noise sequence, P k,k-1 is the one-step prediction mean square error matrix, P k is the estimated mean square error matrix.

[0043] 5. The human body inertial odometer based on the kinematic model optimization according to claim 4, characterized in that:

[0044]

[0045] wherein are respectively the average value of the x-direction velocity and the y-direction velocity of the measuring point M1 in the navigation coordinate system within the set time of the previous paragraph.

[0046] The present application has the following beneficial effects:

[0047] The present application provides a human body inertial odometer based on kinematic model optimization, which fixes the MIMU on the lower leg, constructs a human body odometer kinematic model with velocity information as output, determines the MIMU installation parameters, odometer model parameters and sampling time of human body odometer output by solving the analytical solution of the human body odometer model error minimization problem, adopts Kalman filtering method to fuse the inertial navigation information based on the MIMU and the human body odometer information, forms a human body inertial odometer, realizes the pose output, and effectively realizes the pedestrian autonomous navigation positioning and motion state monitoring. BRIEF DESCRIPTION OF DRAWINGS

[0048] Figure 1 It is a human body inertial odometer schematic diagram;

[0049] Figure 2 It is a flowchart of the human body inertial odometer based on kinematic model optimization provided by the present application;

[0050] Figure 3 It is a MIMU wearing method and coordinate system definition schematic diagram;

[0051] Figure 4 It is a foot-ankle-calf structure schematic diagram;

[0052] Figure 5 It is an experimental trajectory diagram. DETAILED DESCRIPTION

[0053] In order to make the personnel in the technical field better understand the present application scheme, the technical scheme in the present application embodiment will be described clearly and completely in combination with the drawings in the present application embodiment.

[0054] As Figure 1 shown, the human body inertial odometer provided by the present application includes a micro inertial measurement unit MIMU, an inertial navigation solving module, a human body odometer module and a Kalman filtering module.

[0055] Among them, the micro-inertial measurement unit MIMU is set on the calf of the person to be tested. The micro-inertial measurement unit (MIMU) is fixed to the calf to construct a human odometer kinematic model with speed information as output. The forward movement of the human body is achieved by the periodic rotation of the bones of the lower limbs. In each gait cycle, at least one side of the foot is in contact with the ground, which constitutes a rotational movement of the lower limb around the ground contact point. This is similar to the forward movement of land wheeled vehicles. The human body can be regarded as a special wheeled vehicle with a variable wheel radius. Inspired by this, it is considered to establish a human odometer similar to a wheeled speed odometer. Figure 3 As shown, the MIMU composed of gyroscopes and accelerometers will be configured at a certain position of the calf segment above the ankle (in Figure 4 The measured pitch angular velocity is:

[0056]

[0057] where ω pitch is the angular velocity measured by MIMU, is the pitch angular velocity of the calf rotating around the ankle, is the pitch angular velocity of the ankle rotating around the ground contact point. In the configuration adopted by the present invention, the pitch angular velocity is negative when rotating in the direction of the human body moving forward.

[0058] The inertial navigation solution module calculates the attitude, velocity and position of the person to be measured based on the angular velocity and acceleration output from the MIMU and the attitude, velocity and position errors fed back by the Kalman filter module.

[0059] The human body odometer module, such as Figure 1 As shown, the pitch angle of the calf of the person to be tested is obtained based on the pitch angular velocity measured by MIMU and the output of the inertial navigation solution module. Calculate and output vertical velocity and horizontal velocity. The specific calculation process is as follows:

[0060] S1: If Figure 3 The sensor coordinate system is defined as the xyz three-dimensional orthogonal coordinate system, where the x-axis is perpendicular to the calf and is positive forward; the y-axis is perpendicular to the calf and is positive to the right; and the z-axis is along the calf and is positive downward. The human odometer coordinate system is defined as the lp two-dimensional orthogonal coordinate system, where the l-axis is horizontal and is positive forward; the p-axis is vertical (coinciding with the direction of gravity) and is positive downward; and the navigation coordinate system is defined as the north-east coordinate system. Since only a single MIMU is worn, the human odometer kinematic model is constructed as shown in Equation (2) under the condition that only the synthetic angular velocity is known:

[0061]

[0062] in is the horizontal speed output by the human odometer, is the vertical speed output by the human odometer, l is the kinematic model parameter of the human odometer, is the pitch angle of the calf.

[0063] S2: Based on the difference between the human odometer kinematic model and the human lower limb kinematic model shown in formula (2), the error equation of the human odometer kinematic model is constructed. The human calf and foot are connected by the ankle joint. Under normal movement, the ankle joint has a single degree of freedom. Therefore, the present invention models the foot-ankle-calf movement as a two-dimensional multi-rigid body rotational movement. Figure 4 As shown in the figure, C1 is the touchdown point during the heel contact phase, J1 is the ankle joint, J1C1 is a rigid body derived from the simplified heel skeleton, M1 is the measurement point, i.e., the location where the inertial measurement unit is worn, M1J1 is a rigid body derived from the simplified calf skeleton, J2 is the toe joint, C2 is the touchdown point during the heel slightly off the ground phase, J1J2 is a rigid body derived from the simplified dorsum of the foot skeleton, and J2C3 is a rigid body derived from the simplified toe skeleton. The heel contact phase can be viewed as the rotational motion of a two-stage linear inverted pendulum composed of rigid bodies J1C1 and M1J1, with C1 as the center point. The heel slightly off the ground phase can be viewed as the rotational motion of a two-stage linear inverted pendulum composed of rigid bodies J1J2 and M1J1, with C2 as the center point. For the heel contact-heel slightly off the ground phase:

[0064] The velocity equation of point J1 is:

[0065]

[0066] The velocity equation of point J2 is:

[0067]

[0068] Let X, Y be Figure 4 For any point on the midfoot-ankle-calf rigid body model, XY is any segment of the rigid body on the model (X, Y = C1, C2, J1, J2, M1...), then the horizontal velocity component of point X is expressed as The vertical velocity component at point X is expressed as The rotation speed of the rigid body XY around point Y is expressed as ω XY And ω XY <0, the length of the rigid body XY is represented by l XY , the pitch angle of point X is expressed as θ X , counterclockwise is positive, X, Y are the two endpoints of the rigid body. The vertical distance from point X to the ground is represented by h X , the horizontal distance from point X to contact point Y is expressed as d XY .

[0069] Compared with the actual foot-ankle-calf kinematic model, the proposed human odometer kinematic model has a certain model error, and therefore a stage with the minimum model error needs to be found as the sampling stage of the human odometer output, and a parameter that minimizes the model error needs to be found as the optimal model parameter.

[0070] The model error is expressed as:

[0071]

[0072] Since the two-dimensional plane positioning is performed in the present application, the model error in the horizontal direction is preferentially minimized:

[0073]

[0074] wherein l is a settable model parameter, is the MIMU installation parameter, is a physical quantity that changes over time and cannot be configured alone.

[0075] S3: Determine the MIMU installation parameter by solving the analytical solution of the model error minimization problem Odometer model parameter Sampling time of the human odometer output The range of the MIMU installation parameter, the odometer model parameter, and the determination problem of the sampling time of the human odometer output can be converted into an optimization problem of minimizing the human odometer model error:

[0076]

[0077] wherein t is any time in the heel ground contact-heel slightly off ground stage.

[0078] In equation (6) The theoretical minimum value can be 0, and a set of equations as in (8) makes Minimum:

[0079]

[0080] Solving the equation set can obtain the optimal configuration parameter analytical expression:

[0081]

[0082] Therefore, in an ideal case, the MIMU is installed to the position of the calf part is The optimal model parameter is set to l M1J1 , and the model error is minimum.

[0083] is a physical quantity that varies with time, while is a MIMU installation parameter that does not vary with time, therefore, the model error minimization is not constantly true throughout the heel-strike to heel-lift phase, but only true at the time satisfying the condition , thus it is necessary to determine a time period with a distinct feature, and then determine , and further determine the MIMU installation parameter and the optimal model parameter The present application selects the foot-ground contact phase, in which for any satisfies:

[0084]

[0085] wherein is the height of the ankle to the ground when the human body is in a stationary standing state, and ε h , ε m is a very small quantity, and is the allowable error.

[0086] Therefore, the human body odometry kinematic model with the minimum error is determined as:

[0087]

[0088] wherein the MIMU installation parameter satisfies satisfies equation (10).

[0089] S4: Perform the pitch angular velocity peak detection based on the gait cycle constraint, and adjust the sampling time of the human body odometry output according to the detection result to obtain an accurate speed output. The sampling phase of the human body odometry output set by the present application is the foot-ground contact phase, and each time in the time period satisfies the condition of equation (10). According to the motion characteristics of the human foot landing under the walking state, at the time when the calf relative to the ankle pitch angular velocity decays to the minimum value, the calf is very close to the state of being perpendicular to the ground, and the foot is close to the ground, therefore, the time when the calf relative to the ankle pitch angular velocity decays to the minimum value is in the sampling phase of the human body odometry output set by the present application, and at the same time, since the calf relative to the ankle pitch angular velocity and the synthetic angular velocity ω pitch sensitive to the MIMU can simultaneously decay to the minimum value, the triggering problem of the human body odometry output sampling can be converted into the peak detection problem of ω pitch in the heel-strike to heel-lift phase.

[0090] After the second-order mean filtering processing of the pitch angular velocity ω pitch sensitive to the MIMU, the peak detection is performed by using the sliding window method, as shown in equation (12):

[0091]

[0092] wherein represents the pitch angle velocity after mean filtering.

[0093] However, the peak sequence S = {s1, s2, s3,..., s n} obtained by simply using the sliding window method for peak detection will contain many false detection points, so the gait cycle constraint is applied on the basis of the original peak detection, and several peak sub-sequences are divided, each peak sub-sequence {s i ,...,s j} contains a true peak point and several false detection points, and satisfies the constraint condition of formula (13):

[0094]

[0095] wherein T sample is the MIMU sampling frequency, and T min is the shortest motion cycle of the human body.

[0096] In each sub-sequence, the false detection peak points are removed by using formula (14), and the true peak points are retained:

[0097]

[0098] The Kalman filter module fuses the inertial navigation information of the MIMU and the human body odometer information to obtain the pose output of the human body motion. The horizontal velocity output by the human body odometer is decomposed in the fixed coordinate system (navigation coordinate system) to obtain:

[0099]

[0100]

[0101] wherein are the average values of the x-direction velocity and the y-direction velocity of the measurement point M1 in the navigation (fixed) coordinate system in the previous period of time, respectively.

[0102] Taking the position error, the velocity error and the attitude error as the system state variables, the Kalman filter state equation is constructed:

[0103]

[0104] wherein X k is the system state variable at time k, Φ k,k-1 is a one-step transfer matrix from time k-1 to time k, Γ k-1 is a system noise driving matrix at time k-1, and W k-1is the system excitation noise sequence at k-1 moment, δp is the position error, δv is the velocity error, and δψ is the attitude error. n is the acceleration in the navigation coordinate system, T s is the sampling time, S(f n ) is the skew-symmetric matrix composed of f n :

[0105]

[0106] In the above formula, f x , f y , and f z are accelerations in three directions in the navigation coordinate system, respectively.

[0107] The Kalman filter observation equation is constructed by taking the height error and the velocity error as the observation quantities:

[0108]

[0109] where Z k is the system observation matrix at k moment, V k is the system observation noise at k moment, I 4×4 is the unit matrix, is the height of the fixed part of the MIMU to the ground in the standing state, is the speed output by the human body odometer module, is the height obtained by the inertial navigation solution, is the speed obtained by the inertial navigation solution.

[0110] In the Kalman filter, the error can be close to zero after each correction, so in order to simplify the calculation, the time update equation is not used to propagate the error state X, and the Kalman filter iterative update equation is as follows:

[0111]

[0112] where K k is the filter gain matrix, R k is the variance matrix of the measurement noise sequence, Q k-1 is the variance matrix of the system noise sequence, P k,k-1 is the one-step prediction mean square error matrix, P k is the estimated mean square error matrix.

[0113] Figure 5 Fig. 1 shows the positioning trajectory of the human body inertial odometer based on the kinematic model optimization, and the experimental results prove the effectiveness of the method.

[0114] The embodiment effectively realizes accurate and robust autonomous navigation and positioning of pedestrians in a wearing mode of MIMU fixed on the lower leg, and has easy use and simplicity, and can be applied to multiple fields such as indoor positioning, motion state monitoring, autonomous positioning of biped robots, etc.

[0115] Of course, the present application can have other various embodiments, and those skilled in the art can certainly make various corresponding changes and modifications according to the present application without departing from the spirit and essence of the present application, but these corresponding changes and modifications shall all belong to the protection scope of the claims attached to the present application.

Claims

1. A human inertial odometer based on kinematic model optimization, characterized in that: Including micro inertial measurement unit MIMU, inertial navigation solution module, human odometer module and Kalman filter module; Among them, the micro inertial measurement unit MIMU is set on the calf of the person to be tested; The inertial navigation settlement module calculates the attitude, velocity and position of the person being measured based on the angular velocity and acceleration output from the MIMU and the attitude, velocity and position errors fed back by the Kalman filter module; The human odometer module is based on the pitch angular velocity measured by the MIMU and the pitch angle of the calf of the person to be measured output by the inertial navigation solution module. Calculate and output the vertical velocity and horizontal velocity. The specific solution process is as follows: S1: Fix the micro inertial measurement unit (MIMU) to the lower leg and build a human odometer kinematic model with velocity information as output: in, is the horizontal speed output by the human odometer module, is the vertical speed output by the human odometer module, ω pitch is the angular velocity measured by MIMU, l is the kinematic model parameter of the human odometer, is the pitch angle of the calf; S2: Subtract the human odometer kinematic model shown in formula (1) from the human lower limb kinematic model to construct the error equation of the human odometer kinematic model: in, is the pitch angular velocity of the calf rotating around the ankle, The distance from the measuring point to the ankle, is the pitch angular velocity of the ankle rotating around the ground contact point, The height from the ankle to the ground; S3: Solve the analytical solution of the model error minimization problem shown in formula (3) and determine The optimal value of Optimal value of odometry model parameter l The time range for sampling human odometry: S4: determining a sampling moment for the human body odometer within a sampling time range, and obtaining a horizontal speed and a vertical speed calculated by the human body odometer module at the sampling moment; The Kalman filter module fuses the inertial navigation information of the MIMU with the speed information output by the human odometer module to obtain the posture output of the human body movement.

2. The human inertial odometer based on kinematic model optimization according to claim 1, characterized in that: In step S3, the time range for sampling the human odometer is determined to be the stage when the foot is in contact with the ground; It is the height from the fixed part of MIMU to the ground in standing state.

3. The human inertial odometer based on kinematic model optimization according to claim 2, characterized in that: The specific method of step S4 includes: The pitch angular velocity ω output by MIMU pitch After the second-order mean filtering, the sliding window method is used for peak detection; Under the following constraints, the peak sequence S obtained by performing peak detection on the sliding window method is S={s1,s2,s3,...,s n } is divided into several peak subsequences {s i ,...,s j }: Where T sample is the MIMU sampling frequency, T min It is the shortest movement cycle of the human body; The peak point in each peak subsequence is found to be the true peak point. The moment corresponding to the true peak point is the determined moment for sampling the human odometer.

4. The human inertial odometer based on kinematic model optimization according to claim 2, characterized in that: The Kalman filter module fuses the inertial navigation information of the MIMU with the speed information output by the human odometer module to obtain the posture output of human motion. The specific process includes: Decompose the horizontal velocity output by the human odometer into the x-axis and y-axis velocities in the navigation coordinate system Taking position error, velocity error and attitude error as system state variables, the Kalman filter state equation is constructed: X k =Φ k,k-1 X k-1 +C k-1 W k-1 Among them, X k is the system state variable at time k, Φ k,k-1 is the one-step transfer matrix from time k-1 to time k, Γ k-1 is the system noise driving matrix at time k-1, W k-1 is the system excitation noise sequence at time k-1; f n is the acceleration in the navigation coordinate system, T s is the sampling time, S(f n ) is the antisymmetric matrix formed by fn: In the above formula, f x 、f y and f z They are the accelerations in three directions in the navigation coordinate system; The Kalman filter observation equation is constructed using height error and velocity error as observation quantities: Z k =H k X k +V k H k =[0 4×2 I 4×4 0 4×3 ] Among them, Z k is the system observation matrix at time k; V k is the system observation noise at time k; I 4×4 is the identity matrix, is the height from the fixed part of the MIMU to the ground in the standing state, is the speed output by the human odometer module, is the height output by the inertial navigation solution module, The three-axis speed output by the inertial navigation solution module; Then the Kalman filter iterative update equation is as follows: X k =K k Z k P k =[I-K k H k ]P k,k-1 Among them, K k is the filter gain matrix, R k is the variance matrix of the measurement noise sequence, Q k-1 is the variance matrix of the system noise sequence, P k,k-1 is the one-step prediction mean square error matrix, P k is the estimated mean square error matrix.

5. The human inertial odometer based on kinematic model optimization according to claim 4, characterized in that: in They are respectively the average x-direction velocity and the average y-direction velocity of the measurement point M1 in the navigation coordinate system within the previous set time.

Images