Motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints

Abstract

This invention discloses a motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints, belonging to the field of motion state perception and inertial measurement technology. The method includes: S1) acquiring measurement signals from a moving subject and processing the signals to obtain an initial motion state sequence; S2) correcting and updating the initial motion state sequence to obtain a corrected motion state sequence; S3) smoothing the corrected and updated motion state sequence to obtain a smoothed motion trajectory with consistent time; S4) constraining and correcting the smoothed motion trajectory and updating it to obtain a corrected trajectory; S5) smoothing the corrected trajectory using the same method as in S3, thereby optimizing the overall structure of the moving subject's motion trajectory and suppressing position drift. This invention uses the above method to optimize the position trajectory, thereby improving the stability and accuracy of motion state perception.

Description

Technical Field

[0001] This invention relates to the field of motion state perception and inertial measurement technology, and in particular to a motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints. Background Technology

[0002] With the development of intelligent sensing technology, motion state sensing methods based on inertial measurement units (IMUs) have been widely applied in mobile devices, wearable systems, and robots. IMUs have advantages such as small size and low power consumption, and can continuously output information such as acceleration, angular velocity, and magnetometer readings, making them crucial sensors for motion state sensing and trajectory estimation. However, due to the unavoidable noise and bias errors in inertial measurement signals, IMU-based motion state estimation is prone to error accumulation during long-term operation, especially in position state estimation, where trajectory drift is a significant problem.

[0003] In existing technologies, the problem of trajectory drift that easily occurs during motion state perception based on inertial measurement is that, when relying solely on inertial measurement or when external information acquisition is limited, position errors will still gradually accumulate over time, making it difficult to obtain long-term effective constraints and easily introducing inconsistent state corrections during long-term operation, thus adversely affecting the stability and robustness of the system. Summary of the Invention

[0004] The purpose of this invention is to provide a motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints, which can effectively suppress position drift in motion trajectory without relying on external positioning equipment. By combining inertial motion state estimation with geometric constraint optimization at the position level, the method optimizes the position trajectory while maintaining the physical consistency of attitude and velocity estimation, thereby improving the stability and accuracy of motion state perception.

[0005] To achieve the above objectives, this invention provides a motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints, comprising the following steps: S1. The inertial measurement unit collects measurement signals consisting of the angular velocity and acceleration of the moving subject, and processes the measurement signals to obtain the initial motion state sequence of the moving subject; S2. Based on the observation information of the motion state, the initial motion state sequence is corrected and updated to obtain the corrected motion state sequence, which covers the forward state estimation results and the corresponding prior prediction results of the entire time series. S3. Smooth the corrected and updated motion state sequence to obtain a smooth motion trajectory with consistent time. S4. Constrain and correct the smooth motion trajectory and update it to obtain the corrected trajectory; S5. The correction trajectory is smoothed in the same way as in S3 to optimize the overall structure of the motion trajectory of the moving subject and suppress position drift.

[0006] Preferably, the process of S1 is as follows: S11. The inertial measurement unit collects the measurement signals consisting of the angular velocity and acceleration of the moving subject, and sorts the measurement signals according to the sampling time to form a sampling time sequence; S12. Model the motion state of the moving subject in a unified manner within a Lie group. Above, Lie group matrix as follows: ; in Indicates the time of the moving subject The attitude rotation matrix, , Represents the velocity vector. Represents a position vector; S13. Define the system state in the Lie group and the error state in the Lie algebra space using the invariant extended Kalman filter. The process of defining the error state is as follows: ; ; in This indicates the current estimated state of the system. express Exponential mapping operations on, Represents the error state vector. This represents the mapping operation from vectors to Lie algebras. , and These represent attitude error, velocity error, and position error, respectively. The error state covariance matrix is ​​defined using the error state vector. The process is as follows: ; in Expressing expectations, express The transpose of ; S14. The state of the moving subject is predicted forward using an invariant extended Kalman filter based on an inertial kinematic model. The prior state estimate of the moving subject is obtained by updating the Lie group using the following formula: ; in express Prior state estimation at time 10:00 express Posterior state estimation at time 10:00. Represents the discrete propagation operator. This represents the input quantity consisting of inertial measurement signals; S15. Establish a discrete-time state propagation model of the system based on inertial kinematics, and predict and update the error covariance based on the state propagation model. The process is as follows: ; in express The prior error state covariance matrix at time t. express The posterior error state covariance matrix at time t. The Jacobian matrix represents the error state transition. for The transpose of the matrix, Represents the process noise covariance matrix; S16, combined with S14 and S15, yields the motion process. The estimation results of the forward inertial motion state at time t, and The prior prediction result at time.

