Milemeter design method, system and equipment based on fusion of wheel speed meter and IMU (Inertial Measurement Unit)

Abstract

The invention discloses a speedometer design method, system and equipment based on fusion of a wheel speed meter and an IMU (Inertial Measurement Unit). The method comprises the following steps: acquiring a plurality of data measured by an inertial measurement unit, and acquiring an attitude rotation matrix obtained by sequentially rotating a coordinate system b in the inertial measurement unit according to the directions of a Z axis, an X axis and a Y axis; determining a state space model of the odometer; constructing a speed observation model of the wheel speed meter based on the state space model; calculating a first error state of the speed observation model, and determining an observation matrix of the wheel speed meter according to the first error state; inputting a plurality of data measured by the inertial measurement unit into the state space model to obtain a predicted value output by the state space model; and calculating a second error state of the state space model, and determining the state of the odometer based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter. According to the invention, the accuracy and stability of track plotting can be improved.

Description

Technical Field

[0001] The present application relates to the technical field of odometer design, and in particular to an odometer design method, system and device based on the fusion of a wheel speed meter and an IMU. Background Art

[0002] Odometry is a method that uses data obtained from mobile sensors to estimate the change in the position of an object over time. Common odometer positioning methods include wheel odometers, visual odometers, and visual inertial odometers. In addition to vehicle positioning and mapping, odometers can also be used in other scenarios that require odometers.

[0003] The higher the precision of the odometer design, the higher the accuracy of the vehicle's dead reckoning, which is a very common positioning method. However, since the existing odometer system contains unobservable directions, this will result in poor accuracy and stability of track reckoning. Summary of the invention

[0004] This application aims to propose an odometer design method, system and equipment based on the fusion of wheel speed meter and IMU, which can improve the accuracy and stability of track calculation.

[0005] In a first aspect, an embodiment of the present application provides an odometer design method based on the fusion of a wheel speed meter and an IMU, the method comprising: Acquire a plurality of data measured by an inertial measurement unit, and acquire an attitude rotation matrix obtained by rotating a coordinate system b in the inertial measurement unit in sequence along the directions of the Z axis, the X axis, and the Y axis; Determining a state space model of the odometer based on the attitude rotation matrix; Based on the state space model, construct a speed observation model of the wheel speed meter; Calculating a first error state of the speed observation model, and determining an observation matrix of the wheel speed meter according to the first error state; Inputting a plurality of data measured by the inertial measurement unit into the state-space model to obtain a predicted value output by the state-space model; A second error state of the state-space model is calculated, and a state of the odometer is estimated based on the state-space model, the second error state, the predicted value, and an observation matrix of the wheel speed meter.

[0006] Compared with the prior art, the first aspect of the present application has the following beneficial effects: The method obtains multiple data measured by an inertial measurement unit, and obtains an attitude rotation matrix obtained by rotating the coordinate system b in the inertial measurement unit in the directions of the Z axis, the X axis and the Y axis in sequence; determines the state space model of the odometer based on the attitude rotation matrix; constructs the speed observation model of the wheel speed meter based on the state space model; calculates the first error state of the speed observation model, and determines the observation matrix of the wheel speed meter according to the first error state; inputs multiple data measured by the inertial measurement unit into the state space model to obtain the predicted value output by the state space model; calculates the second error state of the state space model, and estimates the state of the odometer based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter. In this way, by rotating the coordinate system b in the directions of the Z axis, the X axis and the Y axis in sequence to obtain the attitude rotation matrix, then constructing the state space model based on the attitude rotation matrix, and then estimating the state of the odometer system based on the state space model, the state space model does not include the direction that cannot be observed, so that the accuracy and stability of the track calculation can be improved.

[0007] In some implementations, the state space model of the odometer is determined based on the attitude rotation matrix in the following manner:

[0008] in, Represents the inertial measurement unit coordinate system In the navigation coordinate system The derivative of the velocity under the system, Representing coordinate system In the navigation coordinate system The acceleration of the system, Representing coordinate system To the navigation coordinate system The attitude rotation matrix of the system, , Representing coordinate system The rotation matrix obtained by rotating the system around the Z axis. Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents the measurement value of the accelerometer, represents the measured zero bias of the accelerometer, express The gravitational acceleration of the system, express The gravitational acceleration of the system, Representing coordinate system To the navigation coordinate system The derivative of the attitude rotation matrix of the system, Representing coordinate system In the navigation coordinate system The angular velocity of the system, represents the measurement value of the gyroscope, represents the measured zero bias of the gyroscope, represents the derivative of the measured bias of the accelerometer, represents the derivative of the measured bias of the gyroscope, represents transpose, A matrix representing the cross product.

