CN105737853A - Method for calibrating drifting of robot inertial navigation system - Google Patents

Abstract

The invention provides a method for calibrating drifting of a robot inertial navigation system.The method is used for calibrating the robot speed measured by the inertial navigation system.The method comprises the steps that firstly, two adjacent movement state change time points of a robot are acquired, then, the speed values of the robot at the two state change time points are acquired so that the change slope of robot speed drifting can be calculated, and finally, the speed measured by the inertial navigation system is corrected according to the change slope of the robot speed drifting so that the corrected speed can be obtained.By means of the method, the speed measured by the inertial navigation system can be corrected without frequent halt, and the method has the advantage that the correction precision is high.

Description

Drifting calibration method of robot inertial navigation system

Technical Field

The invention belongs to the field of robots, and particularly relates to a drift calibration method of a robot inertial navigation system, which is used for calibrating the robot speed measured by the inertial navigation system.

Background

As one of the most successful robot technologies for civilization, floor sweeping robots are moving into more and more families, and intelligent cleaning is making people's home life more and more attentive. Pursuing higher levels of intelligence is a major line in the development of robots. Through the sensor of various new functions, some common use problems of robot of sweeping the floor such as anticollision, dropproof have been solved successfully, use the beacon of installing additional on filling electric pile simultaneously and realized that the robot is back to filling navigation, greatly made things convenient for people's use when low-power.

The latest development trend of the sweeping robot at present is to realize 100% indoor environment cleaning without dead angles by automatically planning a sweeping route, which is greatly superior to the conventional random collision type path-finding mode. The technical core of automatically planning the cleaning route is that the robot can autonomously navigate and acquire the position and the motion track of the robot in a room in real time.

Inertial navigation technology (hereinafter abbreviated as inertial navigation, INS) is a relatively common robot navigation method. The navigation system does not depend on external information and is an autonomous navigation system which is not easy to be interfered. The inertial navigation measures the acceleration of a carrier in an inertial reference system (usually, the reference system based on the robot itself), converts the acceleration into a navigation coordinate system (generally, the angular velocity is default to the indoor ground for a sweeping robot) by using the angular velocity measured by a gyroscope, and then obtains attitude information such as the instantaneous velocity, the instantaneous position, the yaw angle and the like of the carrier through integral operation. The inertial navigation has the advantages of interference resistance, only initial coordinates (the sweeping robot can use a charging pile as an initial point of each movement) need to be given, and the position, the direction and the speed of the robot at any moment can be determined without external reference in the operation process. The method is suitable for accurate positioning and orientation in complex geographic environment and places with serious external interference. At present, the national Qingdao Haitong robot and the Xingsong AGV robot carrying system are used for researching and developing a robot inertial navigation technology. The key of the inertial navigation system applied to robot navigation is to solve the problem of drift of the inertial navigation system along with time, in particular to a commonly used MEMS inertial navigation system, although MEMS gyroscopes and accelerometers have the advantages of small size, easiness in installation, low power consumption and the like, the drift of the gyroscopes and the accelerometers can cause the walking path of the robot calculated by an inertial navigation algorithm to deviate from a real route seriously.

The core devices of the inertial navigation are a gyroscope and an accelerometer, and the output of the gyroscope and the accelerometer is used for coordinate transformation and integration to solve the walking track of the robot. The outputs of both devices drift and the integration operation accumulates the interference caused by the drift, further exacerbating the interference problem. The invention aims to solve the problem that the method can inhibit the drift of the inertial navigation system.

The drift correction method of the inertial navigation system which is commonly used at present is zero-speed correction, namely, the two adjacent parking moments of the robot are searched, the speed during parking is 0, if the detected speed during parking is not 0, the speed is caused by the drift of the sensor, and the drift of the sensor along with the time can be calculated through the time interval of the two times and the sensor calculation value (which is not 0 due to the drift of the sensor) of the speed during parking. However, zero-speed correction is not suitable for the sweeping robot, and the sweeping robot usually needs to work for a long time without stop, so that the moment of immobility is difficult to occur.

Although the time close to zero speed such as wall-collision turning back and pivot steering occurs, at this time, the output value of the accelerometer greatly vibrates due to the vibration of the motor because the motor power system of the robot does not stop working, and the zero speed time is difficult to be judged through the acceleration.

If the robot needs to be stopped frequently to perform zero-speed correction, the sweeping time of the robot can be greatly prolonged, the efficiency is reduced, and the motor load is seriously fluctuated and the service life is damaged due to the frequent stopping and zero-speed correction.

Disclosure of Invention

Technical problem to be solved

