CN106767795A - A kind of mobile robot displacement computational algorithm based on inertial navigation - Google Patents

Abstract

The invention relates to a mobile robot displacement calculation algorithm based on inertial navigation. First, the three-axis acceleration is read through the three-axis accelerometer of the inertial measurement unit on the mobile robot; Jump data generated by hardware instability; then use the peak-valley algorithm to obtain the peak-valley value of the three-axis acceleration; then use the least square method to obtain the best linear fitting function for the peak-valley of the triaxial acceleration, And the displacement is calculated by integrating the fitting function; then the displacement calculation algorithm is corrected, and the modified displacement calculation algorithm is used to calculate the displacement filtered by the anti-pulse interference average filter method; finally, the two displacements are fused to eliminate the error. The invention greatly eliminates the error of calculating the displacement of the inertial navigation accelerometer, realizes the accurate displacement calculation only by the accelerometer, and improves the indoor positioning accuracy of the mobile robot based on the inertial navigation.

Description

A Displacement Calculation Algorithm of Mobile Robot Based on Inertial Navigation

technical field

The invention relates to the field of robot positioning, in particular to a displacement calculation algorithm of a mobile robot based on inertial navigation.

Background technique

At present, the GPS-based outdoor positioning system for mobile robots is quite mature, but in large indoor buildings due to poor reception of satellite signals, indoor positioning systems have become a technology that application manufacturers are competing to invest in. At present, indoor positioning technologies are mainly based on Wi-Fi, Bluetooth and inertial navigation, but Wi-Fi and Bluetooth need to deploy a large number of network systems, the construction cost is high, and the accuracy is also affected by indoor obstacles. Indoor positioning of mobile robots for navigation not only has lower construction costs, but also greatly improves accuracy.

Inertial navigation refers to the use of a variety of sensors to form a multi-axis inertial measurement unit (IMU, InertialMeasurement Unit). The sensors mainly include a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer. Inertial navigation used for indoor positioning of mobile robots is generally fused with other positioning sensors. The cumulative error of indoor positioning by inertial navigation alone is large, especially the integration of accelerometers to calculate displacement has not yet been realized in mobile robot positioning technology. The main reason for the error is that the accelerometer is relatively sensitive, and the accelerometer has a relatively serious drift phenomenon. The generated acceleration will fluctuate with slight jitter, and the displacement obtained by the second integration will continue to increase due to the time accumulation error.

At present, the inertial navigation indoor positioning has been successfully applied to the indoor positioning of walking control. The relative displacement is calculated by using the step length control. The acceleration generated in this way fluctuates regularly due to the lifting and lowering of the legs during walking, while the mobile robot maintains horizontal motion based on the plane. Changes irregularly, so the displacement calculation of the mobile robot platform has not seen good results. Moreover, at present, the processing of the three-axis acceleration data of the inertial navigation of mobile robots mostly uses basic digital filtering algorithms, Kalman filtering algorithms, and time-domain filtering algorithms. It cannot solve the problem that the displacement accumulation error keeps increasing after the second integration.

Based on the above-mentioned problem that the accumulative error of acceleration calculation and displacement of mobile robots based on inertial navigation is constantly increasing, the present invention proposes an algorithm that can not only better deal with the problem of accelerometer data instability, but also better calculate displacement through acceleration.

Contents of the invention

In view of this, the purpose of the present invention is to propose a mobile robot displacement calculation algorithm based on inertial navigation, which can better deal with the problem of unstable accelerometer data and calculate displacement through acceleration.

The present invention adopts the following scheme to realize: a mobile robot displacement calculation algorithm based on inertial navigation, specifically comprising the following steps:

Step S1: Read the three-axis acceleration a(a _x , a _y , a _z ) of the mobile robot;

Step S2: Filter the three-axis acceleration by using the anti-pulse interference average filtering method to filter out jump data due to hardware instability;

Step S3: Obtain the peak-valley value of the three-axis acceleration by using the peak-valley algorithm for the filtered data in step S2;

Step S4: using the least squares method to obtain the best linear fitting function for the peak-valley value of the triaxial acceleration obtained in step S3, and calculating the displacement by integrating the fitting function;

Step S5: Correct the displacement calculation algorithm, and use the revised displacement calculation algorithm to calculate the displacement filtered by the anti-pulse interference average filtering method;