[0007] Preferably, the forward prediction of the state of the moving subject in S14 includes updating the attitude state, velocity state, and position state, as follows: The propagation relationship for updating the attitude state is as follows: ; in express The prior attitude state at any given moment. express The posterior pose state at time 1. Indicates the first The angular velocity measurement value at the sampling time. Indicates the sampling time interval; The propagation relationship for updating the velocity state is as follows: ; in express The prior velocity state at time t, express The posterior velocity state at time t. This represents the measured acceleration value. Represents the gravitational acceleration vector; The propagation relationship for updating the location state is as follows: ; in express The prior position state at any given moment. express The posterior position state at time t.

[0008] Preferably, the process of S2 is as follows: S21. Establish an observation model and calculate... Time-constrained observations The calculation process is as follows: ; in Represents the observation function, Indicates observation noise; S22, Based on the observation model Linearization is performed on the prior state estimate at time step 1 to obtain a linear observation model for the error state: ; in This represents the Jacobian matrix of the observation model with respect to the error state vector. express Prior state estimation at time step; S23. Construct the Kalman gain matrix based on the linear observation model, and perform the Kalman update process based on the error state vector using the Kalman gain matrix as follows: ; in express The prior error state covariance matrix at time t. express The transpose of the matrix, Represents the observation noise covariance matrix; S24. Based on the Kalman gain matrix, the error state vector is corrected and updated to obtain the updated value of the error state vector. The process is as follows: ; S25. Correct and update the error state covariance matrix and the prior state estimate to obtain... Posterior error state covariance matrix at time 1 and posterior state estimation The process is as follows: ; ; in Represents the identity matrix; S26, Through Posterior error state covariance matrix at time 1 and posterior state estimation Returning to S1, we calculate the prior error state covariance matrix and prior state estimate for the next time step, continuing this process until the entire time series is covered, thus obtaining a forward state estimate covering the entire time series. and the corresponding prior prediction results .

[0009] Preferably, the observation function in S21 Constructed based on the specific constraint type of the observation information; Observation noise The statistical properties are determined by the observation noise covariance matrix. describe.

[0010] Preferably, the process of S3 is as follows: S31. Using the Kalman smoothing algorithm, starting from the end of the time series, calculate the smoothing gain matrix based on state propagation relationships and covariance information. The process is as follows: ; in The transpose of the error state transition Jacobian matrix; S32. The error state vector is updated in reverse time using the smoothing gain matrix, as follows: ; in Represents the smoothed first... Time error state estimation, No. The forward filtering estimation at time step 1 predicts the first time step 2. Current state Indicates the first Smooth state estimation at time step 1 The vee operation represents the mapping of a Lie algebra matrix to a vector form. S33. Perform a smooth update of the error state covariance matrix, as follows: ; in Represents the smoothed first... The time-state covariance matrix of the error state. Represents the smoothed first... The time-state covariance matrix of the error state. express The transpose of the matrix; S34. Repeat steps S31-S33 until the error state covariance matrix of the entire time series is smoothed, resulting in the smoothed error state sequence. ; S35. After completing the time-reverse smoothing of the error state, the smoothed error state estimate is mapped back to the state space using the Lie group back substitution method to obtain the smoothed motion state estimate result. The process is as follows: ; in This represents the smoothed motion state estimation result; S36. Based on the results of S35, a smooth motion trajectory is obtained.

