CN111912426A - Low-cost odometer design method based on MEMS IMU - Google Patents

Low-cost odometer design method based on MEMS IMU Download PDF

Info

Publication number
CN111912426A
CN111912426A CN202010793803.2A CN202010793803A CN111912426A CN 111912426 A CN111912426 A CN 111912426A CN 202010793803 A CN202010793803 A CN 202010793803A CN 111912426 A CN111912426 A CN 111912426A
Authority
CN
China
Prior art keywords
wheel
imu
vehicle
output
rotation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202010793803.2A
Other languages
Chinese (zh)
Inventor
张瑞琪
杜爽
鲁琪
甘旭东
王清林
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202010793803.2A priority Critical patent/CN111912426A/en
Publication of CN111912426A publication Critical patent/CN111912426A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a low-cost odometer design method based on an MEMS IMU, which comprises the following steps: step 1: the IMU is arranged on the side surface of the wheel, so that the IMU rotates along with the wheel, and the non-gravitational acceleration and the angular velocity of the wheel relative to an inertial system are measured; step 2: aiming at an IMU (inertial measurement Unit) arranged on a wheel, considering the motion constraint of a vehicle at the same time, and establishing an output model of the IMU; and step 3: taking the wheel rotation angle, the angular velocity and the angular acceleration as state quantities, establishing a Kalman filtering system model based on a constraint relation between the states, and establishing an observation model based on an IMU output model; and 4, step 4: based on the speed and mileage calculation of the artificial neural network and the updating of the parameters, the method has the advantages of low cost, accurate result and the like, makes up the defects of the traditional odometer, and has great application potential.

Description