[0009] In some implementations, a speed observation model of a wheel speedometer is constructed based on the state space model in the following manner:

[0010]

[0011] in, represents the nominal state of the wheel speed meter speed observation model without considering noise, Represents the inertial measurement unit coordinate system In the navigation coordinate system The speed of the tie, Represents the inertial measurement unit coordinate system To the navigation coordinate system From the horizontal coordinate system The speed of the tie, Representing coordinate system The rotation matrix obtained by rotating the system around the Z axis. Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. Indicates transpose.

[0012] In some implementations, the first error state of the velocity observation model is calculated as follows:

[0013] in, represents the first error state of the velocity observation model, represents the true value of the speed observation model of the wheel speed meter, represents the nominal state of the wheel speed meter speed observation model without considering noise, express The error state, Represents the inertial measurement unit coordinate system To the navigation coordinate system From the horizontal coordinate system The speed of the tie, represents the matrix of the cross product, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, Indicates the error state of the attitude rotation vector.

[0014] In some implementations, the second error state of the state-space model is calculated as follows:

[0015] in, express The error state, Represents the inertial measurement unit coordinate system In the navigation coordinate system The velocity in the horizontal coordinate system The projection of the system, express The true value of represents the matrix of the cross product, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, represents the error state of the attitude rotation vector, express The true value of Represents the identity matrix with three rows and three columns.

[0016] In some implementations, the observation matrix of the wheel speed meter is determined as follows:

[0017] in, represents the observability matrix of the wheel speed meter, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, Represents the inertial measurement unit coordinate system In the navigation coordinate system The velocity in the horizontal coordinate system The projection of the system, represents the matrix of the cross product, Indicates all zeros matrix.

[0018] In some implementations, estimating the state of the odometer based on the state-space model, the second error state, the predicted value, and the observation matrix of the wheel speed meter includes: Based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter, a Kalman filter is used to estimate the state of the odometer.

[0019] In a second aspect, the embodiment of the present application further provides an odometer design system based on the fusion of a wheel speed meter and an IMU, the system comprising: A data acquisition unit, used to acquire a plurality of data measured by an inertial measurement unit, and to acquire an attitude rotation matrix obtained by rotating a coordinate system b in the inertial measurement unit in sequence along the directions of the Z axis, the X axis, and the Y axis; A model determination unit, used for determining a state space model of the odometer based on the attitude rotation matrix; A model building unit, used for building a speed observation model of the wheel speed meter based on the state space model; a data calculation unit, configured to calculate a first error state of the speed observation model, and determine an observation matrix of the wheel speed meter according to the first error state; A data prediction unit, used for inputting a plurality of data measured by the inertial measurement unit into the state space model to obtain a predicted value output by the state space model; A state estimation unit is used to calculate a second error state of the state-space model, and estimate the state of the odometer based on the state-space model, the second error state, the predicted value and the observation matrix of the wheel speed meter.

[0020] In a third aspect, an embodiment of the present application also provides an electronic device, comprising at least one control processor and a memory for communicating with the at least one control processor; the memory stores instructions executable by the at least one control processor, and the instructions are executed by the at least one control processor so that the at least one control processor can execute the odometer design method based on the fusion of a wheel speed meter and an IMU as described above.

[0021] In a fourth aspect, an embodiment of the present application further provides a computer-readable storage medium, wherein the computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute an odometer design method based on the fusion of a wheel speed meter and an IMU as described above.

[0022] It can be understood that the beneficial effects of the second to fourth aspects compared with the related art are the same as the beneficial effects of the first aspect compared with the related art. Please refer to the relevant description in the first aspect, and no further details will be given here. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the description of the embodiments in conjunction with the following drawings, in which: Figure 1 It is a flowchart of an embodiment of an odometer design method based on the fusion of a wheel speed meter and an IMU provided in the present application; Figure 2 It is a structural schematic diagram of an embodiment of an odometer design system based on the fusion of a wheel speed meter and an IMU provided by the present application; Figure 3 It is a structural schematic diagram of an embodiment of an electronic device provided by the present application. DETAILED DESCRIPTION