[0011] Preferably, the process of S4 is as follows: S41. Select several key moment sets from the entire time series. Obtain the corresponding geometric position reference information. Construct geometrically constrained residuals The process is as follows: ; in This represents the estimated position at the corresponding moment in the smoothed motion state; S42, Error State Vector Based on Key Moments The linearization of the geometric position constraint residuals is performed as follows: ; in Represents the linearized geometric position constraint residuals. This represents the observation matrix of the position constraint with respect to the error state vector. Indicates position constraint noise; S43. Based on the invariant extended Kalman filter framework, the error state corresponding to the critical moment of the residual satisfying the linear geometric position constraint is corrected and updated to obtain the error state covariance matrix after constraint correction and update. ; S44. After completing S43, the error state update is mapped back to the system state space using the Lie group back substitution method, and the smoothed motion state is corrected. The process is as follows: ; in This represents the motion state after constraint correction and update. This represents the motion state estimation results at key moments after smoothing. This represents the update amount of the error state vector after smoothing and constraint correction at key moments; S45. Based on S44, obtain the state estimation results for constraint correction and updating at key moments. .

[0012] Preferably, in S42 The covariance is derived from the covariance matrix. describe; Related to the position error component, its form is as follows: ; in Let represent the covariance matrix.

[0013] Preferably, the process of S43 is as follows: S431. Construct the Kalman update process corresponding to the position constraints based on the linear position constraint residuals and the linear observation model, and obtain the Kalman gain matrix at the key moments. as follows: ; in This represents the error state covariance matrix at key moments after smoothing. express The transpose of the matrix; S432. Use the Kalman gain matrix at the critical moment to correct the positional constraints of the error state vector, and obtain the update amount of the error state vector at the critical moment. The process is as follows: ; S433. Update the error state covariance matrix at key moments, as follows: ; in This represents the error state covariance matrix at critical moments.

[0014] Preferably, the process of S5 is as follows: The state estimation results of key moments obtained in S4 are smoothed in the same way as in S3. Position state correction is performed on key moments that meet the constraints. Local geometric constraint information is propagated throughout the entire time series through time backward recursion, thereby achieving consistent optimization of the overall structure of the motion trajectory.

[0015] Therefore, the present invention employs the aforementioned motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints. This method, relying solely on inertial measurement data, fully utilizes the geometric position information inherent in the motion process to effectively constrain and optimize the position state within the motion trajectory, thereby mitigating the accumulation of position errors over long-term operation. By rationally separating inertial motion state estimation from position-level geometric constraint optimization, this method ensures that geometric constraints primarily affect the trajectory structure optimization process, thus maintaining the continuity and stability of the inertial motion state estimation process.

[0016] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0017] Figure 1 This is an overall flowchart of the motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints of the present invention; Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Specific model specifications need to be selected and determined according to the actual specifications of the device, etc. The specific selection calculation method adopts existing technology in the art, and therefore will not be described in detail.

[0019] Example like Figure 1 As shown, this invention provides a motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints, comprising the following steps: S1. The inertial measurement unit collects measurement signals consisting of the angular velocity and acceleration of the moving subject, and processes the measurement signals to obtain the initial motion state sequence of the moving subject; S1 is used to continuously and recursively estimate the attitude, velocity, and position of a moving subject based on motion data collected by an inertial measurement unit without introducing geometric position constraints. This results in an initial motion state sequence describing the entire motion process, providing a foundation for subsequent kinematic constraint correction and trajectory optimization.

[0020] S11. The inertial measurement unit collects the measurement signals consisting of the angular velocity and acceleration of the moving subject, and sorts the measurement signals according to the sampling time to form a sampling time sequence; S12. To ensure geometric consistency in the evolution of posture and pose, the motion states (posture, velocity, and position) of the moving subject are uniformly modeled on a Lie group. Above, Lie group matrix as follows: ; in Indicates the time of the moving subject The attitude rotation matrix, , Represents the velocity vector. Represents a position vector; S13. Based on the inertial kinematics, a discrete-time state propagation model of the system is established. Then, to achieve recursive estimation and characterize state uncertainty, the Invariant Extended Kalman Filter (IEKF) method is introduced to estimate the motion state. The system state is defined in the Lie group and the error state is defined in the Lie algebra space through the IEKF. The process of defining the error state is as follows (taking the right-invariant error as an example): ; ; in This indicates the current estimated state of the system. express Exponential mapping operations on, Represents the error state vector. This represents the mapping operation from vectors to Lie algebras. , and These represent attitude error, velocity error, and position error, respectively. The error state covariance matrix is ​​defined using the error state vector. The process is as follows: ; in Expressing expectations, express The transpose of ; Within each sampling period, IEKF first performs a forward prediction of the state based on the inertial kinematic model to obtain the prior state estimate for the next time step. Its predicted propagation can be given by the discrete recursive relationship between attitude, velocity and position.