In view of the above problems, an object of the present invention is to provide a robot speed correction method that can correct a robot speed without frequent stops and has an advantage of high correction accuracy.

(II) technical scheme

The invention provides a drift calibration method of a robot inertial navigation system, which is used for calibrating the robot speed measured by the inertial navigation system and comprises the following steps:

s1, acquiring the adjacent robot motion state change time t_n-1And t_nWherein the motion state of the robot comprises a linear motion state, an original turning state and a static state;

s2, acquiring two state change moments t of the inertial navigation system_n-1And t_nMeasured velocity v_n-1And v_nAnd calculating a change slope v_d：

$v_d=\frac{v_n-v_{n-1}}{t_n-t_{n-1}};$

S3, according to the change slope v_dTo t_n-1～t_nCalibrating the speed measured by the inertial navigation system in the time period to obtain the calibrated speed v_i：

v_i＝v_t-v_d(t-t_n-1)，

Wherein v is_tT ∈ [ t ] is the measured velocity of the inertial navigation system at time t before calibration_n,t_n-1]。

(III) advantageous effects

The invention extracts the state change time of the robot, namely the time when the linear velocity changes as the correction time by analyzing the angular velocity and the acceleration when the robot runs, and the speed is 0 theoretically at the time, so the measured speed is the speed drift at the time, and the speed drift is corrected. The method can correct the speed measured by the inertial navigation system without frequent shutdown, and has the advantage of high correction precision.

Drawings

Fig. 1 is a flowchart of a drift correction method of an inertial navigation system of a sweeping robot according to an embodiment of the present invention.

FIG. 2 is a waveform diagram of an angular velocity signal, an acceleration signal and a composite signal according to an embodiment of the present invention.

FIG. 3 is a graph showing the effects of the velocity on the X-axis, Y-axis, and Z-axis before and after correction in an embodiment of the present invention.

Fig. 4 is a trajectory diagram of the sweeping robot inertial navigation system after speed correction in the embodiment of the invention.

Detailed Description

The invention provides a drift calibration method of a robot inertial navigation system, which is used for calibrating the speed of a robot measured by the inertial navigation system. The method can correct the speed measured by the inertial navigation system without frequent shutdown, and has the advantage of high correction precision.

According to one embodiment of the invention, the drift calibration method of the robot inertial navigation system comprises the following steps:

s1, acquiring the adjacent robot motion state change time t_n-1And t_nThe robot motion state comprises a linear motion state, an original turning state and a static state, so the motion state change time refers to the time when the robot changes among the linear motion state, the original turning state and the static state;

s2, acquiring two state change moments t of the inertial navigation system_n-1And t_nMeasured speedDegree v_n-1And v_nAnd calculating a change slope v_d：

$v_d=\frac{v_n-v_{n-1}}{t_n-t_{n-1}};$

S3, according to the change slope v_dTo t_n-1～t_nCalibrating the speed measured by the inertial navigation system in the time period to obtain the calibrated speed v_i：

v_i＝v_t-v_d(t-t_n-1)，

Wherein v is_tT ∈ [ t ] is the measured velocity of the inertial navigation system at time t before calibration_n,t_n-1]。

According to an embodiment of the present invention, step S1 includes:

s11, acquiring an angular velocity signal and an acceleration signal of the robot and preprocessing the signals;

s12, synthesizing the angular velocity signal and the acceleration signal after the preprocessing to obtain a synthesized signal, where a time corresponding to a signal upper edge or a signal lower edge of the synthesized signal is a state change time, and specifically, performing an or operation on the angular velocity signal and the acceleration signal to obtain the synthesized signal;

s13, two consecutive state change timings are acquired.

According to an embodiment of the present invention, step S11 includes:

comparing the angular velocity signal with a first threshold, and when the angular velocity signal is greater than the first threshold, making the angular velocity signal at a high level, otherwise, making the angular velocity signal at a low level, preferably, the first threshold is 50 degrees/s;

comparing the acceleration signal with a second threshold, and when the acceleration signal is greater than the second threshold, making the acceleration signal at a high level, otherwise, making the acceleration signal at a low level, preferably, the second threshold is 0.001m/s²。

According to an embodiment of the invention, the corrected velocity v_iIncluding the velocity components of the X, Y and Z axes in the navigational coordinate system, the method further comprises: s4, correcting the speed v_iPerforming time dimension integration on each velocity component to obtain position coordinates P of the robot at each moment_iThe position coordinates P of each time are calculated_iAnd connecting to obtain the motion trail of the robot.

According to an embodiment of the present invention, the robot speed correction method further includes:

s0, acquiring an acceleration signal [ a ] of the robot under the self coordinate system_x,ba_y,ba_z,b]And performing coordinate system conversion by using Euler angle method to obtain acceleration signal [ a ] in navigation coordinate system_x,ka_y,ka_z,k]：

$\left[\begin{array}{c}{a}_{x,k}\\ a_{y,k}\\ a_{z,k}\end{array}\right]=\left[\begin{array}{ccc}\cos R\cos H-\sin P\sin R\sin H& \cos R\sin H+\sin P\sin R\cos H& \cos P\sin R\\ -\cos P\sin H& \cos P\cos H& \sin P\\ \sin R\cos H+\sin P\cos R\sin H& \sin R\sin H-\sin P\cos R\cos H& \cos P\cos R\end{array}\right]\left[\begin{array}{c}{a}_{x,b}\\ a_{y,b}\\ a_{z,b}\end{array}\right]$

H, P and R respectively represent a course angle, a pitch angle and a roll angle, and k represents the kth moment of signal sampling of the inertial navigation system;

for the acceleration signal [ a ] under the navigation coordinate system_x,ka_y,ka_z,k]Performing time dimension integration to obtain the velocity v before correction_t：

$v_t=\left[\begin{array}{c}{v}_{x,t}\\ v_{y,t}\\ v_{z,t}\end{array}\right]=\left[\begin{array}{c}\underset{k=1}{\overset{t}{\Σ}}{a}_{x,k}\·\mathrm{\Δ}t\\ \underset{k=1}{\overset{t}{\Σ}}{a}_{y,k}\·\mathrm{\Δ}t\\ \underset{k=1}{\overset{t}{\Σ}}{a}_{z,k}\·\mathrm{\Δ}t\end{array}\right],$

Where t represents the current moment of signal sampling by the inertial navigation system and Δ t represents the sampling time interval. The drift interference in the acceleration is accumulated along with the integral, so that the calculated result of the speed deviates from the actual value and is linearly and rapidly increased.

According to one embodiment of the invention, the inertial navigation system comprises a gyroscope and an accelerometer, which are all MEMS devices, and the gyroscope is used for acquiring an angular velocity signal of the robot, and the accelerometer is used for acquiring an acceleration signal of the robot.

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to specific embodiments and the accompanying drawings.

Fig. 1 is a flowchart of a drift correction method of an inertial navigation system of a sweeping robot according to an embodiment of the present invention, and as shown in fig. 1, the method includes:

s0, acquiring an original acceleration signal [ a ] of the sweeping robot in a self coordinate system through the MEMS accelerometer_x,ba_y,ba_z,b]And performing coordinate system conversion by using Euler angle method to obtain acceleration signal [ a ] in navigation coordinate system_x,ka_y,ka_z,k]：

$\left[\begin{array}{c}{a}_{x,k}\\ a_{y,k}\\ a_{z,k}\end{array}\right]=\left[\begin{array}{ccc}\cos R\cos H-\sin P\sin R\sin H& \cos R\sin H+\sin P\sin R\cos H& \cos P\sin R\\ -\cos P\sin H& \cos P\cos H& \sin P\\ \sin R\cos H+\sin P\cos R\sin H& \sin R\sin H-\sin P\cos R\cos H& \cos P\cos R\end{array}\right]\left[\begin{array}{c}{a}_{x,b}\\ a_{y,b}\\ a_{z,b}\end{array}\right],$

H, P and R respectively represent a course angle, a pitch angle and a roll angle, and k represents the kth moment of signal sampling of the inertial navigation system;

for the acceleration signal [ a ] under the navigation coordinate system_x,ka_y,ka_z,k]Performing time dimension integration to obtain the velocity v before correction_t：

$v_t=\left[\begin{array}{c}{v}_{x,t}\\ v_{y,t}\\ v_{z,t}\end{array}\right]=\left[\begin{array}{c}\underset{k=1}{\overset{t}{\Σ}}{a}_{x,k}\·\mathrm{\Δ}t\\ \underset{k=1}{\overset{t}{\Σ}}{a}_{y,k}\·\mathrm{\Δ}t\\ \underset{k=1}{\overset{t}{\Σ}}{a}_{z,k}\·\mathrm{\Δ}t\end{array}\right],$

Where t represents the current moment of signal sampling by the inertial navigation system and Δ t represents the sampling time interval.

S1, obtaining adjacentMoment t of change of robot motion state_n-1And t_nAnd the motion state of the robot comprises a linear motion state, an original turning state and a static state. The method specifically comprises the following steps:

and S11, acquiring an angular velocity signal and an acceleration signal of the sweeping robot. When the angular velocity signal is greater than 50 degrees/s, enabling the angular velocity signal to be at a high level, otherwise, enabling the angular velocity signal to be at a low level; when the acceleration signal is larger than 0.001m/s²When the acceleration signal is at high level, otherwise, the acceleration signal is at low level.

Fig. 2 is a waveform diagram of an angular velocity signal, an acceleration signal and a composite signal according to an embodiment of the present invention. The sweeping robot usually moves straight line and has a heading angular velocity of substantially 0, and stops at the time of turning and then rotates on site, and at this time, the traveling velocity of the robot is 0, the position coordinate remains unchanged, but the rotation angular velocity is very high, as shown in fig. 2(a) and 2 (b). In the embodiment, the upper limit of the angular speed when the robot travels straight (i.e. 50 degrees/s, which can reach 100 degrees/s when the robot turns in a common "bow" type route sweeping manner) is used as a threshold for judging whether the robot turns. Meanwhile, considering that the sweeping robot is in a stop state at the starting point and the end point, a motor of the power device is not started, no vibration interference is caused to the accelerometer, the output of the accelerometer is 0 at the moment, and a very small acceleration threshold (namely 0.001 m/s) is adopted in the embodiment²) And (5) judging the parking state, namely, the high level is the starting state, and the low level is the parking state.

And S12, performing OR operation on the angular velocity signal and the acceleration signal after the preprocessing to obtain a composite signal, wherein as shown in fig. 2(c), a high level of the composite signal indicates that the linear velocity of the sweeping robot is 0, and a low level indicates that the linear velocity of the sweeping robot is not 0. Therefore, the falling edge of the composite signal can represent two cases: the time from turning to straight line walking of the robot and the time from stopping to starting of the robot; the rising edge of the composite signal may represent two cases: the time from the straight line walking to the turning of the robot and the time from the starting to the stopping of the robot. And taking the time corresponding to the upper edge or the lower edge of the signal of the synthesized signal as the state change time, wherein the linear velocity of the robot is 0 theoretically, so the measured velocity at the time is the velocity drift.

S13, two continuous state change time t of the robot during turning are obtained_n-1And t_n。

S2, acquiring two state change moments t of the robot_n-1And t_nVelocity value v of_n-1And v_nCalculating the change slope v of the robot speed drift_d：

$v_d=\frac{v_n-v_{n-1}}{t_n-t_{n-1}};$

S3, changing slope v according to the speed drift of the robot_dTo t_n-1～t_nCorrecting the speed of the time segment to obtain a corrected speed v_i：

v_i＝v_t-v_d(t-t_n-1)，

Wherein v is_tFor the pre-correction speed t ∈ [ t ]_n,t_n-1]. Wherein the corrected velocity v_iIncluding the X, Y and Z axis velocity components in the navigational coordinate system. FIG. 3 is a graph showing the effects of the velocity on the X-axis, Y-axis and Z-axis before and after correction in the embodiment of the present invention, as shown in FIG. 3, in each directionThe positive velocity substantially eliminates velocity drift.

S4, correcting the speed v_iPerforming time dimension integration on each velocity component to obtain position coordinates P of the robot at each moment_i：

$P_i=P_{i,t_{n-1}}+\underset{t\∈\[t_{n-1},t_n\]}{\Σ}{v}_i$

Wherein,is t_n-1The coordinates of the sweeping robot at the moment.

The position coordinates P of each time point_iAnd connecting to obtain the motion trail of the robot. Fig. 4 is a track diagram of the sweeping robot inertial navigation system after speed correction in the embodiment of the present invention, and as shown in fig. 4, the driving track of the robot is obtained according to the speed measured by the inertial navigation system, it can be seen that the angle of the sweeping robot during turning is substantially a right angle, and is very close to the actual arcuate motion path of the sweeping robot, which indicates that there is substantially no speed drift after the measured speed correction.

The above-mentioned embodiments are intended to illustrate the objects, technical solutions and advantages of the present invention in further detail, and it should be understood that the above-mentioned embodiments are only exemplary embodiments of the present invention, and are not intended to limit the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (10)

1. A drift calibration method of an inertial navigation system of a robot is used for calibrating the speed of the robot measured by the inertial navigation system, and is characterized by comprising the following steps:

s1, acquiring the adjacent robot motion state change time t_n-1And t_nThe robot motion state comprises a linear motion state, an original turning state and a static state;

s2, acquiring the two state change moments t of the inertial navigation system_n-1And t_nMeasured velocity v_n-1And v_nAnd calculating a change slope v_d：