[0024] The embodiments of the present application are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and cannot be understood as limiting the present application.

[0025] In the description of this application, if there is a description of first, second, etc., it is only for the purpose of distinguishing technical features, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features or implicitly indicating the order of the indicated technical features.

[0026] In the description of the present application, it should be understood that the descriptions involving orientation, such as the orientation or positional relationship indicated as up, down, etc., are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0027] In the description of this application, it should be noted that, unless otherwise clearly defined, terms such as setting, installing, connecting, etc. should be understood in a broad sense, and technicians in the relevant technical field can reasonably determine the specific meaning of the above terms in this application based on the specific content of the technical solution.

[0028] Odometry is a method that uses data obtained from mobile sensors to estimate the change in the position of an object over time. Common odometer positioning methods include wheel odometers, visual odometers, and visual inertial odometers. In addition to vehicle positioning and mapping, odometers can also be used in other scenarios that require odometers.

[0029] The higher the precision of the odometer design, the higher the accuracy of the vehicle's dead reckoning, which is a very common positioning method. However, since the existing odometer system contains unobservable directions, this will result in poor accuracy and stability of track reckoning.

[0030] In order to solve the above-mentioned problem that the odometer system contains unobservable directions, resulting in poor accuracy and stability of track estimation, the present application provides an odometer design method, system and device based on the fusion of wheel speed meter and IMU.

[0031] Reference Figure 1 The present application embodiment provides an odometer design method based on the fusion of a wheel speed meter and an IMU, and the method comprises the following steps: Step S100, obtaining a plurality of data measured by an inertial measurement unit, and obtaining an attitude rotation matrix obtained by rotating the coordinate system b in the inertial measurement unit in sequence along the directions of the Z axis, the X axis and the Y axis; Step S200, determining a state space model of the odometer based on the attitude rotation matrix; Step S300: constructing a speed observation model of the wheel speed meter based on the state space model; Step S400, calculating a first error state of the speed observation model, and determining an observation matrix of the wheel speed meter according to the first error state; Step S500, inputting a plurality of data measured by the inertial measurement unit into a state space model to obtain a predicted value output by the state space model; Step S600: Calculate a second error state of the state-space model, and estimate the state of the odometer based on the state-space model, the second error state, the predicted value, and the observation matrix of the wheel speed meter.

[0032] In this embodiment, multiple data measured by the inertial measurement unit are obtained, and the attitude rotation matrix obtained by rotating the coordinate system b in the inertial measurement unit in the directions of the Z axis, the X axis and the Y axis in sequence is obtained; based on the attitude rotation matrix, the state space model of the odometer is determined; based on the state space model, the speed observation model of the wheel speed meter is constructed; the first error state of the speed observation model is calculated, and the observation matrix of the wheel speed meter is determined according to the first error state; the multiple data measured by the inertial measurement unit are input into the state space model to obtain the predicted value output by the state space model; the second error state of the state space model is calculated, and the state of the odometer is estimated based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter. In this way, by rotating the coordinate system b in the directions of the Z axis, the X axis and the Y axis in sequence to obtain the attitude rotation matrix, then constructing the state space model according to the attitude rotation matrix, and then estimating the state of the odometer based on the state space model, the state space model does not include the direction that cannot be observed, so that the accuracy and stability of the track calculation can be improved.

[0033] The above-mentioned inertial measurement unit can be a device for measuring the three-axis attitude angle (or angular rate) and acceleration of an object. Generally, an IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometer detects the acceleration signal of the object in the independent three-axis of the carrier coordinate system, and the gyroscope detects the angular velocity signal of the carrier relative to the navigation coordinate system, measures the angular velocity and acceleration of the object in three-dimensional space, and uses this to calculate the attitude of the object.

[0034] The state space model of the odometer is determined based on the attitude rotation matrix and multiple data. The data can be directly measured by the accelerometer and gyroscope in the inertial measurement unit, or the data calculated by the inertial measurement unit based on the data directly measured by the accelerometer and gyroscope. The state space model of the odometer is constructed based on these data and the attitude rotation matrix.

[0035] The speed observation model of the wheel speed meter is constructed based on the state space model, and the speed observation model of the wheel speed meter can be constructed according to the parameters in the state space model.

