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

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

A low-cost odometer design method based on MEMS IMU

technical field

The invention belongs to the field of navigation, in particular to an odometer design method based on MEMS IMU

Background technique

With the rapid development of transportation worldwide, achieving reliable and accurate vehicle positioning is becoming more and more important in various vehicle navigation and safety-related applications such as route navigation, autonomous driving, and intelligent transportation. Integrated navigation systems based on Global Positioning System (GPS) and Inertial Navigation System (INS) have been widely used in vehicles to provide accurate position and speed information, but during GPS disconnection, INS errors will accumulate rapidly and lead to divergence, especially In vehicle applications, limited by commercial costs, MEMS-grade inertial measurement units (IMUs) with larger errors are often used, which will lead to faster divergence of errors.

In recent years, many scholars have studied the problem of INS error divergence during GPS signal disconnection, and introduced auxiliary sensors such as car odometer to correct navigation errors. The car odometer can provide absolute speed information, which can reduce the position error caused by the quadratic integration of the accelerometer data, but it is difficult to use due to the different odometer data standards and connection standards of different vehicles.

In addition, in robotics, wheeled robots have attracted much attention due to their mobility, stability, and high efficiency. They have been widely used in warehousing and logistics, smart shopping guides, self-service, and automatic inspection. The wheels are relatively small, and odometers are often designed with expensive sensors such as encoders for assisted navigation, which greatly increases the cost of wheeled robots.

SUMMARY OF THE INVENTION

The problem to be solved by the present invention is: in order to make up for the deficiencies of the odometer in the existing vehicles and wheeled robots, the present invention proposes a novel low-cost odometer design method based on MEMS IMU.

The invention adopts the three-axis MEMS IMU installed on the side of the wheel of the vehicle or wheeled robot. When the wheel rotates, the output of the accelerometer will change regularly. For example, the projection of the gravitational acceleration on the sensitive axis of the accelerometer will take the frequency as the rotational angular velocity The present invention models the relationship between the accelerometer output and the wheel rotational angular velocity to extract the wheel rotational angular velocity information. At the same time, the invention also uses the gyroscope to correct the angular velocity information: when the wheel speed is too fast and the sampling rate of the accelerometer is low, the accelerometer will not be able to calculate the real speed according to the sampling law, and the output angular velocity of the gyroscope is "absolute", which can compensate for the acceleration Due to the lack of sampling rate of the meter, and because MEMS-level gyroscopes often have large errors, the angular velocity derived from the accelerometer can also reduce the rotational speed error caused by the gyroscope. The invention uses the extended Kalman filter to fuse the data of the accelerometer and the gyroscope, and makes up for their respective deficiencies to obtain a more accurate wheel rotation angle and angular velocity. Combined with the relationship between the vehicle mileage and the wheel rotation angle, the mileage and speed of the vehicle or wheeled robot are calculated.

The technical scheme of the present invention is: a low-cost odometer design method based on MEMS IMU, comprising the following steps:

Step 1: Install the IMU on the side of the wheel, make the IMU rotate with the wheel, and measure the non-gravitational acceleration and angular velocity of the wheel relative to the inertial frame;

Step 2: For the IMU installed on the wheel, while considering the motion constraints of the vehicle, establish the output model of the IMU;

Step 3: Take the wheel rotation angle, angular velocity, and angular acceleration as state quantities, establish a system model of Kalman filtering based on the constraint relationship between the states, and establish an observation model based on the output model of the IMU;

Step 4: Speed, mileage calculation and parameter update based on artificial neural network.

Further, the step 1 specifically includes:

A MEMS-level IMU sensor including a three-axis accelerometer and a three-axis gyroscope is installed on the side of the rear wheel of the vehicle or wheeled robot, and the sensor coordinate system sx ^s y ^s z ^s is established with the IMU as the origin, and the y ^s axis is negative. The direction points to the wheel center, the x ^s axis is perpendicular to the tire side and outward, and the y ^s axis and the z ^s axis are located in the wheel rotation plane. The carrier coordinate system bx ^b y ^b z ^b is established, so that the x ^b axis coincides with the x ^s axis, the y ^b axis points to the front of the vehicle, the z ^b axis is perpendicular to the x ^b axis and the y ^b axis is upward; when the wheel starts to rotate, s The system rotates around the ^{x s} axis, and the gravitational acceleration will be periodically projected to the y ^s axis and the z ^s axis in a sinusoidal form, and its frequency is equal to the rotational angular velocity of the wheel. is the product of the square of the rotational angular velocity of the wheel and the distance from the IMU to the wheel center. The rotational velocity of the wheel will also be projected on the x-axis of the gyroscope.

Further, the step 1 also includes:

By adjusting the distance from the IMU to the wheel center, the actual acceleration will not exceed the upper limit of the accelerometer output due to centripetal acceleration; the sensor is installed on the wheel and rotates continuously, and the sensor integrates wireless communication devices, including Bluetooth, WIFI and the control system on the vehicle information exchange.

Further, the step 2 specifically includes:

The output of the accelerometer is represented by the sum of the acceleration of gravity, the centripetal acceleration generated by the rotation of the wheel and the acceleration of the vehicle relative to the ground, and the local horizontal coordinate system is used as the navigation coordinate system n system; note:

Then the projection of the gravitational acceleration in the sensor coordinate system is:

表示b系到s系的旋转矩阵，

为n系到b系的旋转矩阵，R_ij为

中的元素；i＝1，2，3；j＝1，2，3；where g is the local gravitational acceleration, θ is the counterclockwise rotation angle of the IMU relative to the starting position,

represents the rotation matrix from the b system to the s system,

is the rotation matrix from the n system to the b system, and R _ij is

elements in; i=1, 2, 3; j=1, 2, 3;

r为IMU距轮心的距离，

为θ的一阶导数，即车轮旋转的角速度；The centripetal acceleration generated by the rotation of the wheel is projected in the negative direction of the y ^s axis and expressed as:

r is the distance from the IMU to the wheel center,

is the first derivative of θ, that is, the angular velocity of wheel rotation;

考虑到车辆的运动约束，横向和天向加速度为零即：The acceleration of the vehicle in the b system can be expressed as

Considering the motion constraints of the vehicle, the lateral and celestial accelerations are zero, namely:

The vehicle acceleration in the direction of the longitudinal axis is expressed as the wheel rotational angular acceleration multiplied by the wheel radius:

为θ的二阶导数，即车轮旋转的角加速度；R is the wheel radius,

is the second derivative of θ, that is, the angular acceleration of wheel rotation;

投影到传感器坐标系为：Therefore, the acceleration of the vehicle relative to the ground in the b system is

Projection to the sensor coordinate system is:

与θ的关系为：So the output of the accelerometer in the IMU

The relationship with θ is:

Neglecting the Earth's rotation, the output of the gyroscope's x ^s axis is approximately equal to the angular velocity of the wheel's rotation relative to the carrier:

Further, the step 3 specifically includes:

使用

作为状态量，根据θ、

之间的关系建立状态更新方程；只有加速度计y^s轴的输出

z^s轴的输出

陀螺仪x^s轴的输出

与X相关，故使用

作为观测量，依此对公式(9)(10)进行线性化建立量测更新方程：Use the extended Kalman filter EKF to fuse the data of the accelerometer and the gyroscope to make up for their respective shortcomings to obtain a more accurate wheel rotation angle and angular velocity. Specifically, it includes: considering that the accelerometer output model in step 2 contains θ,

use

As a state quantity, according to θ,

The relationship between establishes the state update equation; only the output of the y ^s axis of the accelerometer

z ^s axis output

The output of the gyroscope x ^s axis

related to X, so use

As the observed amount, the equation (9) (10) is linearized according to this to establish the measurement update equation:

State update equation:

where [w _x w _y w _z ] ^T is the sensor noise of the corresponding axis, and [v _x v _y v _z ] ^T is the measurement noise.