Low-cost odometer design method based on MEMS IMU
Technical Field
The invention belongs to the field of navigation, and particularly relates to a design method of a milemeter based on MEMS IMU
Background
With the rapid development of transportation worldwide, the realization of reliable and accurate vehicle positioning is increasingly important in various vehicle navigation and safety-related applications such as route navigation, automatic driving, intelligent transportation, and the like. Combined navigation systems based on Global Positioning System (GPS) and Inertial Navigation System (INS) have been widely used in vehicles to provide accurate position and velocity information, but during GPS disconnection, INS errors accumulate rapidly leading to divergence, especially in vehicle applications, subject to commercial costs, often using MEMS-grade Inertial Measurement Units (IMU) with larger errors leading to faster divergence of errors.
Many scholars have studied the problem of dispersion of INS errors during GPS signal disconnection in recent years, introducing aiding sensors such as automobile odometers to correct for navigation errors. The automobile odometer can provide absolute speed information, can reduce position errors caused by twice integration of accelerometer data, and is difficult to utilize due to different odometer data standards and connection standards of different vehicles.
In addition, in the robot technology, the wheeled robot is concerned with due to the characteristics of mobility, strong stability and high efficiency, and is widely used in storage logistics, intelligent shopping guide, self-service, automatic inspection and other aspects, and the expensive sensors such as encoders are often used to design odometers for auxiliary navigation due to the relatively small wheels, so that the cost of the wheeled robot is greatly increased.
Disclosure of Invention
The invention aims to solve the problems that: in order to make up for the defects of the odometer in the existing vehicle and wheeled robot, the invention provides a novel low-cost odometer design method based on an MEMS IMU.
The invention adopts the mode that a three-axis MEMS IMU is arranged on the side surface of a wheel of a vehicle or a wheeled robot, when the wheel rotates, the output of an accelerometer changes regularly, for example, the projection of gravity acceleration on a sensitive axis of the accelerometer changes in a sine wave form with the frequency as the angular velocity of rotation, and the invention models the relation between the output of the accelerometer and the angular velocity of rotation of the wheel to extract the information of the angular velocity of rotation of the wheel. Meanwhile, the invention also uses the gyroscope to correct the angular velocity information: when the rotating speed of the wheel is too fast and the sampling rate of the accelerometer is low, the accelerometer cannot calculate the real rotating speed according to the sampling law, the output angular speed of the gyroscope is 'absolute', the shortage of the sampling rate of the accelerometer can be made up, and meanwhile, because the MEMS-level gyroscope often has larger errors, the angular speed derived by the accelerometer can also reduce the rotating speed errors caused by the gyroscope. The invention uses the extended Kalman filter to fuse the data of the accelerometer and the gyroscope, and makes up the respective defects to obtain more accurate wheel rotation angle and angular velocity. And calculating the driving mileage and speed of the vehicle or the wheeled robot by combining the relationship between the vehicle mileage and the wheel rotation angle.
The technical scheme of the invention is as follows: a low-cost odometer design method based on MEMS IMU comprises the following steps:
step 1: the IMU is arranged on the side surface of the wheel, so that the IMU rotates along with the wheel, and the non-gravitational acceleration and the angular velocity of the wheel relative to an inertial system are measured;
step 2: aiming at an IMU (inertial measurement Unit) arranged on a wheel, considering the motion constraint of a vehicle at the same time, and establishing an output model of the IMU;
and step 3: taking the wheel rotation angle, the angular velocity and the angular acceleration as state quantities, establishing a Kalman filtering system model based on a constraint relation between the states, and establishing an observation model based on an IMU output model;
and 4, step 4: and calculating speed and mileage based on the artificial neural network and updating parameters.
Further, the step 1 specifically includes:
an MEMS-level IMU sensor comprising a three-axis accelerometer and a three-axis gyroscope is arranged on the side surface of a rear wheel of a vehicle or a wheeled robot, and a sensor coordinate system s-x is established by taking the IMU as an originsyszsLet ysAxial direction directed toward the wheel center, xsAxis directed outwards perpendicular to the side of the tire, ysAxis, zsThe axle lies in the plane of rotation of the wheel. Establishing carrier seatsThe designation b-xbybzbIs that x isbAxis and xsThe axes coincide, ybThe axis being directed in front of the vehicle, zbAxis perpendicular to xbAxis and ybIn the axial direction; when the wheel starts to rotate, s is wound around xsThe axis rotates and the gravitational acceleration will be projected onto y periodically in a sinusoidal fashionsAxis, zsOn the axis, with frequency equal to the angular velocity of rotation of the wheel, and ysThe centrifugal acceleration due to the rotation of the wheel, the magnitude of which is the product of the square of the angular velocity of rotation of the wheel and the distance of the IMU from the wheel center, will be added to the axis, and the rotational velocity of the wheel will also be projected on the x-axis of the gyroscope.
Further, the step 1 further includes:
by adjusting the distance from the IMU to the wheel center, the fact that the actual acceleration exceeds the upper limit output by the accelerometer due to centripetal acceleration is avoided; the sensor is installed and constantly rotates on the wheel, and the integrated wireless communication device of sensor carries out information interaction including bluetooth, WIFI and the control system on the vehicle.
Further, the step 2 specifically includes:
the accelerometer output is expressed by the sum of gravity acceleration, centripetal acceleration generated by wheel rotation and acceleration of the vehicle relative to the ground, and a local horizontal coordinate system is taken as a navigation coordinate system n; recording:
Figure BDA0002624757000000031
Figure BDA0002624757000000032
the projection of the gravitational acceleration on the sensor coordinate system is then:
Figure BDA0002624757000000033
where g is the local gravitational acceleration and θ is the IMU relative to the home positionBy rotating the angle of the rotation in the counter-clockwise direction,
Figure BDA0002624757000000034
representing a rotation matrix of b to s,
Figure BDA0002624757000000035
is a rotation matrix of n to b, RijIs composed of
Figure BDA0002624757000000036
The elements of (1); i is 1, 2, 3; j is 1, 2, 3;
the centripetal acceleration generated by the rotation of the wheel is projected on ysAxial negative direction, expressed as:
Figure BDA0002624757000000037
r is the distance of the IMU from the wheel center,
Figure BDA0002624757000000038
is the first derivative of θ, the angular velocity of wheel rotation;
the acceleration of the vehicle under b can be expressed as
Figure BDA0002624757000000039
Considering the motion constraints of the vehicle, the lateral and the zenith accelerations are zero:
Figure BDA00026247570000000310
the vehicle acceleration in the vertical axis direction is represented by the wheel rotation angular acceleration multiplied by the wheel radius:
Figure BDA00026247570000000311
r is the radius of the wheel,
Figure BDA00026247570000000312
is the second derivative of θ, the angular acceleration of wheel rotation;
so that the acceleration of the vehicle with respect to the ground is
Figure BDA00026247570000000313
Projected to the sensor coordinate system:
Figure BDA0002624757000000041
output of accelerometer in IMU
Figure BDA0002624757000000042
The relationship to θ is:
Figure BDA0002624757000000043
neglecting earth rotation, gyroscope xsThe output of the shaft is approximately equal to the angular velocity of the wheel rotating relative to the carrier:
Figure BDA0002624757000000044
further, the step 3 specifically includes:
the data fusion of the accelerometer and the gyroscope by using the extended Kalman filter EKF makes up respective defects to obtain more accurate wheel rotation angle and angular velocity, and specifically comprises the following steps: considering that the accelerometer output model in the step 2 respectively contains theta,
Figure BDA0002624757000000045
Use of
Figure BDA0002624757000000046
The state quantities are represented by theta,
Figure BDA0002624757000000047
Establishing a state updating equation according to the relationship between the two; only accelerometer ysOutput of the shaft
Figure BDA0002624757000000048
zsOutput of the shaft
Figure BDA0002624757000000049
Gyroscope xsOutput of the shaft
Figure BDA00026247570000000410
Related to X, so use
Figure BDA00026247570000000411
As observed quantities, the equations (9) and (10) are linearized to establish a measurement update equation:
Figure BDA00026247570000000412
the state update equation:
Figure BDA00026247570000000413
wherein [ wx wy wz]TFor sensor noise of the corresponding axis, [ v ]x vy vz]TTo measure noise.
Further, the step 4 specifically includes: in step 3, theta and
Figure BDA0002624757000000051
then, the mileage and the speed need to be calculated, when the vehicle normally runs, the wheels and the ground do not slide relatively, the distance rolled by the wheels on the ground is equal to the running distance of the vehicle,
Pk＝P0+θkR (10)
Figure BDA0002624757000000052
P0is the initial mileage, PkMileage at time K, θk、
Figure BDA0002624757000000053
For the angle and angular velocity the wheel turned at time K,
Figure BDA0002624757000000054
r is the wheel radius, which is the speed of the vehicle at time K.
However, in actual conditions, due to the influence of factors such as vehicle speed, humidity, temperature, wind resistance coefficient, friction coefficient and the like, relative sliding often occurs, even the radius of a wheel is different due to changes of load and tire pressure, so that errors exist in calculated mileage, the types of the factors causing the errors are various, and the measurement and modeling by using a traditional method are difficult. The factors can change slowly, such as humidity, temperature, wind resistance coefficient, friction coefficient, tire pressure and the like, or the vehicle speed can be obtained from the previous steps, so the invention introduces the BP neural network, performs parameter training on the BP neural network by using GPS data when GPS signals exist, and predicts mileage and vehicle speed by using the BP neural network when GPS signals do not exist. The working principle of the BP algorithm is that firstly, the output of each layer of nodes is calculated through the weight between the nodes, the error is obtained by comparing the output layer with the expected output, and then the error is propagated reversely, which is based on the Widrow-Hoff learning rule, namely, the connection weight between the nodes is adjusted towards the direction that the error is reduced through the steepest descending direction of the sum of squares of relative errors, and the weight and the offset of the network are continuously adjusted. A new round of calculation is then performed until the error value reaches the expectation or its training number reaches a threshold.
The invention designs a three-layer feedforward neural network comprising a two-node input layer, a five-node hidden layer and a single-node output layer, and a sigmoid function is used as an activation function. In the training stage, the input layer is the wheel rotation angle and the angular speed theta,
Figure BDA0002624757000000055
The output layer is the difference between the speed measurement value of the GPS and the speed in the formula (11)
Figure BDA0002624757000000056
And carrying out parameter training on the neural network. In the prediction stage, only the input is theta,
Figure BDA0002624757000000057
Predicting that the system will output odometer speed
Figure BDA0002624757000000058
Relative to GPS velocity
Figure BDA0002624757000000059
Velocity difference v ofkWill be wrong
Figure BDA00026247570000000510
And vkAnd
Figure BDA00026247570000000511
as the final speed output of the system. In the training stage of the model, a mode of combining offline learning and online learning is used, namely, enough offline training is performed in advance when GPS signals exist, the obtained weight values of all nodes are stored as basic weight values, and online training is performed by taking the basic weight values as initial weight values when GPS signals exist in the actual vehicle running every time, so that the training time can be shortened, the input-output relation can be more consistent with the current conditions, errors are reduced, and the problem that the GPS signals cannot be searched in the initial running stage of the vehicle can be solved.
When the vehicle loses the GPS signal, the system enters a prediction mode to predict the speed error v at the current momentkThen the final system output speed VkAnd mileage PkComprises the following steps:
Figure BDA0002624757000000061
Figure BDA0002624757000000062
dt is the sampling time interval.
Parameter R in equation (8) during vehicle motion13、R23、R33Always varying with time, the need for R13、R23、R33And continuously updating. R13、R23、R33As a matrix
Figure BDA00026247570000000613
The elements in (1) are updated after each EKF update through a formula
Figure BDA0002624757000000063
Computing
Figure BDA0002624757000000064
And (6) updating. Wherein the content of the first and second substances,
Figure BDA0002624757000000065
the obtained θ in step 3 is calculated according to the formula (1), and
Figure BDA0002624757000000066
the output of the IMU is calculated by using a traditional strapdown inertial navigation attitude update equation, namely:
Figure BDA0002624757000000067
Figure BDA0002624757000000068
is a rotation matrix of s to n,
Figure BDA0002624757000000069
is composed of
Figure BDA00026247570000000610
The derivative of (a) of (b),
Figure BDA00026247570000000611
being gyroscopesOutputting the corresponding oblique symmetrical matrix and outputting the oblique symmetrical matrix,
Figure BDA00026247570000000612
is a diagonal symmetric matrix corresponding to the expression of the rotation angular velocity of the system of n relative to the system of i of the system of inertia in the system of s.
The invention has the beneficial effects that:
(1) the invention uses MEMS IMU to realize mileage and speed calculation, and has the advantages of low cost, low power consumption, simple realization, accurate result, and the like.
(2) By utilizing the odometer arranged on the output of the wheel IMU, the problem that data is difficult to utilize due to different vehicle data standards and different interface standards can be solved for vehicles, and the cost is reduced for wheeled robots.
(3) For most vehicle and wheeled robot navigation applications, an IMU is often equipped and can be utilized, the IMU is reformed and utilized under the condition of not influencing the original navigation application, the odometer is realized while a navigation result is output, and the odometer can assist a navigation system to increase the navigation precision in turn, so that the application potential is high.
Drawings
FIG. 1: the method of the present invention is generally flow chart.
FIG. 2: the invention discloses an IMU installation diagram;
fig. 3 (a): BP neural network structure training schematic diagram;
fig. 3 (b): BP neural network structure prediction schematic diagram;
FIG. 4: and (5) a parameter updating flow diagram.
Detailed Description
The technical solutions in the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, rather than all embodiments, and all other embodiments obtained by a person skilled in the art based on the embodiments of the present invention belong to the protection scope of the present invention without creative efforts.
According to an embodiment of the present invention, a low-cost odometer design method based on a MEMS IMU is provided, and fig. 1 is a flow chart of the method of the present invention, which specifically includes the following steps:
step 1, IMU installation scheme
Referring to fig. 2, an MEMS-level IMU including a three-axis accelerometer and a three-axis gyroscope is installed on the side of a rear wheel of a vehicle or wheeled robot, and a sensor coordinate system s-x is established with the IMU as an originsyszsLet ysAxial direction directed toward the wheel center, xsAxis directed outwards perpendicular to the side of the tire, ysAxis, zsThe axle lies in the plane of rotation of the wheel. Establishing a carrier coordinate system b-xbybzbIs that x isbAxis and xsThe axes coincide, ybThe axis being directed in front of the vehicle, zbAxis perpendicular to xbAxis and ybAxially. When the wheel starts to rotate, s is wound around xsThe axis rotates and the gravitational acceleration will be projected onto y periodically in a sinusoidal fashionsAxis, zsOn the axis, with frequency equal to the angular velocity of rotation of the wheel, and ysThe centrifugal acceleration due to the rotation of the wheel, the magnitude of which is the product of the square of the angular velocity of rotation of the wheel and the distance of the IMU from the wheel center, will be added to the axis, and the rotational velocity of the wheel will also be projected on the x-axis of the gyroscope. And adjusting the distance from the IMU to the wheel center to avoid the actual acceleration exceeding the upper limit of the output of the accelerometer due to centripetal acceleration. The sensor is installed and constantly rotates on the wheel, and the integrated wireless communication device of sensor carries out information interaction including bluetooth, WIFI and the control system on the vehicle.
Step 2, output modeling of IMU under carrier motion constraint condition
Considering the error characteristic of the MEMS IMU, neglecting the influence caused by the rotation of the earth, the output of the accelerometer can be expressed by the sum of the gravity acceleration, the centripetal acceleration generated by the rotation of the wheels and the acceleration of the vehicle relative to the ground, and the local horizontal coordinate system is taken as a navigation coordinate system n system; recording:
Figure BDA0002624757000000071
Figure BDA0002624757000000072
the projection of the gravitational acceleration on the sensor coordinate system is then:
Figure BDA0002624757000000081
where g is the local gravitational acceleration, θ is the angle of counterclockwise rotation of the IMU relative to the home position,
Figure BDA0002624757000000082
representing a rotation matrix of b to s,
Figure BDA0002624757000000083
is a rotation matrix of n to b, RijIs composed of
Figure BDA0002624757000000084
The elements of (1); i is 1, 2, 3; j is 1, 2, 3;
the centripetal acceleration generated by the rotation of the wheel is projected on ysThe negative axis direction, can be expressed as:
Figure BDA0002624757000000085
r is the distance of the IMU from the wheel center,
Figure BDA0002624757000000086
the first derivative of theta, the angular velocity of wheel rotation.
The acceleration of the vehicle under b can be expressed as
Figure BDA0002624757000000087
Considering the motion constraints of the vehicle, the lateral and the zenith accelerations are zero:
Figure BDA0002624757000000088
the vehicle acceleration in the vertical axis direction is represented by the wheel rotation angular acceleration multiplied by the wheel radius:
Figure BDA0002624757000000089
r is the radius of the wheel,
Figure BDA00026247570000000810
the second derivative of θ, the angular acceleration of wheel rotation.
So that the acceleration of the vehicle with respect to the ground is
Figure BDA00026247570000000811
Projected to the sensor coordinate system:
Figure BDA00026247570000000812
output of accelerometer in IMU
Figure BDA00026247570000000813
The relationship to θ is:
Figure BDA0002624757000000091
in addition, neglecting earth rotation, gyroscope xsThe output of the shaft is approximately equal to the angular velocity of the wheel rotating relative to the carrier:
Figure BDA0002624757000000092
step 3, establishing a rotation angle observation model based on an accelerometer and a gyroscope
When the rotating speed of the wheel is too high and the sampling rate of the accelerometer is low, the accelerometer cannot calculate the real rotating speed according to the sampling law, the output angular speed of the gyroscope is 'absolute', the shortage of the sampling rate of the accelerometer can be made up, and meanwhile, because the MEMS-level gyroscope often has larger errors, the rotating speed errors caused by the gyroscope can also be reduced due to the angular speed derived by the accelerometer. The invention uses the Extended Kalman Filter (EKF) to fuse the data of the accelerometer and the gyroscope, and makes up the respective defects to obtain more accurate wheel rotation angle and angular velocity.
Considering that the accelerometer output model in the step 2 respectively contains theta,
Figure BDA0002624757000000093
Use of the invention
Figure BDA0002624757000000094
The state quantities are represented by theta,
Figure BDA0002624757000000095
The relationship between them establishes a state update equation. Only accelerometer ysOutput of the shaft
Figure BDA0002624757000000096
zsOutput of the shaft
Figure BDA0002624757000000097
Gyroscope xsOutput of the shaft
Figure BDA0002624757000000098
Related to X, so use
Figure BDA0002624757000000099
As observed quantities, the equations (9) and (10) are linearized to establish a measurement update equation:
Figure BDA00026247570000000910
the state update equation:
Figure BDA00026247570000000911
wherein [ wx wy wz]TFor sensor noise of the corresponding axis, [ v ]x vy vz]TTo measure noise;
step 4, calculating speed and mileage and updating parameters
Theta and theta are obtained in each step 3
Figure BDA0002624757000000101
Then, mileage and speed need to be calculated, and parameters need to be updated simultaneously
Figure BDA0002624757000000102
For the next cycle.
When the vehicle normally runs, the wheel and the ground do not slide relatively, the wheel rolls on the ground for a distance equal to the running distance of the vehicle,
Pk＝P0+θkR (10)
Figure BDA0002624757000000103
P0is the initial mileage, PkMileage at time K, θk、
Figure BDA0002624757000000104
For the angle and angular velocity the wheel turned at time K,
Figure BDA0002624757000000105
r is the wheel radius, which is the speed of the vehicle at time K.
However, in actual conditions, due to the influence of factors such as vehicle speed, humidity, temperature, wind resistance coefficient, friction coefficient and the like, relative sliding often occurs, even the radius of a wheel is different due to changes of load and tire pressure, so that errors exist in calculated mileage, the types of the factors causing the errors are various, and the measurement and modeling by using a traditional method are difficult. The factors can change slowly, such as humidity, temperature, wind resistance coefficient, friction coefficient, tire pressure and the like, or the vehicle speed can be obtained from the previous steps, so the invention introduces the BP neural network, performs parameter training on the BP neural network by using GPS data when GPS signals exist, and predicts mileage and vehicle speed by using the BP neural network when GPS signals do not exist. The working principle of the BP algorithm is that firstly, the output of each layer of nodes is calculated through the weight between the nodes, the error is obtained by comparing the output layer with the expected output, and then the error is propagated reversely, which is based on the Widrow-Hoff learning rule, namely, the connection weight between the nodes is adjusted towards the direction that the error is reduced through the steepest descending direction of the sum of squares of relative errors, and the weight and the offset of the network are continuously adjusted. A new round of calculation is then performed until the error value reaches the expectation or its training number reaches a threshold.
The invention designs a three-layer feedforward neural network comprising a two-node input layer, a five-node hidden layer and a single-node output layer, and a sigmoid function is used as an activation function. As shown in FIG. 3(a), in the training stage, the input layers are the wheel turning angle and the angular velocity θ,
Figure BDA0002624757000000106
The output layer is the difference between the speed measurement value of the GPS and the speed in the formula (11)
Figure BDA0002624757000000107
And carrying out parameter training on the neural network. In the prediction stage, as shown in FIG. 3(b), the input is only θ,
Figure BDA00026247570000001012
Predicting that the system will output odometer speed
Figure BDA0002624757000000108
Relative to GPS velocity
Figure BDA0002624757000000109
Velocity difference v ofkWill be wrong
Figure BDA00026247570000001010
And vkAnd
Figure BDA00026247570000001011
as the final speed output of the system. In the training stage of the model, a mode of combining offline learning and online learning is used, namely, enough offline training is performed in advance when GPS signals exist, the obtained weight values of all nodes are stored as basic weight values, and online training is performed by taking the basic weight values as initial weight values when GPS signals exist in the actual vehicle running every time, so that the training time can be shortened, the input-output relation can be more consistent with the current conditions, errors are reduced, and the problem that the GPS signals cannot be searched in the initial running stage of the vehicle can be solved.
When the vehicle loses the GPS signal, the system enters a prediction mode to predict the speed error v at the current momentkThen the final system output speed VkAnd mileage PkComprises the following steps:
Figure BDA0002624757000000111
Figure BDA0002624757000000112
dt is the sampling time interval.
The parameter R in equation 8 during vehicle motion13、R23、R33Always varying with time, the need for R13、R23、R33And continuously updating. As shown in FIG. 4, R13、R23、R33As a matrix
Figure BDA0002624757000000113
The elements in (1) are updated after each EKF update through a formula
Figure BDA0002624757000000114
Computing
Figure BDA0002624757000000115
And (6) updating. Wherein the content of the first and second substances,
Figure BDA0002624757000000116
the obtained θ in step 3 is calculated according to the formula (1), and
Figure BDA0002624757000000117
the output of the IMU is calculated by using a traditional strapdown inertial navigation attitude update equation, namely:
Figure BDA0002624757000000118
Figure BDA0002624757000000119
is a rotation matrix of s to n,
Figure BDA00026247570000001110
is composed of
Figure BDA00026247570000001111
The derivative of (a) of (b),
Figure BDA00026247570000001112
a corresponding oblique symmetric matrix is output for the gyroscope,
Figure BDA00026247570000001113
is a diagonal symmetric matrix corresponding to the expression of the rotation angular velocity of the system of n relative to the system of i of the system of inertia in the system of s.
Although illustrative embodiments of the present invention have been described above to facilitate the understanding of the present invention by those skilled in the art, it should be understood that the present invention is not limited to the scope of the embodiments, but various changes may be apparent to those skilled in the art, and it is intended that all inventive concepts utilizing the inventive concepts set forth herein be protected without departing from the spirit and scope of the present invention as defined and limited by the appended claims.