[0036] In some implementations, the state space model of the odometer is determined based on the attitude rotation matrix in the following manner:

[0037] in, Represents the inertial measurement unit coordinate system In the navigation coordinate system The derivative of the velocity under the system, Representing coordinate system In the navigation coordinate system The acceleration of the system, Representing coordinate system To the navigation coordinate system The attitude rotation matrix of the system, , Representing coordinate system The rotation matrix obtained by rotating the system around the Z axis. Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents the measurement value of the accelerometer, represents the measurement bias of the accelerometer, express The gravitational acceleration of the system, express The gravitational acceleration of the system, Representing coordinate system To the navigation coordinate system The derivative of the attitude rotation matrix of the system, Representing coordinate system In the navigation coordinate system The angular velocity of the system, represents the measurement value of the gyroscope, represents the measured zero bias of the gyroscope, represents the derivative of the measured bias of the accelerometer, represents the derivative of the gyroscope's measured bias, represents transpose, A matrix representing the cross product.

[0038] In this embodiment, the coordinate system To the navigation coordinate system The attitude rotation matrix of the a system is deformed to obtain the attitude rotation matrix of the b system rotated in the directions of the Z axis, X axis and Y axis. Since the Z axis is parallel to the gravity direction, that is, the Z axis is perpendicular to the horizontal plane of gravity, the state space model of the odometer determined according to the attitude rotation matrix and multiple data can be used for the later establishment of The independent state space description lays a good data foundation.

[0039] In some implementations, a speed observation model of a wheel speed meter is constructed based on a state space model in the following manner:

[0040]

[0041] in, represents the nominal state of the wheel speed meter speed observation model without considering noise, Represents the inertial measurement unit coordinate system In the navigation coordinate system The speed of the tie, Represents the inertial measurement unit coordinate system To the navigation coordinate system From the horizontal coordinate system The speed of the tie, Representing coordinate system The rotation matrix obtained by rotating the system around the Z axis. Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. Indicates transpose.

[0042] In this embodiment, according to the attitude rotation matrix in the state space model, the speed observation model of the wheel speed meter is changed to The unobservable direction is changed to the observable direction by an irrelevant model, i.e. , which lays a good data foundation for improving the accuracy of track calculation.

[0043] In some implementations, the first error state of the velocity observation model is calculated as follows:

[0044] in, represents the first error state of the velocity observation model, represents the true value of the speed observation model of the wheel speed meter, represents the nominal state of the wheel speed meter speed observation model without considering noise, express The error state, Represents the inertial measurement unit coordinate system To the navigation coordinate system From the horizontal coordinate system The speed of the tie, represents the matrix of the cross product, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, Indicates the error state of the attitude rotation vector.

[0045] In some implementations, the second error state of the state-space model is calculated as follows:

[0046] in, express The error state, Represents the inertial measurement unit coordinate system In the navigation coordinate system The velocity in the horizontal coordinate system The projection of the system, express The true value of represents the matrix of the cross product, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, represents the error state of the attitude rotation vector, express The true value of Represents the identity matrix with three rows and three columns.

[0047] In some embodiments, the rewritten error state differential equation and state transfer matrix are established as follows: The differential equation for the rewritten error state is:

[0048] The state transfer matrix is ​​established as:

[0049] in, represents the differential of the rewritten error state, and represents the coefficient matrix, Indicates the error status of the rewrite, represents the measurement noise of the accelerometer, represents the measurement noise of the gyroscope, represents the derivative of the true zero bias of the accelerometer, represents the derivative of the true bias of the gyroscope, express The error state, Represents the inertial measurement unit coordinate system In the navigation coordinate system The velocity in the horizontal coordinate system The projection of the system, represents the error state of the attitude rotation vector, Indicates the error state of the accelerometer's measurement bias, Indicates the error state of the gyroscope's measured zero bias, express Time has come The state transition matrix at time .

[0050] In some embodiments, the observability matrix of the odometer is constructed based on the observability matrix of the wheel speed meter and the solution of the differential equation in the following manner:

[0051] in, represents the target observability matrix of the odometry, represents the observability matrix of the wheel speed meter, express Time has come The state transfer matrix at time , , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, Represents the inertial measurement unit coordinate system In the navigation coordinate system The velocity in the horizontal coordinate system The projection of the system, A matrix representing the cross product.