后，需要计算里程与速度，在车辆正常行驶时，车轮与地面没有产生相对滑动，车轮在地面滚动过的距离等于车辆的行驶距离，Further, the step 4 specifically includes: obtaining θ and

After that, it is necessary to calculate the mileage and speed. When the vehicle is running normally, there is no relative sliding between the wheels and the ground, and the distance the wheels roll on the ground is equal to the driving distance of the vehicle.

P ^k =P ⁰ +θ ^k R (10)

为K时刻车轮转过的角度和角速度，

为车辆在K时刻行驶速度，R为车轮半径。P ⁰ is the initial mileage, P ^k is the mileage at time K, θ ^k ,

is the angle and angular velocity of the wheel at time K,

is the speed of the vehicle at time K, and R is the wheel radius.

However, in actual situations, affected by factors such as vehicle speed, humidity, temperature, wind resistance coefficient, friction coefficient, etc., relative slippage often occurs, and even the radius of the wheel will vary due to changes in load and tire pressure, resulting in the calculated mileage. There are errors, and there are many types of factors that cause these errors, which are difficult to measure and model with traditional methods. These factors either change slowly, such as humidity, temperature, wind resistance coefficient, friction coefficient, tire pressure, etc., or they can be obtained from the previous steps, such as vehicle speed, so the invention introduces BP neural network, and uses GPS data to BP neural network when there is a GPS signal. Parameter training is performed to predict mileage and vehicle speed using BP neural network when there is no GPS signal. The working principle of the BP algorithm is to first calculate the output of each layer of nodes through the weights between the nodes, compare the output layer with the expected output to get the error, and then propagate the error back. It is based on the Widrow-Hoff learning rule. , that is, through the steepest descent direction of the relative sum of squared errors, the connection weights between each node are adjusted in the direction of decreasing error, and the weights and offsets of the network are continuously adjusted. After that, a new round of calculation is performed until the error value reaches the expected value or the number of training times reaches the threshold.

输出层为GPS的速度测量值与公式(11)中速度的差值

对神经网络进行参数训练。而在预测阶段,只需要输入为θ、

预测系统将输出里程计速度

相对于GPS速度

的速度差值δv^k，将误差

与δv^k的和

作为系统最终的速度输出。在模型的训练阶段，使用离线学习与在线学习相结合的方式，即在有GPS信号时预先进行足够的离线训练，将得到的各节点权值储存作为基础权值，而在每次实际的车辆运行中当有GPS信号时以基础权值作为初始权值进行在线训练，这样可以缩短训练时间的同时可以使输入输出关系更符合当时的条件减小误差，也可以解决车辆运行初期就搜索不到GPS信号的情况。The present invention designs a three-layer feedforward neural network including a two-node input layer, a five-node hidden layer, and a single-node output layer, and uses a sigmoid function as an activation function. In the training phase, the input layer is the wheel rotation angle and angular velocity θ,

The output layer is the difference between the GPS velocity measurement and the velocity in equation (11)

Parameter training of the neural network. In the prediction stage, only the input θ,

The prediction system will output the odometer speed

Relative to GPS speed

The speed difference δv ^k of , the error

Sum with δv ^k

As the final speed output of the system. In the training stage of the model, a combination of offline learning and online learning is used, that is, sufficient offline training is performed in advance when there is a GPS signal, and the obtained weights of each node are stored as the basic weights, and each time the actual vehicle When there is a GPS signal during operation, the basic weight is used as the initial weight for online training, which can shorten the training time and make the input-output relationship more in line with the conditions at that time to reduce errors, and can also solve the problem that the vehicle cannot be searched in the early stage of operation. GPS signal condition.

When the vehicle loses the GPS signal, the system enters the prediction mode to predict the speed error δv ^k at the current moment, then the final system output speed V ^k and mileage P ^k are:

dt is the sampling time interval.

中的元素，在每一次EKF更新后通过公式

计算

进行更新。其中，

在步骤3中得到θ根据公式(1)计算，而