Claims (9)

1. A low-cost odometer design method based on MEMS IMU is characterized by comprising the following steps:
step 1: the IMU is arranged on the side surface of the wheel, so that the IMU rotates along with the wheel, and the non-gravitational acceleration and the angular velocity of the wheel relative to an inertial system are measured;
step 2: aiming at an IMU (inertial measurement Unit) arranged on a wheel, considering the motion constraint of a vehicle at the same time, and establishing an output model of the IMU;
and step 3: taking the wheel rotation angle, the angular velocity and the angular acceleration as state quantities, establishing a Kalman filtering system model based on a constraint relation between the states, and establishing an observation model based on an IMU output model;
and 4, step 4: and calculating speed and mileage based on the artificial neural network and updating parameters.
2. The MEMS IMU-based low-cost odometer design method of claim 1, wherein the step 1 specifically comprises:
an MEMS-level IMU sensor comprising a three-axis accelerometer and a three-axis gyroscope is arranged on the side surface of a rear wheel of a vehicle or a wheeled robot, and a sensor coordinate system s-x is established by taking the IMU as an originsyszsLet ysAxial direction directed toward the wheel center, xsAxis directed outwards perpendicular to the side of the tire, ysAxis, zsThe axle lies in the plane of rotation of the wheel. Establishing a carrier coordinate system b-xbybzbIs that x isbAxis and xsThe axes coincide, ybThe axis being directed in front of the vehicle, zbAxis perpendicular to xbAxis and ybIn the axial direction; when the wheel starts to rotate, s is wound around xsThe axis rotates and the gravitational acceleration will be projected onto y periodically in a sinusoidal fashionsAxis, zsOn the axis, with frequency equal to the angular velocity of rotation of the wheel, and ysThe centrifugal acceleration generated by the rotation of the wheel is added to the shaft, the magnitude of the centrifugal acceleration is the product of the square of the rotation angular velocity of the wheel and the distance between the IMU and the wheel center of the wheel, and the rotation speed of the wheelWill also project on the x-axis of the gyroscope.
3. The MEMS IMU based low cost odometer design method of claim 2, further comprising: by adjusting the distance from the IMU to the wheel center, the fact that the actual acceleration exceeds the upper limit output by the accelerometer due to centripetal acceleration is avoided; the sensor is installed and constantly rotates on the wheel, and the integrated wireless communication device of sensor carries out information interaction including bluetooth, WIFI and the control system on the vehicle.
4. The MEMS IMU-based low-cost odometer design method of claim 1, wherein the step 2 specifically comprises:
the accelerometer output is expressed by the sum of gravity acceleration, centripetal acceleration generated by wheel rotation and acceleration of the vehicle relative to the ground, and a local horizontal coordinate system is taken as a navigation coordinate system n; recording:
Figure FDA0002624756990000011
Figure FDA0002624756990000021
the projection of the gravitational acceleration on the sensor coordinate system is then:
Figure FDA0002624756990000022
where g is the local gravitational acceleration, θ is the angle of counterclockwise rotation of the IMU relative to the home position,
Figure FDA0002624756990000023
representing a rotation matrix of b to s,
Figure FDA0002624756990000024
is a rotation matrix of n to b, RijIs composed of
Figure FDA0002624756990000025
The elements of (1); i is 1, 2, 3; j is 1, 2, 3;
the centripetal acceleration generated by the rotation of the wheel is projected on ysAxial negative direction, expressed as:
Figure FDA0002624756990000026
r is the distance of the IMU from the wheel center,
Figure FDA0002624756990000027
is the first derivative of θ, the angular velocity of wheel rotation;
the acceleration of the vehicle is expressed as
Figure FDA0002624756990000028
Considering the motion constraints of the vehicle, the lateral and the zenith accelerations are zero:
Figure FDA0002624756990000029
the vehicle acceleration in the vertical axis direction is represented by the wheel rotation angular acceleration multiplied by the wheel radius:
Figure FDA00026247569900000210
r is the radius of the wheel,
Figure FDA00026247569900000211
is the second derivative of θ, the angular acceleration of wheel rotation;
so that the acceleration of the vehicle with respect to the ground is
Figure FDA00026247569900000212
Projected to the sensor coordinate system:
Figure FDA00026247569900000213
output of accelerometer in IMU
Figure FDA0002624756990000031
The relationship to θ is:
Figure FDA0002624756990000032
neglecting earth rotation, gyroscope xsThe output of the shaft is approximately equal to the angular velocity of the wheel rotating relative to the carrier:
Figure FDA0002624756990000033
5. the MEMS IMU-based low-cost odometer design method of claim 1, wherein the step 3 specifically comprises:
the data fusion of the accelerometer and the gyroscope by using the extended Kalman filter EKF makes up respective defects to obtain more accurate wheel rotation angle and angular velocity, and specifically comprises the following steps: considering that the accelerometer output model in the step 2 respectively contains theta,
Figure FDA0002624756990000034
Use of
Figure FDA0002624756990000035
The state quantities are represented by theta,
Figure FDA0002624756990000036
Establishing a state updating equation according to the relationship between the two;only accelerometer ysOutput of the shaft
Figure FDA0002624756990000037
zsOutput of the shaft
Figure FDA0002624756990000038
Gyroscope xsOutput of the shaft
Figure FDA0002624756990000039
Related to X, so use
Figure FDA00026247569900000310
As observed quantities, the equations (9) and (10) are linearized to establish a measurement update equation:
Figure FDA00026247569900000311
the state update equation:
Figure FDA00026247569900000312
wherein [ wx wy wz]TFor sensor noise of the corresponding axis, [ v ]x vy vz]TTo measure noise.
6. The MEMS IMU-based low-cost odometer design method of claim 5, wherein the step 4 specifically comprises: in step 3, theta and
Figure FDA0002624756990000041
then, mileage and speed need to be calculated; when the vehicle normally runs, the wheels roll on the ground for a distance equal to the running distance of the vehicle,
Pk＝P0+θkR (10)
Figure FDA0002624756990000042
P0is the initial mileage, PkMileage at time K, θk、
Figure FDA0002624756990000043
For the angle and angular velocity the wheel turned at time K,
Figure FDA0002624756990000044
r is the wheel radius, which is the speed of the vehicle at time K.
7. The MEMS IMU based low cost odometer design method of claim 6, wherein a BP neural network is introduced, the BP neural network is subjected to parameter training using GPS data when GPS signals exist, and the BP neural network is used for predicting mileage and vehicle speed when GPS signals do not exist; the neural network comprises a three-layer feedforward neural network with a two-node input layer, a five-node hidden layer and a single-node output layer, and a sigmoid function is used as an activation function; in the training stage, the input layer is the wheel rotation angle and the angular speed theta,
Figure FDA0002624756990000045
The output layer is the difference between the speed measurement value of the GPS and the speed in the formula (11)
Figure FDA0002624756990000046
Carrying out parameter training on the neural network; in the prediction stage, only the input is theta,
Figure FDA0002624756990000047
Predicting that the system will output odometer speed
Figure FDA0002624756990000048
Relative to GPS velocity
Figure FDA0002624756990000049
Velocity difference v ofkWill be wrong
Figure FDA00026247569900000410
And vkAnd
Figure FDA00026247569900000411
as the final speed output of the system.
8. The MEMS IMU based low cost odometer design method of claim 7, wherein when the vehicle loses GPS signal, the system enters a prediction mode to predict the speed error v at the current timekThen the final system output speed VkAnd mileage PkComprises the following steps:
Figure FDA00026247569900000412
Figure FDA00026247569900000413
dt is the sampling time interval.
9. The MEMS IMU based low cost odometer design method of claim 5,
in step 3, theta and
Figure FDA00026247569900000421
later, the matrix needs to be updated
Figure FDA00026247569900000414
To perform the next cycle; during the movement of the vehicleParameter R in formula (8)13、R23、R33Always varying with time, the need for R13、R23、R33Continuously updating; r13、R23、R33As a matrix
Figure FDA00026247569900000415
The elements in (1) are updated after each EKF update through a formula
Figure FDA00026247569900000416
Computing
Figure FDA00026247569900000417
Updating is carried out; wherein the content of the first and second substances,
Figure FDA00026247569900000418
the obtained θ in step 3 is calculated according to the formula (1), and
Figure FDA00026247569900000419
calculating by the output of the IMU with a strapdown inertial navigation attitude update equation, namely:
Figure FDA00026247569900000420
Figure FDA0002624756990000051
is a rotation matrix of s to n,
Figure FDA0002624756990000052
is composed of
Figure FDA0002624756990000053
The derivative of (a) of (b),
Figure FDA0002624756990000054
corresponding diagonal pairs for gyroscope outputsThe matrix is called, and the matrix is called,
Figure FDA0002624756990000055
is a diagonal symmetric matrix corresponding to the expression of the rotation angular velocity of the system of n relative to the system of i of the system of inertia in the system of s.
CN202010793803.2A 2020-08-10 2020-08-10 Low-cost odometer design method based on MEMS IMU Pending CN111912426A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010793803.2A CN111912426A (en) 2020-08-10 2020-08-10 Low-cost odometer design method based on MEMS IMU

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010793803.2A CN111912426A (en) 2020-08-10 2020-08-10 Low-cost odometer design method based on MEMS IMU