$v_d=\frac{v_n-v_{n-1}}{t_n-t_{n-1}};$

S3, according to the change slope v_dTo t_n-1～t_nCalibrating the speed measured by the inertial navigation system in the time period to obtain the calibrated speed v_i：

v_i＝v_t-v_d(t-t_n-1)，

Wherein v is_tT ∈ [ t ] is the measured velocity of the inertial navigation system at time t before calibration_n,t_n-1]。

2. The drift calibration method for an inertial navigation system of a robot according to claim 1, wherein said step S1 includes:

s11, acquiring an angular velocity signal and an acceleration signal of the robot and preprocessing the signals;

s12, synthesizing the angular velocity signal and the acceleration signal after the preprocessing to obtain a synthesized signal, wherein the time corresponding to the signal upper edge or the signal lower edge of the synthesized signal is the state change time;

s13, two consecutive state change timings are acquired.

3. The method for calibrating drift of an inertial navigation system of a robot according to claim 2, wherein said preprocessing in step S11 includes:

comparing the angular velocity signal with a first threshold, and when the angular velocity signal is greater than the first threshold, enabling the angular velocity signal to be at a high level, otherwise, enabling the angular velocity signal to be at a low level;

and comparing the acceleration signal with a second threshold value, and when the acceleration signal is greater than the second threshold value, enabling the acceleration signal to be at a high level, otherwise, enabling the acceleration signal to be at a low level.

4. The method of drift calibration of a robotic inertial navigation system of claim 3, wherein the first threshold is 50 degrees/s.

5. The method of claim 3, wherein the second threshold is 0.001m/s²。

6. The drift calibration method for an inertial navigation system of a robot according to claim 3, wherein in step S2, the preprocessed angular velocity signal and the preprocessed acceleration signal are OR-ed to obtain a composite signal.

7. The method of claim 1, wherein the corrected velocity v is calibrated_iIncluding velocity components of the X, Y and Z axes of the navigational coordinate system, the method further comprising:

s4, correcting the speed v_iPerforming time dimension integration on each velocity component to obtain position coordinates P of the robot at each moment_iThe position coordinates P of each time are calculated_iAnd connecting to obtain the motion trail of the robot.

8. The method of drift calibration for a robotic inertial navigation system of claim 1, further comprising:

s0, acquiring an original acceleration signal [ a ] of the robot under a self coordinate system_x,ba_y,ba_z,b]And performing coordinate system conversion to obtain an acceleration signal [ a ] under the navigation coordinate system_x,ka_y,ka_z,k]：

$\left[\begin{array}{c}{a}_{x,k}\\ a_{y,k}\\ a_{z,k}\end{array}\right]=\left[\begin{array}{ccc}\cos R\cos H-\sin P\sin R\sin H& \cos R\sin H+\sin P\sin R\cos H& \cos P\sin R\\ -\cos P\sin H& \cos P\cos H& \sin P\\ \sin R\cos H+\sin P\cos R\sin H& \sin R\sin H-\sin P\cos R\cos H& \cos P\cos R\end{array}\right]\left[\begin{array}{c}{a}_{x,b}\\ a_{y,b}\\ a_{z,b}\end{array}\right],$

H, P and R respectively represent a course angle, a pitch angle and a roll angle, and k represents the kth moment of signal sampling of the inertial navigation system;

for the acceleration signal [ a ] in the navigation coordinate system_x,ka_y,ka_z,k]Performing time dimension integration to obtain the velocity v before correction_t：

$v_t=\left[\begin{array}{c}{v}_{x,t}\\ v_{y,t}\\ v_{z,t}\end{array}\right]=\left[\begin{array}{c}\underset{k=1}{\overset{t}{\Σ}}{a}_{x,k}\·\mathrm{\Δ}t\\ \underset{k=1}{\overset{t}{\Σ}}{a}_{y,k}\·\mathrm{\Δ}t\\ \underset{k=1}{\overset{t}{\Σ}}{a}_{z,k}\·\mathrm{\Δ}t\end{array}\right]$

Where t represents the current moment of signal sampling by the inertial navigation system and Δ t represents the sampling time interval.

9. The drift calibration method for an inertial navigation system of a robot according to claim 2, wherein said inertial navigation system comprises a gyroscope and an accelerometer, wherein said gyroscope is used for sampling angular velocity signals of said robot, and said accelerometer is used for sampling acceleration signals of said robot.

10. The method of drift calibration of a robotic inertial navigation system of claim 9, wherein the gyroscopes and accelerometers are MEMS devices.