[0021] S14. The state of the moving subject is predicted forward using an invariant extended Kalman filter based on an inertial kinematic model. The prior state estimate of the moving subject is obtained by updating the Lie group using the following formula: ; in express Prior state estimation at time 10:00 express Posterior state estimation at time 10:00. Represents the discrete propagation operator. It represents the input quantity (angular velocity, acceleration, etc.) composed of inertial measurement signals. Forward prediction of the state of a moving subject includes updating its attitude, velocity, and position states, as follows: The propagation relationship for updating the attitude state is as follows: ; in express The prior attitude state at any given moment. express The posterior pose state at time 1. Indicates the first The angular velocity measurement value at the sampling time. Indicates the sampling time interval; The propagation relationship for updating the velocity state is as follows: ; in express The prior velocity state at time t, express The posterior velocity state at time t. This represents the measured acceleration value. Represents the gravitational acceleration vector; The propagation relationship for updating the location state is as follows: ; in express The prior position state at any given moment. express The posterior position state at time t.

[0022] S15. Establish a discrete-time state propagation model of the system based on inertial kinematics, and predict and update the error covariance based on the state propagation model. The process is as follows: ; in express The prior error state covariance matrix at time t. express The posterior error state covariance matrix at time t. The Jacobian matrix represents the error state transition. for The transpose of the matrix, Represents the process noise covariance matrix; S16, combined with S14 and S15, yields the motion process. The estimation results of the forward inertial motion state at time t, and The prior prediction result at time.

[0023] Through the prediction and update process of IEKF described above, the estimated result of the forward inertial motion state during the entire motion process can be obtained, which can be used as the initial trajectory for subsequent trajectory optimization.

[0024] S2. Based on the observation information of the motion state, the initial motion state sequence is corrected and updated to obtain the corrected motion state sequence, which covers the forward state estimation results and the corresponding prior prediction results of the entire time series. The main purpose of this step is to constrain and correct the error components in the motion state without disrupting the continuity of inertial motion, thereby improving the stability and reliability of motion state estimation.

[0025] S21. Establish an observation model and calculate... Time-constrained observations The calculation process is as follows: ; in Represents the observation function, Indicates observation noise; Observation function Constructed based on the specific constraint type of the observation information; Observation noise The statistical properties are determined by the observation noise covariance matrix. Description, used to describe the relationship between kinematic constraints or state priors and system states.

[0026] S22. Within the Lie group-invariant extended Kalman filter framework, based on the observation model... Prior state estimation at time 1 Linearization is performed at this point to obtain a linear observation model for the error state vector: ; in This represents the Jacobian matrix of the observation model with respect to the error state vector. express Prior state estimation at time step; S23. Construct the Kalman gain matrix based on the linear observation model, and perform the Kalman update process based on the error state vector using the Kalman gain matrix as follows: ; in express The prior error state covariance matrix at time t. express The transpose of the matrix, Represents the observation noise covariance matrix; S24. Based on the Kalman gain matrix, the error state vector is corrected and updated to obtain the updated value of the error state vector. The process is as follows: ; S25. Correct and update the error state covariance matrix and the prior state estimate to obtain... Posterior error state covariance matrix at time 1 and posterior state estimation The process is as follows: ; After completing the error state update, the error correction is mapped back to the system state space using the Lie group back substitution method, and the prior state estimate is corrected and updated. The state update formula is as follows: ; in Represents the identity matrix; S26, Through Posterior error state covariance matrix at time 1 and posterior state estimation Returning to S1, we calculate the prior error state covariance matrix and prior state estimate for the next time step, continuing this process until the entire time series is covered, thus obtaining a forward state estimate covering the entire time series. and the corresponding prior prediction results .