由IMU的输出用传统的捷联惯导姿态更新方程进行计算，即：During the movement of the vehicle, the parameters R ₁₃ , R ₂₃ , and R ₃₃ in formula (8) change with time, and it is necessary to continuously update R ₁₃ , R ₂₃ , and R ₃₃ . R ₁₃ , R ₂₃ , R ₃₃ as matrices

elements in , passed the formula after each EKF update

calculate

to update. in,

Obtained in step 3 θ is calculated according to formula (1), while

Calculated from the output of the IMU with the traditional SINS attitude update equation, namely:

为s系到n系的旋转矩阵，

为

的导数，

为陀螺仪输出对应的斜对称矩阵，

为n系相对惯性系i系的旋转角速度在s系中的表达对应的斜对称矩阵。

is the rotation matrix from the s system to the n system,

for

the derivative of ,

is the oblique symmetric matrix corresponding to the gyroscope output,

is the oblique symmetric matrix corresponding to the expression of the rotational angular velocity of the n system relative to the inertial system i system in the s system.

Beneficial effects of the present invention:

(1) The present invention uses the MEMS IMU to realize the calculation of mileage and speed, and has the advantages of low cost, low power consumption, simple implementation, and accurate results.

(2) Using the odometer installed at the output of the wheel IMU, for vehicles, it can solve the problem of different vehicle data standards and interface standards, and the data is difficult to use, while for wheeled robots, it reduces the cost.

(3) For most navigation applications of vehicles and wheeled robots, IMUs are often equipped themselves, which can be used, and can be transformed and used without affecting the original navigation applications. The odometer can be realized while outputting the navigation results. In turn, it will assist the navigation system and increase the navigation accuracy, which has great application potential.

Description of drawings

Figure 1: Overall flow chart of the method of the present invention.

Figure 2: IMU installation schematic diagram in the present invention;

Figure 3(a): Schematic diagram of BP neural network structure training;

Figure 3(b): Schematic diagram of BP neural network structure prediction;

Figure 4: Schematic diagram of the parameter update process.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

According to an embodiment of the present invention, a low-cost odometer design method based on MEMS IMU is proposed. FIG. 1 is a flowchart of the method of the present invention, which specifically includes the following steps:

Step 1. IMU installation scheme

Referring to Figure 2, a MEMS-level IMU including a three-axis accelerometer and a three-axis gyroscope is installed on the side of the rear wheel of the vehicle or wheeled robot, and the sensor coordinate system sx ^s y ^s z ^s is established with the IMU as the origin, so that y The negative direction of the ^s -axis points to the wheel center, the x- ^s -axis is perpendicular to the side of the tire and outwards, and the y- ^s -axis and the z- ^s -axis are located in the wheel rotation plane. The carrier coordinate system bx ^b y ^b z ^b is established, so that the x ^b axis coincides with the x ^s axis, the y ^b axis points to the front of the vehicle, the z ^b axis is perpendicular to the x ^b axis and the y ^b axis is upward. When the wheel starts to rotate, the ^s system rotates around the x ^s axis, and the gravitational acceleration will be periodically projected on the y ^s axis and the z ^s axis in a sinusoidal form, and its frequency is equal to the angular velocity of the wheel rotation. The resulting centrifugal acceleration is the product of the square of the rotational angular velocity of the wheel and the distance from the IMU to the wheel center. The rotational velocity of the wheel will also be projected on the x-axis of the gyroscope. Adjust the distance from the IMU to the wheel center to prevent the actual acceleration from exceeding the upper limit of the accelerometer output due to centripetal acceleration. The sensor is installed on the wheel and rotates continuously, and the sensor is integrated with a wireless communication device, including Bluetooth and WIFI, to exchange information with the control system on the vehicle.

Step 2. Modeling the output of the IMU under the constraints of the carrier motion

Considering the error characteristics of the MEMS IMU and ignoring the influence of the earth's rotation, the accelerometer output can be represented by the sum of the acceleration of gravity, the centripetal acceleration generated by the rotation of the wheel and the acceleration of the vehicle relative to the ground, and the local horizontal coordinate system is used as the navigation. Coordinate system n system; note:

Then the projection of the gravitational acceleration in the sensor coordinate system is:

表示b系到s系的旋转矩阵，

为n系到b系的旋转矩阵，R_ij为

中的元素；i＝1，2，3；j＝1，2，3；where g is the local gravitational acceleration, θ is the counterclockwise rotation angle of the IMU relative to the starting position,

represents the rotation matrix from the b system to the s system,

is the rotation matrix from the n system to the b system, and R _ij is

elements in; i=1, 2, 3; j=1, 2, 3;

r为IMU距轮心的距离，

为θ的一阶导数，即车轮旋转的角速度。The centripetal acceleration generated by the rotation of the wheel is projected in the negative direction of the y ^s axis and can be expressed as:

r is the distance from the IMU to the wheel center,

is the first derivative of θ, that is, the angular velocity of wheel rotation.

考虑到车辆的运动约束，横向和天向加速度为零即：The acceleration of the vehicle in the b system can be expressed as

Considering the motion constraints of the vehicle, the lateral and celestial accelerations are zero, namely:

The vehicle acceleration in the direction of the longitudinal axis is expressed as the wheel rotational angular acceleration multiplied by the wheel radius:

为θ的二阶导数，即车轮旋转的角加速度。R is the wheel radius,

is the second derivative of θ, that is, the angular acceleration of wheel rotation.

投影到传感器坐标系为：Therefore, the acceleration of the vehicle relative to the ground in the b system is

Projection to the sensor coordinate system is:

与θ的关系为：So the output of the accelerometer in the IMU

The relationship with θ is:

Also, ignoring the Earth's rotation, the output of the gyroscope's x- ^s axis is approximately equal to the angular velocity of the wheel's rotation relative to the carrier:

Step 3. Establish a rotation angle observation model based on accelerometer and gyroscope

When the wheel speed is too fast and the sampling rate of the accelerometer is low, the accelerometer will not be able to calculate the real speed according to the sampling law, and the output angular velocity of the gyroscope is "absolute", which can make up for the lack of the sampling rate of the accelerometer. There is often a large error, and the angular velocity derived from the accelerometer can also reduce the rotational speed error caused by the gyroscope. The invention uses the extended Kalman filter (EKF) to fuse the data of the accelerometer and the gyroscope, and makes up for their respective deficiencies to obtain more accurate wheel rotation angle and angular velocity.

发明使用

作为状态量，根据θ、

之间的关系建立状态更新方程。只有加速度计y^s轴的输出

z^s轴的输出

陀螺仪x^s轴的输出

与X相关，故使用

作为观测量，依此对公式(9)(10)进行线性化建立量测更新方程：Considering that the accelerometer output model in step 2 contains θ,

invention and use

As a state quantity, according to θ,

The relationship between establishes the state update equation. Only the output of the y ^s axis of the accelerometer

z ^s axis output

The output of the gyroscope x ^s axis

related to X, so use

As the observed amount, the equation (9) (10) is linearized according to this to establish the measurement update equation:

State update equation:

Where [w _x w _y w _z ] ^T is the sensor noise of the corresponding axis, [v _x v _y v _z ] ^T is the measurement noise;

Step 4. Calculation of speed and mileage and update of parameters

后，需要计算里程与速度，同时更新参数

以进行下一次循环。at each step 3 get theta and

After that, you need to calculate the mileage and speed, and update the parameters at the same time

for the next cycle.

When the vehicle is running normally, it can be considered that there is no relative sliding between the wheel and the ground, and the distance the wheel rolls on the ground is equal to the driving distance of the vehicle.

P ^k =P ⁰ +θ ^k R (10)

为K时刻车轮转过的角度和角速度，

为车辆在K时刻行驶速度，R为车轮半径。P ⁰ is the initial mileage, P ^k is the mileage at time K, θ ^k ,

is the angle and angular velocity of the wheel at time K,

is the speed of the vehicle at time K, and R is the wheel radius.