[0052] In this embodiment, the target observability matrix of the odometer calculated based on the observability matrix of the wheel speed meter and the solution of the differential equation does not include unobservable directions, thereby improving the accuracy and stability of track estimation.

[0053] In some implementations, the observation matrix of the wheel speed meter is determined as follows:

[0054] in, represents the observability matrix of the wheel speed meter, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, Represents the inertial measurement unit coordinate system In the navigation coordinate system The velocity in the horizontal coordinate system The projection of the system, represents the matrix of the cross product, Indicates all zeros matrix.

[0055] In some implementations, estimating the state of the odometer based on the state-space model, the second error state, the predicted value, and the observation matrix of the wheel speed meter includes: Based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter, the Kalman filter is used to estimate the state of the odometer.

[0056] In this embodiment, based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter, the state of the odometer system is estimated by Kalman filtering or nonlinear optimization, which can improve the accuracy and stability of track estimation.

[0057] The above-mentioned Kalman filter can be an algorithm that uses the linear system state equation and the system input and output observation data to optimally estimate the system state. Since the observation data includes the influence of noise and interference in the system, the optimal estimation can also be regarded as a filtering process.

[0058] The above-mentioned nonlinear optimization method and the like may be a nonlinear optimization method and the like in the prior art that can estimate the state of the odometer system, and this embodiment does not impose any specific limitation thereto.

[0059] To facilitate understanding by those skilled in the art, a set of best embodiments is provided below: Since the existing odometer system contains unobservable directions, this will lead to poor accuracy and stability of track calculation. The existing odometer system is analyzed as follows: (1) in, Represents the inertial measurement unit (IMU) coordinate system In the navigation coordinate system The derivative of the velocity under the system, Representing coordinate system In the navigation coordinate system The acceleration of the system, Representing coordinate system To the navigation coordinate system The rotation matrix of the system, represents the measurement value of the accelerometer, represents the measurement bias of the accelerometer, express The gravitational acceleration of the system, express The gravitational acceleration of the system, Representing coordinate system In the navigation coordinate system The angular velocity of the system, represents the measurement value of the gyroscope, represents the measured zero bias of the gyroscope, represents transpose, A matrix representing the cross product.

[0060] In a real environment, the noise of the IMU needs to be considered, so the real measurements of the IMU are: (2) Among them, the superscript represents the true value of the state, express The true value of express The true value of represents the measurement noise of the accelerometer, represents the measurement noise of the gyroscope, express The true value of express The true value of express The true value of .

[0061] For the real system state, the following definition is made: (3) in, , for the real posture have ,and , express The true value of express The error state, express The error state, express The error state. Then for the true state: (4) in, express The derivative of .

[0062] Then the error state is obtained from formula (1) to formula (4): for: (5) in, express The error state after differentiation is, represents the derivative of the true zero bias of the accelerometer, represents the derivative of the true bias of the gyroscope, Represents the system attitude rotation vector The error state after differentiation is, express The error state after differentiation is, express Error state after differentiation.

[0063] The speed observation model of the wheel speed meter is: (6) in, It represents the nominal state of the wheel speed meter speed observation model when the noise is not considered.

[0064] Then for the error state: (7) From formula (7), we can know that the observation matrix of chassis speed measurement is: (8) Analyzing the observability of the system, the system differential equation is rewritten as follows: (9) but: (10) but Time has come The state transfer matrix of the system at time t is: (11) According to formula (9) to formula (11), the solution of differential equation (12) is: (12) Then the observability matrix of the system is: Observability: Not all state variables are measurable or have physical meaning. Therefore, the question of whether the state quantity of the system can be obtained through measurable output quantities is the observability problem of the system.

[0065] (13) Then we have: (14) but An observable direction for the odometer system , is a sufficiently small real number, and its physical meaning is analyzed as follows. The appropriate transformation is: (15) From formula (15), we can know that the unobservable direction corresponds to the rotation around the gravity axis.

[0066] In order to solve the problem that the existing odometer system includes unobservable directions, resulting in poor accuracy and stability of dead reckoning, this embodiment improves the existing odometer system as follows: In order to make the odometer system no longer contain the above unobservable directions, The appropriate deformations are: (16) in, , and They represent the rotation matrices obtained by rotating the b system along the Z axis, X axis and Y axis in sequence. Since Z is the first rotation axis, the direction of the Z axis is parallel to the direction of gravity, that is, the Z axis is perpendicular to the horizontal plane of gravity.