Step S6: Fusion the two displacements to eliminate errors and calculate the final displacement.

Further, the step S1 specifically includes: reading the three-axis acceleration a(a _x , a _y , a _z ) of the three-axis accelerometer of the inertial measurement unit on the mobile robot through the main control board of the Linux system by means of IIC communication.

Further, the step S2 specifically includes: sorting the n triaxial acceleration data collected continuously, storing and updating the latest n data at the same time, removing the largest and smallest m data from the sorted data, Find the average of the remaining data.

Further, the step S3 specifically includes the following steps;

Step S31: Detect the peak-trough, read the latest a _xn data, and compare it with the previous data a _x(n-1) , if a _xn > a _x(n-1) , record d=1; if a _xn <a _x(n-1) , then record d=-1; if a _xn ＝a _x(n-1), record d=0;

Step S32: If it is detected that d=1 last time and d=-1 at this time, it means that the last time is the peak value; if it is detected that d=-1 last time and d=1 at this time, it means that the last time is the valley value;

Step S33: Use the latest peak-to-valley value as the absolute value difference. If the absolute value difference is greater than the set threshold, it indicates that the acceleration sensitivity value is not caused by the robot shaking, and the calculation result is included in the next calculation;

Step S34: Using the methods of steps S31 to S33, respectively calculate the peak-to-valley values of a _y and a _z , and compare the absolute value difference with the set threshold.

Further, the step S4 specifically includes the following steps:

Step S41: Take the latest stored peak-trough or peak-trough acceleration value as a set of data values, recorded as A _xi ={a _x1 ,a _x1 ,...,a _xm },(i=1,2 ,...,m);

Step S42: Calculate the variance S _xi of each group of acceleration a _x , denoted as S _xi =(S _x1 ,S _x2 ,...,S _xm ), and use the following formula to calculate:

in, is the average acceleration;

Step S43: According to the formula of the least squares method, use the minimum principle of S _xi to obtain the best linear fitting function, and perform quadratic integration on the linear fitting function to obtain the displacement of this segment, and calculate the total displacement L′ _{x by} accumulating the previous displacement ;

Step S44: Calculate the total displacements L' _y and L' _z of the Y-axis and the Z-axis by using the method from Step S41 to Step S43.

Further, the step S5 is specifically: setting a threshold value for the acceleration change filtered by the anti-pulse interference average filtering method to solve the acceleration drift phenomenon, and when the change exceeds this threshold value, the velocity and displacement are calculated according to the following correction formula:

V _n =2*V _n-1 -V _n-2 +Δa _n,n-1 *Δt;

ΔV=V _n -V _n-1 =V _n-1 -V _n-2 +Δa _n,n-1 *Δt;

S _n =2*S _n-1 -S _n-2 +a _n-1 *t ² ;

Among them, V _n , S _n , and t respectively represent the velocity at the current moment, the total displacement at the current moment, and the integration time;

The total displacement L″ _x , L″ _y , L″ _z of the X, Y, and Z axes can be calculated from the above formula.

Further, the step S6 is specifically: use the acceleration collected each time as a fitting function to integrate the displacement calculation algorithm and the corrected displacement calculation algorithm to obtain the displacement L', L", respectively, and calculate the average of the two numbers to obtain the final displacement L.

Compared with the prior art, the present invention has the following beneficial effects: the present invention can effectively solve the problem that the accumulated displacement error becomes larger when the mobile robot adopts the inertial navigation indoor positioning, realizes the accurate displacement calculation only by the accelerometer, and improves the Accuracy of Indoor Localization for Mobile Robots Based on Inertial Navigation.

Description of drawings

Fig. 1 is a schematic flow chart of the algorithm of the present invention.

Fig. 2 is a hardware module diagram in the embodiment of the present invention.

Fig. 3 is an acceleration peak-valley diagram obtained in an embodiment of the present invention.

Fig. 4 is a diagram of the best fitting function obtained by the acceleration least squares method in the embodiment of the present invention.

Fig. 5 is a diagram of the final displacement trajectory after fusion in the embodiment of the present invention.

detailed description

The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

FIG. 1 is a schematic flowchart of an algorithm of an embodiment of the present invention, and FIG. 2 is a diagram of a hardware module in an embodiment of the present invention. As shown in Figure 1 and Figure 2, a mobile robot displacement calculation algorithm based on inertial navigation provided in this embodiment specifically includes the following steps:

S1: Read the three-axis acceleration a(a _x , a _y , a _z ) of the three-axis accelerometer of the inertial measurement unit on the mobile robot shown in Figure 2 through the main control board of the Linux system using IIC communication.

S2: The three-axis acceleration is filtered by the anti-pulse interference average filter method. Sort the n three-axis acceleration data a(a _x , a _y , a _z ) collected continuously, store and update the latest n data at the same time, and remove the largest and smallest m data from the sorted data, The remaining data were averaged to obtain an average value. In this embodiment, n=10, m=2.

S3: FIG. 3 is a peak-valley diagram of the three-axis acceleration obtained by using the peak-valley algorithm. In this embodiment, the specific peak-trough algorithm is as follows: first detect the peak-trough, read the latest a _xn data, compare with the last data a _x(n-1) , if a _xn > a _{x( n-1)} means d=1, a _xn < a _x(n-1) means d=-1, a _xn = a _x(n-1) means d=0. If it is detected that d=1 last time and d=-1 at this time, it means that the last a _{x(n-1) is} the peak value; if it is detected that d=-1 last time and d=1 at this time, then It shows that the last a _{x(n-1) is} the valley value. Use the latest peak-to-valley value as the absolute value difference. If it is greater than the set threshold, it indicates that the acceleration sensitivity value is not caused by robot shaking, and it will be included in the next calculation. Use this algorithm to calculate the peak-valley values of a _y and a _z similarly, and compare the absolute value difference with the threshold. In this embodiment, k _x =10 cm/s, k _y =10 cm/s, k _z =0 cm/s.

S4: Figure 4 is the best linear fitting function obtained by using the least square method for the acceleration value between a certain peak and trough. In this embodiment, the latest peak-trough or peak-trough acceleration values stored in S3 are used as a set of data values, which are recorded as A _xi ={a _x1 ,a _x1 ,...,a _xm },(i =1,2,...,m); calculate the variance S _xi of each group of acceleration a _x , denoted as S _xi =(S _x1 ,S _x2 ,...,S _xm ), where is the average acceleration. According to the formula of the least square method, use the minimum principle of S _xi to find the best linear fitting function, and do the quadratic integration of the linear fitting function to find the displacement of this section, and calculate the total displacement L′ _x by accumulating with the previous displacement. Use this algorithm to calculate the median displacements L' _y and L' _z of the Y-axis and Z-axis in the same way.

S5: Correct the displacement calculation algorithm, and use the revised displacement calculation algorithm to calculate the displacement filtered by the anti-pulse interference average filtering method. Set a threshold value for the acceleration change filtered by the anti-pulse interference average filtering method to solve the acceleration drift phenomenon. When the change exceeds this threshold value, calculate the velocity and displacement according to the following correction formula:

V _n =2*V _n-1 -V _n-2 +Δa _n,n-1 *Δt;

ΔV=V _n -V _n-1 =V _n-1 -V _n-2 +Δa _n,n-1 *Δt;

S _n =2*S _n-1 -S _n-2 +a _n-1 *t ² ;

The total displacement L″ _x , L″ _y , L″ _z of the X, Y, and Z axes can be calculated from the above formula.

Among them, V _n , S _n , and t respectively represent the velocity at the current moment, the total displacement at the current moment, and the integration time;

S6: Fig. 5 is a diagram of the final displacement trajectory after the fusion of the two displacements. In this embodiment, the S04 and S05 algorithms are used to calculate the displacements L′ and L″ respectively based on the acceleration collected each time, and the final displacement L is obtained by averaging the two numbers.

The above descriptions are only preferred embodiments of the present invention, and all equivalent changes and modifications made according to the scope of the patent application of the present invention shall fall within the scope of the present invention.

Claims (7)

1. A mobile robot displacement calculation algorithm based on inertial navigation is characterized in that: the method comprises the following steps:

step S1: reading three-axis acceleration a (a) of a mobile robot_x,a_y,a_z)；

Step S2: filtering the triaxial acceleration by adopting an anti-pulse interference average filtering method to filter jump data generated by hardware instability;

step S3: obtaining a peak-valley value of the triaxial acceleration by adopting a peak-valley algorithm on the data filtered in the step S2;

step S4: obtaining the best linear fitting function by adopting a least square method for the peak-trough value of the triaxial acceleration obtained in the step S3, and calculating the displacement by integrating the fitting function;