Publications (1)

Publication Number Publication Date
CN111912426A true CN111912426A (en) 2020-11-10

Family

ID=73283357

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010793803.2A Pending CN111912426A (en) 2020-08-10 2020-08-10 Low-cost odometer design method based on MEMS IMU

Country Status (1)

Country Link
CN (1) CN111912426A (en)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112697153A (en) * 2020-12-31 2021-04-23 广东美的白色家电技术创新中心有限公司 Positioning method of autonomous mobile device, electronic device and storage medium
CN112902982A (en) * 2021-01-18 2021-06-04 惠州市德赛西威汽车电子股份有限公司 Method and system for accurately calculating vehicle mileage and automobile
CN113009173A (en) * 2021-02-02 2021-06-22 惠州市德赛西威汽车电子股份有限公司 High-precision vehicle speed calculation method
CN113155154A (en) * 2021-04-07 2021-07-23 扬州大学 Error correction method based on attitude and mileage of sensor and camera
CN114152269A (en) * 2021-11-09 2022-03-08 南京邮电大学 On-site calibration method for installation parameters of wheel installation inertia measurement unit
CN114353822A (en) * 2022-01-11 2022-04-15 深圳市几米物联有限公司 Trailer mileage recording method, trailer and recording system
CN114353822B (en) * 2022-01-11 2024-05-24 深圳市几米物联有限公司 Trailer mileage recording method, trailer and recording system

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080071476A1 (en) * 2006-09-19 2008-03-20 Takayuki Hoshizaki Vehicle dynamics conditioning method on MEMS based integrated INS/GPS vehicle navigation system
US8914196B1 (en) * 2013-11-01 2014-12-16 Automotive Technologies International, Inc. Crash sensor systems utilizing vehicular inertial properties
CN105737842A (en) * 2016-03-23 2016-07-06 南京航空航天大学 Vehicle-mounted autonomous navigation method based on rotary modulation and virtual odometer
CN105823481A (en) * 2015-12-21 2016-08-03 上海华测导航技术股份有限公司 GNSS-INS vehicle attitude determination method based on single antenna
CN106767847A (en) * 2016-12-15 2017-05-31 北京三驰科技发展有限公司 A kind of vehicle attitude safe early warning method and system
CN107390246A (en) * 2017-07-06 2017-11-24 电子科技大学 A kind of GPS/INS Combinated navigation methods based on genetic neural network
CN109506674A (en) * 2017-09-15 2019-03-22 高德信息技术有限公司 A kind of bearing calibration of acceleration and device
CN110873575A (en) * 2020-01-17 2020-03-10 立得空间信息技术股份有限公司 Mileage measurement method based on inertial sensor
CN110887481A (en) * 2019-12-11 2020-03-17 中国空气动力研究与发展中心低速空气动力研究所 Carrier dynamic attitude estimation method based on MEMS inertial sensor
CN110954102A (en) * 2019-12-18 2020-04-03 无锡北微传感科技有限公司 Magnetometer-assisted inertial navigation system and method for robot positioning

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080071476A1 (en) * 2006-09-19 2008-03-20 Takayuki Hoshizaki Vehicle dynamics conditioning method on MEMS based integrated INS/GPS vehicle navigation system
US8914196B1 (en) * 2013-11-01 2014-12-16 Automotive Technologies International, Inc. Crash sensor systems utilizing vehicular inertial properties
CN105823481A (en) * 2015-12-21 2016-08-03 上海华测导航技术股份有限公司 GNSS-INS vehicle attitude determination method based on single antenna
CN105737842A (en) * 2016-03-23 2016-07-06 南京航空航天大学 Vehicle-mounted autonomous navigation method based on rotary modulation and virtual odometer
CN106767847A (en) * 2016-12-15 2017-05-31 北京三驰科技发展有限公司 A kind of vehicle attitude safe early warning method and system
CN107390246A (en) * 2017-07-06 2017-11-24 电子科技大学 A kind of GPS/INS Combinated navigation methods based on genetic neural network
CN109506674A (en) * 2017-09-15 2019-03-22 高德信息技术有限公司 A kind of bearing calibration of acceleration and device
CN110887481A (en) * 2019-12-11 2020-03-17 中国空气动力研究与发展中心低速空气动力研究所 Carrier dynamic attitude estimation method based on MEMS inertial sensor
CN110954102A (en) * 2019-12-18 2020-04-03 无锡北微传感科技有限公司 Magnetometer-assisted inertial navigation system and method for robot positioning
CN110873575A (en) * 2020-01-17 2020-03-10 立得空间信息技术股份有限公司 Mileage measurement method based on inertial sensor

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
ZHIMING LI，等: "Attitude Determination for Land Vehicles Based on Low Cost GPS/MEMS-IMU", 《2018 EIGHTH INTERNATIONAL CONFERENCE ON INSTRUMENTATION AND MEASUREMENT, COMPUTER, COMMUNICATION AND CONTROL》 *
陈映秋，等: "基于车轮安装惯性测量单元的车载组合导航", 《中国惯性技术学报》 *

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112697153A (en) * 2020-12-31 2021-04-23 广东美的白色家电技术创新中心有限公司 Positioning method of autonomous mobile device, electronic device and storage medium
CN112902982A (en) * 2021-01-18 2021-06-04 惠州市德赛西威汽车电子股份有限公司 Method and system for accurately calculating vehicle mileage and automobile
CN112902982B (en) * 2021-01-18 2023-11-17 惠州市德赛西威汽车电子股份有限公司 Vehicle driving mileage accurate calculation method, system and automobile
CN113009173A (en) * 2021-02-02 2021-06-22 惠州市德赛西威汽车电子股份有限公司 High-precision vehicle speed calculation method
WO2022166083A1 (en) * 2021-02-02 2022-08-11 惠州市德赛西威汽车电子股份有限公司 High-precision vehicle velocity calculation method
CN113009173B (en) * 2021-02-02 2023-10-03 惠州市德赛西威汽车电子股份有限公司 High-precision vehicle speed calculation method
CN113155154A (en) * 2021-04-07 2021-07-23 扬州大学 Error correction method based on attitude and mileage of sensor and camera
CN113155154B (en) * 2021-04-07 2022-09-20 扬州大学 Error correction method based on attitude and mileage of sensor and camera
CN114152269A (en) * 2021-11-09 2022-03-08 南京邮电大学 On-site calibration method for installation parameters of wheel installation inertia measurement unit
CN114152269B (en) * 2021-11-09 2024-03-22 南京邮电大学 On-site calibration method for installation parameters of wheel installation inertia measurement unit
CN114353822A (en) * 2022-01-11 2022-04-15 深圳市几米物联有限公司 Trailer mileage recording method, trailer and recording system
CN114353822B (en) * 2022-01-11 2024-05-24 深圳市几米物联有限公司 Trailer mileage recording method, trailer and recording system