However, in actual situations, affected by factors such as vehicle speed, humidity, temperature, wind resistance coefficient, friction coefficient, etc., relative sliding often occurs, and even the radius of the wheel will vary due to changes in load and tire pressure, resulting in the calculated mileage. There are errors, and there are many types of factors that cause these errors, which are difficult to measure and model with traditional methods. These factors either change slowly, such as humidity, temperature, wind resistance coefficient, friction coefficient, tire pressure, etc., or they can be obtained from the previous steps, such as vehicle speed, so the invention introduces BP neural network, and uses GPS data to BP neural network when there is a GPS signal. Parameter training is performed to predict mileage and vehicle speed using BP neural network when there is no GPS signal. The working principle of the BP algorithm is to first calculate the output of each layer of nodes through the weights between the nodes, compare the output layer with the expected output to get the error, and then propagate the error back. It is based on the Widrow-Hoff learning rule. , that is, through the steepest descent direction of the relative sum of squared errors, the connection weights between each node are adjusted in the direction of decreasing error, and the weights and offsets of the network are continuously adjusted. After that, a new round of calculation is performed until the error value reaches the expected value or the number of training times reaches the threshold.

输出层为GPS的速度测量值与公式(11)中速度的差值

对神经网络进行参数训练。而在预测阶段，如图3(b)所示,只需要输入为θ、

预测系统将输出里程计速度

相对于GPS速度

的速度差值δv^k，将误差

与δv^k的和

作为系统最终的速度输出。在模型的训练阶段，使用离线学习与在线学习相结合的方式，即在有GPS信号时预先进行足够的离线训练，将得到的各节点权值储存作为基础权值，而在每次实际的车辆运行中当有GPS信号时以基础权值作为初始权值进行在线训练，这样可以缩短训练时间的同时可以使输入输出关系更符合当时的条件减小误差，也可以解决车辆运行初期就搜索不到GPS信号的情况。The present invention designs a three-layer feedforward neural network including a two-node input layer, a five-node hidden layer, and a single-node output layer, and uses a sigmoid function as an activation function. As shown in Figure 3(a), in the training phase, the input layer is the wheel rotation angle and angular velocity θ,

The output layer is the difference between the GPS velocity measurement and the velocity in equation (11)

Parameter training of the neural network. In the prediction stage, as shown in Figure 3(b), only the input θ,

The prediction system will output the odometer speed

Relative to GPS speed

The speed difference δv ^k of , the error

Sum with δv ^k

As the final speed output of the system. In the training stage of the model, a combination of offline learning and online learning is used, that is, sufficient offline training is performed in advance when there is a GPS signal, and the obtained weights of each node are stored as the basic weights, and each time the actual vehicle When there is a GPS signal during operation, the basic weight is used as the initial weight for online training, which can shorten the training time and make the input-output relationship more in line with the conditions at that time to reduce errors, and can also solve the problem that the vehicle cannot be searched in the early stage of operation. GPS signal condition.

When the vehicle loses the GPS signal, the system enters the prediction mode to predict the speed error δv ^k at the current moment, then the final system output speed V ^k and mileage P ^k are:

dt is the sampling time interval.

中的元素，在每一次EKF更新后通过公式

计算

进行更新。其中，

在步骤3中得到θ根据公式(1)计算，而

由IMU的输出用传统的捷联惯导姿态更新方程进行计算，即：During the motion of the vehicle, the parameters R ₁₃ , R ₂₃ , and R ₃₃ in Equation 8 keep changing with time, and it is necessary to continuously update R ₁₃ , R ₂₃ , and R ₃₃ . As shown in Figure 4, R ₁₃ , R ₂₃ , and R ₃₃ are used as matrices

elements in , passed the formula after each EKF update

calculate

to update. in,

Obtained in step 3 θ is calculated according to formula (1), while

Calculated from the output of the IMU with the traditional SINS attitude update equation, namely:

为s系到n系的旋转矩阵，

为

的导数，

为陀螺仪输出对应的斜对称矩阵，

为n系相对惯性系i系的旋转角速度在s系中的表达对应的斜对称矩阵。