[0027] In one embodiment, when the kinematic constraint is expressed as a zero-velocity constraint, the observation function can be... The structure is designed as a constraint on the velocity state components, thereby effectively suppressing the accumulation of velocity errors when the moving subject is stationary or in a low-speed phase. In another embodiment, the kinematic constraint may also be a low-speed constraint, a contact constraint, or a state constraint composed of prior task information, which is not limited in this invention.

[0028] By repeatedly performing the above correction and update process at the moments that meet the constraints within the time series, the rapid growth of inertial measurement error can be effectively suppressed while maintaining the continuity of inertial kinematics, providing a more stable state estimation basis for trajectory smoothing and geometric position constraint optimization in subsequent steps.

[0029] S3. Smooth the corrected and updated motion state sequence to obtain a smooth motion trajectory with consistent time. After completing the correction update based on kinematic constraints or state priors as described in S2, a smoothing method is introduced to optimize the state throughout the entire motion process in order to further refine the motion state estimation. The main purpose of this step is to fully utilize the forward estimation information in the entire time domain and perform time-reverse smoothing on the motion state estimation results without introducing new external constraints, thereby reducing the impact of single update errors on the overall trajectory and obtaining more consistent and smoother state estimation results.

[0030] In this embodiment, the Rauch–Tung–Striebel (RTS) smoothing method is used to smooth the state sequence obtained by forward estimation.

[0031] S31. Starting from the end of the time series using the Kalman smoothing algorithm (Rauch–Tung–Striebel (RTS) smoothing method), calculate the smoothing gain matrix using state propagation relationships and covariance information. The process is as follows: ; in The transpose of the error state transition Jacobian matrix; S32. The error state vector is updated in reverse time using the smoothing gain matrix, as follows: ; in Represents the smoothed first... Time error state estimation, No. The forward filtering estimation at time step 1 predicts the first time step 2. Current state Indicates the first Smooth state estimation at time step 1 The vee operation represents the mapping of a Lie algebra matrix to a vector form. S33. Perform a smooth update of the error state covariance matrix, as follows: ; in Represents the smoothed first... The time-state covariance matrix of the error state. Represents the smoothed first... The time-state covariance matrix of the error state. express The transpose of the matrix; S34. Repeat steps S31-S33 until the error state covariance matrix of the entire time series is smoothed, resulting in the smoothed error state sequence. ; S35. After completing the time-reverse smoothing of the error state, the smoothed error state estimate is mapped back to the state space using the Lie group back substitution method to obtain the smoothed motion state estimate result. The process is as follows: ; in This represents the smoothed motion state estimation result; S36. Based on the results of S35, a smooth motion trajectory is obtained.

[0032] By performing the smoothing process described above on the entire time series, a motion state trajectory with better consistency in the time dimension can be obtained. Compared with methods that rely solely on forward recursive estimation, RTS smoothing can effectively reduce the impact of single-time-time estimation errors on the overall trajectory, improving the stability and continuity of motion state estimation during long-term operation. It should be noted that this step only utilizes the state propagation information and covariance relationship obtained in the forward estimation stage for time series smoothing, without introducing new geometric position constraints or external measurement information. The resulting smoothing result serves as the basis for introducing geometric position constraints and optimizing the trajectory in subsequent steps.

[0033] S4. Constrain and correct the smooth motion trajectory and update it to obtain the corrected trajectory; After obtaining the smoothed motion trajectory of the time series described in S3, to further suppress the accumulation of inertial errors in the spatial dimension, geometric position constraints related to the spatial structure of the motion trajectory are introduced to update the position state in the motion trajectory. The main function of this step is to impose restrictions on the position state from the perspective of the overall trajectory, providing a constraint basis for subsequent trajectory optimization.

[0034] In actual motion, the trajectory often contains certain geometric structural information, such as repeatedly traversed spatial locations, positional consistency relationships between closed motion segments, or sparse positional information obtained from prior conditions, environmental features, or task requirements. However, existing inertial motion sensing methods still have limited utilization of this type of information, or lack reasonable constraint mechanisms during utilization, failing to effectively suppress positional drift while ensuring consistency of motion state. This embodiment does not limit the specific source of geometric positional constraints, but rather abstracts them uniformly as constraints acting on the positional state.