[0067] The wheel speed meter observation is transformed as follows: (17) Then we have: (18) in, and Respectively and The derivative of .

[0068] Since the following relationship always holds true: (19) Then there must be: (20) According to formula (19) and formula (20), we have: (twenty one) From formula (18) to formula (21), we have: (twenty two) for have: (twenty three) The error states of velocity and attitude are redefined as follows: (twenty four) For the newly defined error state, the differential equation is: (25) (26) At this time, the observation equation for the wheel speed meter in the error state is: (27) Then its observation matrix is: (28) We established irrelevant state space description. But consider In the physical meaning, we actually remove the yaw angle YAW in the Euler angle obtained by rotating along the Z axis, X axis and Y axis in the attitude, and establish the error state of the equivalent rotation vector of the rotation matrix synthesized by the pitch angle Pitch and the roll angle Roll. In fact, in the odometer system, only the Pitch and Roll degrees of freedom are estimated for the attitude, so is a two-dimensional surface embedded in three-dimensional space. It is not difficult to find that if Regardless of the system state, at any time The YAW in the Euler angle obtained by rotating the Z axis, X axis and Y axis in order is always 0, then The following constraints should be met: (29) in, represents the YAW in the Euler angle obtained by rotating the Z axis, X axis and Y axis in order. Then, a special observation of the odometer system can be obtained from formula (29): (30) in, Indicates the error status of the yaw angle YAW.

[0069] For the current system, the differential equation is rewritten as follows: (31) have: (32) in, , , , , as well as The matrices are A matrix that is block-wise non-zero.

[0070] Then the system state transfer matrix is: (33) The solution to the above differential equation is: (34) Then the system observability matrix is: (35) At this time, we introduce the direction that the system could not observe before: (36) This direction is no longer the unobservable direction described by the state space before the improvement. The state of the odometer system is solved by running Kalman filtering or nonlinear optimization through the improved state space model, making the odometer system's estimation of speed and other states more stable and accurate.

[0071] Reference Figure 2 The embodiment of the present application also provides an odometer design system based on the fusion of a wheel speed meter and an IMU, the system comprising a data acquisition unit 100, a model determination unit 200, a model construction unit 300, a data calculation unit 400, a data prediction unit 500 and a state estimation unit 600, wherein: The data acquisition unit 100 is used to acquire a plurality of data measured by the inertial measurement unit, and to acquire an attitude rotation matrix obtained by rotating the coordinate system b in the inertial measurement unit in sequence along the directions of the Z axis, the X axis and the Y axis; A model determination unit 200, used to determine a state space model of the odometer based on the attitude rotation matrix; A model building unit 300 is used to build a speed observation model of the wheel speed meter based on the state space model; The data calculation unit 400 is used to calculate a first error state of the speed observation model and determine an observation matrix of the wheel speed meter according to the first error state; The data prediction unit 500 is used to input a plurality of data measured by the inertial measurement unit into the state space model to obtain a predicted value output by the state space model; The state estimation unit 600 is used to calculate the second error state of the state space model, and estimate the state of the odometer based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter.

[0072] It should be noted that since the odometer design system based on the fusion of a wheel speed meter and an IMU in this embodiment and the odometer design method based on the fusion of a wheel speed meter and an IMU mentioned above are based on the same inventive concept, the corresponding contents in the method embodiment are also applicable to the system embodiment and will not be described in detail here.

[0073] Reference Figure 3 The present application also provides an electronic device, which includes: at least one memory; at least one processor; at least one program; The program is stored in the memory, and the processor executes at least one program to implement the odometer design method based on the fusion of wheel speed meter and IMU in the present disclosure.

[0074] The electronic device may be any intelligent terminal including a mobile phone, a tablet computer, a personal digital assistant (PDA), a car computer, etc.

[0075] The electronic device according to the embodiment of the present application is described in detail below.

[0076] The processor 1600 may be implemented by a general-purpose central processing unit (CPU), a microprocessor, an application-specific integrated circuit (ASIC), or one or more integrated circuits, and is used to execute relevant programs to implement the technical solutions provided by the embodiments of the present disclosure; The memory 1700 can be implemented in the form of a read-only memory (ROM), a static storage device, a dynamic storage device, or a random access memory (RAM). The memory 1700 can store an operating system and other application programs. When the technical solution provided in the embodiment of this specification is implemented by software or firmware, the relevant program code is stored in the memory 1700, and the processor 1600 calls and executes the odometer design method based on the fusion of the wheel speed meter and the IMU in the embodiment of the present disclosure.