step S5: correcting the displacement calculation algorithm, and calculating the displacement after the pulse interference prevention average filtering method is used for filtering by using the corrected displacement calculation algorithm;

step S6: and fusing the two displacements to eliminate errors and calculate the final displacement.

2. The inertial navigation-based mobile robot displacement calculation algorithm according to claim 1, wherein: the step S1 specifically includes: reading triaxial acceleration a (a) of a triaxial accelerometer of an inertial measurement unit on the mobile robot by adopting an IIC communication mode through a Linux system main control board_x,a_y,a_z)。

3. The inertial navigation-based mobile robot displacement calculation algorithm according to claim 1, wherein: the step S2 specifically includes: respectively sequencing n continuously acquired triaxial acceleration data, simultaneously storing and updating the latest n data, removing the largest and smallest m data from the sequenced data, and calculating the average value of the rest data.

4. The inertial navigation-based mobile robot displacement calculation algorithm according to claim 1, wherein: the step S3 specifically includes the following steps;

step S31: detecting the peak-trough, reading the latest a_xnData and with the last data a_x(n-1)In comparison, if a_xn＞a_x(n-1)If d is 1; if a_xn＜a_x(n-1)If d is-1; if a_xn＝a_x(n-1)D is taken as 0;

step S32: if d is detected to be 1 and d is-1, the last time is the wave peak value; if d is detected to be-1 last time and d is 1 at the moment, the last time is the valley value;

step S33: taking the latest peak-trough value as an absolute value difference, if the absolute value difference is larger than a set threshold, indicating that the acceleration sensitive value is not caused by the shaking of the robot, and bringing the calculation result into the next calculation;

step S34: calculating a respectively by the method of steps S31 to S33_yAnd a_zAnd comparing the absolute value difference with a set threshold value.

5. The inertial navigation-based mobile robot displacement calculation algorithm according to claim 1, wherein: the step S4 specifically includes the following steps:

step S41: taking the latest stored acceleration value between wave crest-wave trough or wave trough-wave crest as a group of data values, and recording the data values as A_xi＝{a_x1,a_x1,...,a_xm},(i＝1,2,...,m)；

Step S42: calculating each set of accelerations a_xVariance S of_xiIs recorded as S_xi＝(S_x1,S_x2,...,S_xm) Calculated using the following formula:

$S_{xi}^2=\&lsqb;{(a_{x1}-\stackrel{\&OverBar;}{a})}^2+{(a_{x2}-\stackrel{\&OverBar;}{a})}^2+...+{(a_{xm}-\stackrel{\&OverBar;}{a})}^2\&rsqb;,$

wherein,is the average acceleration;

step S43: according to the formula of least square method, using S_xiCalculating the best linear fitting function by the least principle, calculating the displacement of the segment by quadratic integration of the linear fitting function, and accumulating the displacement with the previous displacement to calculate the total displacement L'_x；

Step S44: calculating total displacement L 'of the Y axis and the Z axis by adopting the method from the step S41 to the step S43'_y、L'_z。

6. The inertial navigation-based mobile robot displacement calculation algorithm according to claim 1, wherein: the step S5 specifically includes: setting a threshold value for the acceleration change filtered by the pulse interference prevention average filtering method to solve the acceleration drift phenomenon, and calculating the speed and the displacement according to the following correction formula when the change exceeds the threshold value:

V_n＝2*V_n-1-V_n-2+Δa_n,n-1*Δt；

ΔV＝V_n-V_n-1＝V_n-1-V_n-2+Δa_n,n-1*Δt；

S_n＝2*S_n-1-S_n-2+a_n-1*t²；

$\mathrm{\&Delta;}S=\frac{1}{2}\mathrm{\&Delta;}V*t;$

wherein, V_n、S_nT represents the current moment speed, the current moment total displacement and the integral time respectively;

from the above formula, the total displacement L of the X, Y, Z axis can be calculated "_x,L”_y,L”_z。

7. The inertial navigation-based mobile robot displacement calculation algorithm according to claim 1, wherein: the step S6 specifically includes: and respectively solving the displacements L 'and L' by using the acceleration acquired each time as a fitting function integral calculation displacement algorithm and a corrected displacement calculation algorithm, and averaging the two numbers to obtain the final displacement L.