[0035] S41. Select several key moment sets from the entire time series. Obtain the corresponding geometric position reference information. Construct geometrically constrained residuals The process is as follows: ; in This represents the position estimate at the corresponding moment in the smoothed motion state; the residuals mentioned above are used to characterize the degree of deviation between the current trajectory and the geometric constraints.

[0036] S42, Error State Vector Based on Key Moments The linearization of the geometric position constraint residuals is performed as follows: ; in Represents the linearized geometric position constraint residuals. This represents the observation matrix of the position constraint with respect to the error state vector. Indicates position constraint noise; The covariance is derived from the covariance matrix. describe; In a typical implementation, Related to the position error component, its form is as follows: ; in Represents a unit array.

[0037] S43. Based on the invariant extended Kalman filter framework, the error state corresponding to the critical moment of the residual satisfying the linear geometric position constraint is corrected and updated to obtain the error state covariance matrix after constraint correction and update. ; S431. Construct the Kalman update process corresponding to the position constraints based on the linear position constraint residuals and the linear observation model, and obtain the Kalman gain matrix at the key moments. as follows: ; in This represents the error state covariance matrix at key moments after smoothing. express The transpose of the matrix; S432. Use the Kalman gain matrix at the critical moment to correct the positional constraints of the error state vector, and obtain the update amount of the error state vector at the critical moment. The process is as follows: ; S433. Update the error state covariance matrix at key moments, as follows: ; in This represents the error state covariance matrix at critical moments.

[0038] S44. After completing S43, the error state update is mapped back to the system state space using the Lie group back substitution method, and the smoothed motion state is corrected. The process is as follows: ; in This represents the motion state after constraint correction and update. This represents the motion state estimation results at key moments after smoothing. This represents the update amount of the error state vector after smoothing and constraint correction at key moments; S45. Based on S44, obtain the state estimation results for constraint correction and updating at key moments. .

[0039] By introducing precise geometric position constraints as described above, the state can be updated, thereby eliminating drift. It should be noted that the geometric position constraints constructed in this step do not directly participate in the forward inertial state propagation, but are introduced as constraints in the subsequent trajectory optimization stage, thus avoiding any adverse effects on the continuity of inertial kinematics.

[0040] S5. The correction trajectory is smoothed in the same way as in S3 to optimize the overall structure of the motion trajectory of the moving subject and suppress position drift.

[0041] The state estimation results of key moments obtained in S4 are smoothed in the same way as in S3. Position state correction is performed on key moments that meet the constraints. Local geometric constraint information is propagated throughout the entire time series through time backward recursion, thereby achieving consistent optimization of the overall structure of the motion trajectory.

[0042] Specifically, based on the S3 time series smoothing framework, the geometric position constraints constructed in S4 are introduced into the smoothing process as position-level observation information. Position state corrections are performed at key moments that satisfy the constraints, and the local geometric constraint information is propagated throughout the entire time series via a backward time recursion, thereby achieving consistent optimization of the overall structure of the motion trajectory. Through this smoothing optimization process based on geometric position constraints, a final motion trajectory that improves both spatial structure consistency and temporal continuity can be obtained.

[0043] The examples demonstrate that, relying solely on the inertial measurement unit (IMU), position drift caused by the accumulation of inertial errors in the motion trajectory can be effectively suppressed. Compared to methods using only forward inertial filtering, the introduction of trajectory smoothing optimization based on geometric position constraints significantly improves both the overall structural consistency and long-term stability of the motion trajectory.

[0044] Therefore, this method has a low dependence on external sensors and is suitable for application scenarios where external positioning information is missing or unstable. It can be widely used in motion state perception and trajectory reconstruction in mobile devices and wearable systems.

[0045] Therefore, this invention employs a motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints. This method, relying solely on inertial measurement data, fully utilizes the geometric position information inherent in the motion process to effectively constrain and optimize the position state within the motion trajectory, thereby mitigating the accumulation of position errors over long-term operation. By rationally separating inertial motion state estimation from position-level geometric constraint optimization, this method ensures that geometric constraints primarily affect the trajectory structure optimization process, thus maintaining the continuity and stability of the inertial motion state estimation process.