[0077] Input / output interface 1800, used to implement information input and output; Communication interface 1900, used to realize communication interaction between the device and other devices, which can be realized through wired mode (such as USB, network cable, etc.) or wireless mode (such as mobile network, WIFI, Bluetooth, etc.); Bus 2000 , which transmits information between various components of the device (e.g., processor 1600 , memory 1700 , input / output interface 1800 , and communication interface 1900 ); The processor 1600 , the memory 1700 , the input / output interface 1800 , and the communication interface 1900 are connected to each other in communication within the device via the bus 2000 .

[0078] The embodiment of the present disclosure also provides a storage medium, which is a computer-readable storage medium. The computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the above-mentioned odometer design method based on the fusion of a wheel speed meter and an IMU.

[0079] The memory, as a non-transient computer-readable storage medium, can be used to store non-transient software programs and non-transient computer executable programs. In addition, the memory may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely disposed relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0080] The embodiments described in the embodiments of the present disclosure are intended to more clearly illustrate the technical solutions of the embodiments of the present disclosure and do not constitute a limitation on the technical solutions provided by the embodiments of the present disclosure. Those skilled in the art will appreciate that with the evolution of technology and the emergence of new application scenarios, the technical solutions provided by the embodiments of the present disclosure are also applicable to similar technical problems.

[0081] Those skilled in the art will appreciate that the technical solutions shown in the figures do not limit the embodiments of the present disclosure and may include more or fewer steps than shown in the figures, or a combination of certain steps, or different steps.

[0082] The device embodiments described above are merely illustrative, and the units described as separate components may or may not be physically separated, that is, they may be located in one place or distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0083] Those skilled in the art will appreciate that all or some of the steps in the methods disclosed above, and the functional modules / units in the systems and devices may be implemented as software, firmware, hardware, or a suitable combination thereof.

[0084] The terms "first", "second", "third", "fourth", etc. (if any) in the specification of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0085] It should be understood that in the present application, "at least one (item)" means one or more, and "plurality" means two or more. "And / or" is used to describe the association relationship of associated objects, indicating that three relationships may exist. For example, "A and / or B" can mean: only A exists, only B exists, and A and B exist at the same time, where A and B can be singular or plural. The character " / " generally indicates that the objects associated before and after are in an "or" relationship. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single or plural items. For example, at least one of a, b or c can mean: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, c can be single or multiple.

[0086] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0087] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0088] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0089] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including multiple instructions to enable an electronic device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), disk or optical disk and other media that can store programs. The above is a detailed description of the embodiments of the present application in conjunction with the accompanying drawings, but the present application is not limited to the above embodiments. Within the scope of knowledge possessed by ordinary technicians in the relevant technical field, various changes can be made without departing from the purpose of the present application.

Claims

1. A design method for an odometer based on the fusion of a wheel speed meter and an IMU, characterized in that: The method comprises: Acquire a plurality of data measured by an inertial measurement unit, and acquire an attitude rotation matrix obtained by rotating a coordinate system b in the inertial measurement unit in sequence along the directions of the Z axis, the X axis, and the Y axis; Determining a state space model of the odometer based on the attitude rotation matrix; Based on the state space model, construct a speed observation model of the wheel speed meter; Calculating a first error state of the speed observation model, and determining an observation matrix of the wheel speed meter according to the first error state; Inputting a plurality of data measured by the inertial measurement unit into the state-space model to obtain a predicted value output by the state-space model; A second error state of the state-space model is calculated, and a state of the odometer is estimated based on the state-space model, the second error state, the predicted value, and an observation matrix of the wheel speed meter.