Similar Documents

Publication Publication Date Title
CN111912426A (en) Low-cost odometer design method based on MEMS IMU
Guo et al. Vehicle dynamic state estimation: State of the art schemes and perspectives
CN110207697B (en) Inertial navigation resolving method based on angular accelerometer/gyroscope/accelerometer
Xia et al. Vehicle sideslip angle estimation by fusing inertial measurement unit and global navigation satellite system with heading alignment
CN105300379B (en) A kind of Kalman filtering Attitude estimation method and system based on acceleration
Song et al. Vehicle state estimation for INS/GPS aided by sensors fusion and SCKF-based algorithm
CN107063305B (en) Method for correcting downhill suspended rear wheel odometer error by using inertial navigation and pressure sensors
CN109870173A (en) A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
CN106500693A (en) A kind of AHRS algorithms based on adaptive extended kalman filtering
CN110239462A (en) A kind of method for early warning and system of vehicle rollover
Li et al. Vehicle sideslip angle estimation based on hybrid Kalman filter
CN111678514B (en) Vehicle-mounted autonomous navigation method based on carrier motion condition constraint and single-axis rotation modulation
CN105371846A (en) Carrier attitude detection method and system
CN115406446A (en) Multi-axis special vehicle state estimation method based on neural network and unscented Kalman filtering
CN106646569B (en) Navigation positioning method and equipment
Wang et al. Accuracy and robustness of ODO/NHC measurement models for wheeled robot positioning
Song et al. Reliable estimation of automotive states based on optimized neural networks and moving horizon estimator
Lyu et al. A SINS/GNSS/VDM integrated navigation fault-tolerant mechanism based on adaptive information sharing factor
CN115451949A (en) Vehicle positioning method based on wheel installation inertia measurement unit
Kumar et al. Integrating on-board diagnostics speed data with sparse GPS measurements for vehicle trajectory estimation
CN109033017B (en) Vehicle roll angle and pitch angle estimation method under packet loss environment
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN110567456B (en) BDS/INS combined train positioning method based on robust Kalman filtering
CN110737194B (en) Multi-MEMS sensor combination attitude measurement method
CN116039636B (en) Wheel slip considered self-adaptive neural network control method for wheeled mobile robot

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
AD01 Patent right deemed abandoned
AD01 Patent right deemed abandoned

Effective date of abandoning: 20231110