[0046] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints, characterized in that, Includes the following steps: S1. The inertial measurement unit collects measurement signals consisting of the angular velocity and acceleration of the moving subject, and processes the measurement signals to obtain the initial motion state sequence of the moving subject; S2. Based on the observation information of the motion state, the initial motion state sequence is corrected and updated to obtain the corrected motion state sequence, which covers the forward state estimation results and the corresponding prior prediction results of the entire time series. S3. Smooth the corrected and updated motion state sequence to obtain a smooth motion trajectory with consistent time. S4. Constrain and correct the smooth motion trajectory and update it to obtain the corrected trajectory; S5. The correction trajectory is smoothed in the same way as in S3 to optimize the overall structure of the motion trajectory of the moving subject and suppress position drift.

2. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 1, characterized in that, The process of S1 is as follows: S11. The inertial measurement unit collects the measurement signals consisting of the angular velocity and acceleration of the moving subject, and sorts the measurement signals according to the sampling time to form a sampling time sequence; S12. Model the motion state of the moving subject in a unified manner within a Lie group. Above, Lie group matrix as follows: ； in Indicates the time of the moving subject The attitude rotation matrix, , Represents the velocity vector. Represents a position vector; S13. Define the system state in the Lie group and the error state in the Lie algebra space using the invariant extended Kalman filter. The process of defining the error state is as follows: ； ； in This indicates the current estimated state of the system. express Exponential mapping operations on, Represents the error state vector. This represents the mapping operation from vectors to Lie algebras. , and These represent attitude error, velocity error, and position error, respectively. The error state covariance matrix is ​​defined using the error state vector. The process is as follows: ； in Expressing expectations, express The transpose of ; S14. The state of the moving subject is predicted forward using an invariant extended Kalman filter based on an inertial kinematic model. The prior state estimate of the moving subject is obtained by updating the Lie group using the following formula: ； in express Prior state estimation at time 10:00 express Posterior state estimation at time 10:

00. Represents the discrete propagation operator. This represents the input quantity consisting of inertial measurement signals; S15. Establish a discrete-time state propagation model of the system based on inertial kinematics, and predict and update the error covariance based on the state propagation model. The process is as follows: ； in express The prior error state covariance matrix at time t. express The posterior error state covariance matrix at time t. The Jacobian matrix represents the error state transition. for The transpose of the matrix, Represents the process noise covariance matrix; S16, combined with S14 and S15, yields the motion process. The estimation results of the forward inertial motion state at time t, and The prior prediction result at time.

3. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 2, characterized in that, The forward prediction of the state of the moving subject in S14 includes updating the attitude state, velocity state, and position state, as follows: The propagation relationship for updating the attitude state is as follows: ； in express The prior attitude state at any given moment. express The posterior pose state at time 1. Indicates the first The angular velocity measurement value at the sampling time. Indicates the sampling time interval; The propagation relationship for updating the velocity state is as follows: ； in express The prior velocity state at time t, express The posterior velocity state at time t. This represents the measured acceleration value. Represents the gravitational acceleration vector; The propagation relationship for updating the location state is as follows: ； in express The prior position state at any given moment. express The posterior position state at time t.

4. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 3, characterized in that, The process of S2 is as follows: S21. Establish an observation model and calculate... Time-constrained observations The calculation process is as follows: ； in Represents the observation function, Indicates observation noise; S22, Based on the observation model Linearization is performed on the prior state estimate at time step 1 to obtain a linear observation model for the error state: ； in This represents the Jacobian matrix of the observation model with respect to the error state vector. express Prior state estimation at time t; S23. Construct the Kalman gain matrix based on the linear observation model, and perform the Kalman update process based on the error state vector using the Kalman gain matrix as follows: ； in express The prior error state covariance matrix at time t. express The transpose of the matrix, Represents the observation noise covariance matrix; S24. Based on the Kalman gain matrix, the error state vector is corrected and updated to obtain the updated value of the error state vector. The process is as follows: ； S25. Correct and update the error state covariance matrix and the prior state estimate to obtain... Posterior error state covariance matrix at time 1 and posterior state estimation The process is as follows: ； ； in Represents the identity matrix; S26, Through Posterior error state covariance matrix at time 1 and posterior state estimation Returning to S1, we calculate the prior error state covariance matrix and prior state estimate for the next time step, continuing this process until the entire time series is covered, thus obtaining a forward state estimate covering the entire time series. and the corresponding prior prediction results .

5. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 4, characterized in that, Observation function in S21 Constructed based on the specific constraint type of the observation information; Observation noise The statistical properties are determined by the observation noise covariance matrix. describe.

6. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 5, characterized in that, The process of S3 is as follows: S31. Using the Kalman smoothing algorithm, starting from the end of the time series, calculate the smoothing gain matrix based on state propagation relationships and covariance information. The process is as follows: ； in The transpose of the error state transition Jacobian matrix; S32. The error state vector is updated in reverse time using the smoothing gain matrix, as follows: ； in Represents the smoothed first... Time error state estimation, Indicates the first The forward filtering estimation at time step 1 predicts the first time step 2. Current state Indicates the first Smooth state estimation at time step 1 The vee operation represents the mapping of a Lie algebra matrix to a vector form. S33. Perform a smooth update of the error state covariance matrix, as follows: ； in Represents the smoothed first... The time-state covariance matrix of the error state. Represents the smoothed first... The time-state covariance matrix of the error state. express The transpose of the matrix; S34. Repeat steps S31-S33 until the error state covariance matrix of the entire time series is smoothed, resulting in the smoothed error state sequence. ; S35. After completing the time-reverse smoothing of the error state, the smoothed error state estimate is mapped back to the state space using the Lie group back substitution method to obtain the smoothed motion state estimate result. The process is as follows: ； in This represents the smoothed motion state estimation result; S36. Based on the results of S35, a smooth motion trajectory is obtained.

7. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 6, characterized in that, The process of S4 is as follows: S41. Select several key moment sets from the entire time series. Obtain the corresponding geometric position reference information. Construct geometrically constrained residuals The process is as follows: ； in This represents the estimated position at the corresponding moment in the smoothed motion state; S42, Error State Vector Based on Key Moments The linearization of the geometric position constraint residuals is performed as follows: ； in Represents the linearized geometric position constraint residuals. This represents the observation matrix of the position constraint with respect to the error state vector. Indicates position constraint noise; S43. Based on the invariant extended Kalman filter framework, the error state corresponding to the critical moment of the residual satisfying the linear geometric position constraint is corrected and updated to obtain the error state covariance matrix after constraint correction and update. ; S44. After completing S43, the error state update is mapped back to the system state space using the Lie group back substitution method, and the smoothed motion state is corrected. The process is as follows: ； in This represents the motion state after constraint correction and update. This represents the motion state estimation results at key moments after smoothing. This represents the update amount of the error state vector after smoothing and constraint correction at key moments; S45. Based on S44, obtain the state estimation results for constraint correction and updating at key moments. .

8. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 7, characterized in that, S42 The covariance is derived from the covariance matrix. describe; Related to the position error component, its form is as follows: ； in Represents a unit array.

9. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 8, characterized in that, The process of S43 is as follows: S431. Construct the Kalman update process corresponding to the position constraints based on the linear position constraint residuals and the linear observation model, and obtain the Kalman gain matrix at the key moments. as follows: ； in This represents the error state covariance matrix at key moments after smoothing. express The transpose of the matrix; S432. Use the Kalman gain matrix at the critical moment to correct the positional constraints of the error state vector, and obtain the update amount of the error state vector at the critical moment. The process is as follows: ； S433. Update the error state covariance matrix at key moments, as follows: ； in This represents the error state covariance matrix at critical moments.

10. The motion state perception and trajectory optimization method based on inertial measurement and geometric position constraints according to claim 9, characterized in that, The process of S5 is as follows: The state estimation results of key moments obtained in S4 are smoothed in the same way as in S3. Position state correction is performed on key moments that meet the constraints. Local geometric constraint information is propagated throughout the entire time series through time backward recursion, thereby achieving consistent optimization of the overall structure of the motion trajectory.

Images