2. The odometer design method based on the fusion of wheel speed meter and IMU according to claim 1 is characterized in that: The state space model of the odometer is determined based on the attitude rotation matrix in the following manner: ， in, Represents the inertial measurement unit coordinate system In the navigation coordinate system The derivative of the velocity under the system, Representing coordinate system In the navigation coordinate system The acceleration of the system, Representing coordinate system To the navigation coordinate system The attitude rotation matrix of the system, , Representing coordinate system The rotation matrix obtained by rotating the system around the Z axis. Representing coordinate system The rotation matrix is ​​obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents the measurement value of the accelerometer, represents the measured zero bias of the accelerometer, express The gravitational acceleration under the system, express The gravitational acceleration of the system, Representing coordinate system To the navigation coordinate system The derivative of the attitude rotation matrix of the system, Representing coordinate system In the navigation coordinate system The angular velocity of the system, represents the measurement value of the gyroscope, represents the measured zero bias of the gyroscope, represents the derivative of the measured bias of the accelerometer, represents the derivative of the measured bias of the gyroscope, represents transpose, A matrix representing the cross product.

3. The odometer design method based on the fusion of wheel speed meter and IMU according to claim 1 is characterized in that: The speed observation model of the wheel speed meter is constructed based on the state space model in the following manner: ， ， in, represents the nominal state of the wheel speed meter speed observation model without considering noise, Represents the inertial measurement unit coordinate system In the navigation coordinate system The speed of the tie, Represents the inertial measurement unit coordinate system To the navigation coordinate system From the horizontal coordinate system The speed of the tie, Representing coordinate system The rotation matrix obtained by rotating the system around the Z axis. Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. Indicates transpose.

4. The odometer design method based on the fusion of wheel speed meter and IMU according to claim 1 is characterized in that: The first error state of the velocity observation model is calculated as follows: ， in, represents the first error state of the velocity observation model, represents the true value of the speed observation model of the wheel speed meter, represents the nominal state of the wheel speed meter speed observation model without considering noise, express The error state, Represents the inertial measurement unit coordinate system To the navigation coordinate system From the horizontal coordinate system The speed of the tie, represents the matrix of the cross product, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, Indicates the error state of the attitude rotation vector.

5. The odometer design method based on the fusion of wheel speed meter and IMU according to claim 1 is characterized in that: The second error state of the state-space model is calculated as follows: ， in, express The error state, Represents the inertial measurement unit coordinate system In the navigation coordinate system The velocity in the horizontal coordinate system The projection of the system, express The true value of represents the matrix of the cross product, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, represents the error state of the attitude rotation vector, express The true value of Represents the identity matrix with three rows and three columns.

6. The odometer design method based on the fusion of wheel speed meter and IMU according to claim 1 is characterized in that: The observation matrix of the wheel speed meter is determined as follows: ， in, represents the observability matrix of the wheel speed meter, , Representing coordinate system The rotation matrix obtained by rotating the system around the X-axis. Representing coordinate system The rotation matrix is ​​obtained by rotating around the Y axis. represents transpose, Represents the inertial measurement unit coordinate system In the navigation coordinate system The velocity in the horizontal coordinate system The projection of the system, represents the matrix of the cross product, Indicates all zeros matrix.

7. The odometer design method based on the fusion of wheel speed meter and IMU according to claim 1 is characterized in that: The estimating the state of the odometer based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter includes: Based on the state space model, the second error state, the predicted value and the observation matrix of the wheel speed meter, a Kalman filter is used to estimate the state of the odometer.

8. An odometer design system based on the fusion of wheel speed meter and IMU, characterized in that: The system comprises: A data acquisition unit, used to acquire a plurality of data measured by an inertial measurement unit, and to acquire an attitude rotation matrix obtained by rotating a coordinate system b in the inertial measurement unit in sequence along the directions of the Z axis, the X axis, and the Y axis; A model determination unit, used for determining a state space model of the odometer based on the attitude rotation matrix; A model building unit, used for building a speed observation model of the wheel speed meter based on the state space model; a data calculation unit, configured to calculate a first error state of the speed observation model, and determine an observation matrix of the wheel speed meter according to the first error state; A data prediction unit, used for inputting a plurality of data measured by the inertial measurement unit into the state space model to obtain a predicted value output by the state space model; A state estimation unit is used to calculate a second error state of the state-space model, and estimate the state of the odometer based on the state-space model, the second error state, the predicted value and the observation matrix of the wheel speed meter.

9. An electronic device, characterized in that: It includes at least one control processor and a memory for communicating with the at least one control processor; the memory stores instructions that can be executed by the at least one control processor, and the instructions are executed by the at least one control processor so that the at least one control processor can execute the odometer design method based on the fusion of wheel speed meter and IMU as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the odometer design method based on the fusion of a wheel speed meter and an IMU as described in any one of claims 1 to 7.

Images