is the rotation matrix from the s system to the n system,

for

the derivative of ,

is the oblique symmetric matrix corresponding to the gyroscope output,

is the oblique symmetric matrix corresponding to the expression of the rotational angular velocity of the n system relative to the inertial system i system in the s system.

Although illustrative specific embodiments of the present invention have been described above to facilitate understanding of the present invention by those skilled in the art, it should be clear that the present invention is not limited in scope to the specific embodiments, to those skilled in the art, As long as various changes are within the spirit and scope of the present invention as defined and determined by the appended claims, these changes are obvious, and all inventions and creations utilizing the inventive concept are included in the protection list.

Claims (9)

1. a low-cost odometer design method based on MEMS IMU, is characterized in that, comprises the steps: Step 1: Install the IMU on the side of the wheel, make the IMU rotate with the wheel, and measure the non-gravitational acceleration and angular velocity of the wheel relative to the inertial frame; Step 2: For the IMU installed on the wheel, while considering the motion constraints of the vehicle, establish the output model of the IMU; Step 3: Take the wheel rotation angle, angular velocity, and angular acceleration as state quantities, establish a system model of Kalman filtering based on the constraint relationship between the states, and establish an observation model based on the output model of the IMU; Step 4: Speed, mileage calculation and parameter update based on artificial neural network. 2. a kind of low-cost odometer design method based on MEMS IMU according to claim 1, is characterized in that, described step 1 specifically comprises: A MEMS-level IMU sensor including a three-axis accelerometer and a three-axis gyroscope is installed on the side of the rear wheel of the vehicle or wheeled robot, and the sensor coordinate system sx ^s y ^s z ^s is established with the IMU as the origin, and the y ^s axis is negative. The direction points to the wheel center, the x ^s axis is perpendicular to the tire side and outward, and the y ^s axis and the z ^s axis are located in the wheel rotation plane. The carrier coordinate system bx ^b y ^b z ^b is established, so that the x ^b axis coincides with the x ^s axis, the y ^b axis points to the front of the vehicle, the z ^b axis is perpendicular to the x ^b axis and the y ^b axis is upward; when the wheel starts to rotate, s The system rotates around the ^{x s} axis, and the gravitational acceleration will be periodically projected to the y ^s axis and the z ^s axis in a sinusoidal form, and its frequency is equal to the rotational angular velocity of the wheel. is the product of the square of the rotational angular velocity of the wheel and the distance from the IMU to the wheel center. The rotational velocity of the wheel will also be projected on the x-axis of the gyroscope. 3. a kind of low-cost odometer design method based on MEMS IMU according to claim 2, is characterized in that, also comprises: by adjusting the distance from IMU to wheel center, avoid making actual acceleration exceed accelerometer output due to centripetal acceleration The sensor is installed on the wheel and rotates continuously, and the sensor integrates a wireless communication device, including Bluetooth and WIFI, to exchange information with the control system on the vehicle. 4. a kind of low-cost odometer design method based on MEMS IMU according to claim 1, is characterized in that, described step 2 specifically comprises: The output of the accelerometer is represented by the sum of the acceleration of gravity, the centripetal acceleration generated by the rotation of the wheel and the acceleration of the vehicle relative to the ground, and the local horizontal coordinate system is used as the navigation coordinate system n system; note:

Then the projection of the gravitational acceleration in the sensor coordinate system is:

where g is the local gravitational acceleration, θ is the counterclockwise rotation angle of the IMU relative to the starting position,

represents the rotation matrix from the b system to the s system,

is the rotation matrix from the n system to the b system, and R _ij is

elements in; i=1, 2, 3; j=1, 2, 3; The centripetal acceleration generated by the rotation of the wheel is projected in the negative direction of the y ^s axis and expressed as:

r is the distance from the IMU to the wheel center,

is the first derivative of θ, that is, the angular velocity of wheel rotation; The acceleration of the vehicle in the b system is expressed as

Considering the motion constraints of the vehicle, the lateral and celestial accelerations are zero, namely:

The vehicle acceleration in the direction of the longitudinal axis is expressed as the wheel rotational angular acceleration multiplied by the wheel radius:

R is the wheel radius,

is the second derivative of θ, that is, the angular acceleration of wheel rotation; Therefore, the acceleration of the vehicle relative to the ground in the b system is

Projection to the sensor coordinate system is:

So the output of the accelerometer in the IMU

The relationship with θ is:

Neglecting the Earth's rotation, the output of the gyroscope's x ^s axis is approximately equal to the angular velocity of the wheel's rotation relative to the carrier:

5. a kind of low-cost odometer design method based on MEMS IMU according to claim 1, is characterized in that, described step 3 specifically comprises: Use the extended Kalman filter EKF to fuse the data of the accelerometer and the gyroscope to make up for their respective shortcomings to obtain a more accurate wheel rotation angle and angular velocity. Specifically, it includes: considering that the accelerometer output model in step 2 contains θ,

use

As a state quantity, according to θ,

The relationship between establishes the state update equation; only the output of the y ^s axis of the accelerometer

z ^s axis output

The output of the gyroscope x ^s axis

related to X, so use

As the observed amount, the equation (9) (10) is linearized according to this to establish the measurement update equation:

State update equation:

where [w _x w _y w _z ] ^T is the sensor noise of the corresponding axis, and [v _x v _y v _z ] ^T is the measurement noise. 6. A kind of low-cost odometer design method based on MEMS IMU according to claim 5, is characterized in that, described step 4 specifically comprises: in step 3 obtain θ and

After that, the mileage and speed need to be calculated; when the vehicle is running normally, there is no relative slippage between the wheels and the ground, and the distance the wheel rolls on the ground is equal to the driving distance of the vehicle, P ^k =P ⁰ +θ ^k R (10)

P ⁰ is the initial mileage, P ^k is the mileage at time K, θ ^k ,

is the angle and angular velocity of the wheel at time K,

is the speed of the vehicle at time K, and R is the wheel radius. 7. a kind of low-cost odometer design method based on MEMS IMU according to claim 6, it is characterized in that, introduce BP neural network, use the data of GPS to carry out parameter training to BP neural network when there is GPS signal, in no BP neural network is used to predict mileage and vehicle speed when GPS signals are used; the neural network includes a two-node input layer, a five-node hidden layer, and a three-layer feedforward neural network with a single-node output layer, using a sigmoid function as Activation function; in the training phase, the input layer is the wheel rotation angle and angular velocity θ,

The output layer is the difference between the GPS velocity measurement and the velocity in equation (11)

Parameter training is performed on the neural network; in the prediction stage, only the input θ,

The prediction system will output the odometer speed

Relative to GPS speed

The speed difference δv ^k of , the error

Sum with δv ^k

As the final speed output of the system. 8. A low-cost odometer design method based on MEMS IMU according to claim 7, characterized in that, when the vehicle loses the GPS signal, the system enters the prediction mode to predict the speed error δv ^k at the current moment, then the final system output The speed V ^k and the mileage P ^k are:

dt is the sampling time interval. 9. a kind of low-cost odometer design method based on MEMS IMU according to claim 5, is characterized in that, In step 3 get θ and

After that, you need to update the matrix

to carry out the next cycle; in the process of vehicle motion, the parameters R ₁₃ , R ₂₃ , and R ₃₃ in formula (8) change with time, and R ₁₃ , R ₂₃ , and R ₃₃ need to be updated continuously; R ₁₃ , R 33 ₂₃ , R ₃₃ as a matrix

elements in , passed the formula after each EKF update

calculate

to update; where,

Obtained in step 3 θ is calculated according to formula (1), while

The SINS attitude update equation is used to calculate from the output of the IMU, namely:

is the rotation matrix from the s system to the n system,

for

the derivative of ,

is the oblique symmetric matrix corresponding to the gyroscope output,

is the oblique symmetric matrix corresponding to the expression of the rotational angular velocity of the n system relative to the inertial system i system in the s system.

Images