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

Abstract

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. The method first obtains two adjacent motion state change moments of the robot, and then obtains the two state change moments of the robot In order to calculate the change slope of the robot speed drift, finally, according to the change slope of the robot speed drift, the speed measured by the inertial navigation system is corrected to obtain the corrected speed. The method can correct the speed measured by the inertial navigation system without frequent shutdown, and has the advantage of high correction accuracy.

Description

A Drift Calibration Method for Robotic Inertial Navigation System

technical field

The invention belongs to the field of robots, and in particular 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 technique

As one of the most successful robot technologies for civilian use, sweeping robots are entering more and more families, and intelligent cleaning is making people's home life more and more warm. The pursuit of a higher level of intelligence is a main line of robot development. Through sensors with various new functions, some common use problems of sweeping robots such as anti-collision and anti-drop have been successfully solved. It is convenient for people to use.

At present, the latest development trend of sweeping robots is to realize 100% indoor environment cleaning without dead ends by automatically planning cleaning routes, which will be much better than the currently commonly used random collision pathfinding mode. The technical core of automatically planning cleaning routes is that the robot must be able to navigate autonomously and know its position and movement trajectory in the house in real time.

Inertial navigation technology (hereinafter referred to as inertial navigation, INS) is a more commonly used robot navigation method. It does not depend on external information and is an autonomous navigation system that is not easily disturbed. Inertial navigation measures the acceleration of the carrier in the inertial reference system (usually the reference system based on the robot itself), and uses the angular velocity measured by the gyroscope to convert it to the navigation coordinate system (for sweeping robots, it is generally the ground in the house by default), Then the attitude information such as the carrier's instantaneous velocity, instantaneous position, and yaw angle is obtained through the integral operation. The advantage of inertial navigation is anti-interference. It only needs to give the initial coordinates (the sweeping robot can use the charging pile as the initial point of each movement), and the position, direction and speed of the robot can be determined at any time without external reference during operation. This method is suitable for precise positioning and orientation in complex geographical environments and occasions with severe external interference. At present, Qingdao Haitong Robot and Xinsong AGV robot handling system in China are all researching and developing robot inertial navigation technology. The key to applying inertial navigation system to robot navigation is to solve the problem of inertial navigation system drift over time, especially the commonly used MEMS inertial navigation system. Although MEMS gyroscopes and accelerometers have many advantages such as small size, easy installation, and low power consumption, However, the drift of the gyroscope and accelerometer will cause the robot's walking path calculated by the inertial navigation algorithm to seriously deviate from the real path.

The core components of inertial navigation are gyroscopes and accelerometers. Using their outputs for coordinate transformation and integration can solve the robot's walking trajectory. The output of these two devices will drift, and the integral operation will accumulate the interference caused by the drift, making the interference problem more serious. What the present invention solves is to invent a method capable of suppressing the drift of the inertial navigation system.

At present, the commonly used inertial navigation system drift correction method is zero-speed correction, that is, to find the two adjacent parking moments of the robot, and the speed when parking is 0. If the measured vehicle speed is not 0 when parking, it is caused by sensor drift. The drift of the sensor over time can be calculated by the two time intervals and the sensor solution value of the vehicle speed when parking (not 0 due to sensor drift). However, zero-speed calibration is not suitable for sweeping robots. Sweeping robots usually have to work non-stop for a long time, and it is difficult to have a static moment.

Although there will be moments close to zero speed such as hitting the wall and turning back, turning in situ, etc., at this time, because the motor power system of the robot has not stopped working, affected by the vibration of the motor, the output value of the accelerometer is vibrating greatly, and it is difficult to judge zero speed through acceleration. time.

If it is necessary to rely on frequent shutdowns for zero-speed calibration, it will greatly prolong the sweeping time of the robot and reduce efficiency, and frequent shutdowns for zero-speed calibration will cause serious fluctuations in motor load and damage life.

Contents of the invention

(1) Technical problems to be solved

In view of the above problems, the object of the present invention is to provide a robot speed calibration method, which can perform robot speed calibration without frequent shutdown, and has the advantage of high calibration accuracy.

(2) Technical solution

The present invention provides a drift calibration method for a robot inertial navigation system, which is used to calibrate the robot speed measured by the inertial navigation system. The method includes:

S1, obtain the adjacent robot motion state change moments t _n-1 and t _n , where the robot motion state includes the linear motion state, the original turning state, and the static state;

S2. Obtain the velocity v _n-1 and v _n measured by the inertial navigation system at two state change moments t _n-1 and t _n , and calculate the change slope v _d :

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; ;</span>$

S3, according to the change slope v _d , calibrate the speed measured by the inertial navigation system during the period t _n-1 ~ t _n , and obtain the calibrated speed v _i :

v _i =v _t -v _d (tt _n-1 ),

Among them, v _t is the velocity measured by the inertial navigation system at time t before calibration, t∈[t _n ,t _n-1 ].

(3) Beneficial effects

The present invention extracts the state change moment of the robot by analyzing the angular velocity and acceleration when the robot is running, that is, the moment when the linear velocity changes as the correction moment. At this time, the theoretical vehicle speed is 0, so the measured at this time The speed is the speed drift, so 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 accuracy.

Description of drawings

FIG. 1 is a flowchart of a drift correction method for an inertial navigation system of a sweeping robot provided by an embodiment of the present invention.

Fig. 2 is a waveform diagram of an angular velocity signal, an acceleration signal and a composite signal in an embodiment of the present invention.

FIG. 3 is an effect diagram of the speed before and after correction on the X-axis, Y-axis and Z-axis in the embodiment of the present invention.

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

detailed description

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. Speed value, so as to calculate the change slope of the robot speed drift. Finally, according to the change slope of the robot speed drift, the speed measured by the inertial navigation system is corrected to obtain the corrected speed. The method can correct the speed measured by the inertial navigation system without frequent shutdown, and has the advantage of high correction accuracy.

According to an embodiment of the present invention, the drift calibration method of the robot inertial navigation system comprises:

S1. Obtain the adjacent robot motion state change moments t _n-1 and t _n , where the robot motion state includes the linear motion state, the original turning state, and the static state, so the motion state change time refers to the robot’s linear motion state, The moment of transition between the original turning state and the stationary state;

S2. Obtain the velocity v _n-1 and v _n measured by the inertial navigation system at two state change moments t _n-1 and t _n , and calculate the change slope v _d :

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; ;</span>$

S3, according to the change slope v _d , calibrate the speed measured by the inertial navigation system during the period t _n-1 ~ t _n , and obtain the calibrated speed v _i :

v _i =v _t -v _d (tt _n-1 ),

Among them, v _t is the velocity measured by the inertial navigation system at time t before calibration, t∈[t _n ,t _n-1 ].

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

S11, acquiring the angular velocity signal and the acceleration signal of the robot and performing preprocessing;

S12. Combining the preprocessed angular velocity signal and acceleration signal to obtain a composite signal. The moment corresponding to the signal rising edge or signal falling edge of the composite signal is the state change moment. Specifically, the angular velocity signal and the acceleration signal can be or operation to obtain a composite signal;

S13. Obtain two consecutive state change moments.

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

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

Comparing the acceleration signal with the second threshold, when the acceleration signal is greater than the second threshold, the acceleration signal is at a high level, otherwise, the acceleration signal is at a low level, preferably, the second threshold is 0.001m/s ² .

According to an embodiment of the present invention, the corrected velocity v _i includes velocity components of the X-axis, Y-axis and Z-axis in the navigation coordinate system, and the method further includes: S4, performing an operation on each velocity component in the corrected velocity v _i Integrate in the time dimension to obtain the position coordinates P _{i of the robot at each time, and connect the position coordinates P i at} each time to obtain the trajectory of the robot.

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

S0, obtain the acceleration signal [a _{x, b} a _{y, b} a _{z, b} ] of the robot in its own coordinate system, and use the Euler angle method to convert the coordinate system to obtain the acceleration signal [a _{x, k} a _{y, k} a _{z, k} ]:

$\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\\ \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\end{array}\right]\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\end{array}\right]$

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

Integrate the acceleration signal [a _{x, k} a _{y, k} a _{z, k} ] in the time dimension of the navigation coordinate system to obtain the velocity v _t before correction:

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\\ \underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\\ \underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\end{array}\right]<span\; class="notranslate">\; ,</span>$

Among them, t represents the current moment of signal sampling of the inertial navigation system, and Δt represents the sampling time interval. Because the drift interference in the acceleration will accumulate with the integral, the calculation result of the speed will deviate from the actual value and increase linearly and rapidly.

According to one embodiment of the present invention, the inertial navigation system includes a gyroscope and an accelerometer, both of which are MEMS devices. The gyroscope is used to obtain the angular velocity signal of the robot, and the accelerometer is used to obtain the acceleration signal of the robot.

In order to make the object, technical solution and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings.

Fig. 1 is a flow chart of the drift correction method of the sweeping robot inertial navigation system provided by the embodiment of the present invention. As shown in Fig. 1, the method includes:

S0, obtain the original acceleration signal [a _{x, b} a _{y, b} a _{z, b} ] of the sweeping robot in its own coordinate system through the MEMS accelerometer, and use the Euler angle method to convert the coordinate system to obtain the acceleration signal in the navigation coordinate system Acceleration signal [a _{x, k} a _{y, k} a _{z, k} ]:

$\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\\ \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\end{array}\right]\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$

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

Integrate the acceleration signal [a _{x, k} a _{y, k} a _{z, k} ] in the time dimension of the navigation coordinate system to obtain the velocity v _t before correction:

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\\ \underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\\ \underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\end{array}\right]<span\; class="notranslate">\; ,</span>$

Among them, t represents the current moment of signal sampling of the inertial navigation system, and Δt represents the sampling time interval.

S1. Obtain adjacent robot motion state change moments t _n-1 and t _n , wherein the robot motion state includes a straight line motion state, an original turning state, and a static state. Specifically include:

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, make the angular velocity signal at a high level, otherwise, make the angular velocity signal at a low level; when the acceleration signal is greater than 0.001m/s ² , make the acceleration signal at a high level, Otherwise, make the acceleration signal low.

Fig. 2 is a waveform diagram of an angular velocity signal, an acceleration signal and a composite signal provided by an embodiment of the present invention. The sweeping robot usually walks in a straight line, and the heading angular velocity is basically 0. When turning, it stops first and then turns on the spot. At this time, the robot’s driving speed is 0. The position coordinates remain unchanged, but the rotational angular velocity is very large, as shown in Figure 2(a) and Figure 2(b) shows. In this embodiment, the upper limit of the angular velocity when the robot travels in a straight line (that is, 50 degrees/s, and the robot can reach 100 degrees/s when turning in the commonly used "bow" route sweeping mode) is used as the threshold for judging whether the robot turns. At the same time, considering that the sweeping robot is in a parking state at the starting point and the end point, the motor of the power unit is not started, and there is no vibration interference to the accelerometer. At this time, the output of the accelerometer is 0, and this embodiment takes a very small acceleration threshold (i.e. 0.001m/s ² ) Determine the parking state, that is, the high level is the starting state, and the low level is the parking state.

S12, perform an OR operation on the preprocessed angular velocity signal and acceleration signal to obtain a composite signal, as shown in Figure 2(c), the high level of the composite signal indicates that the linear velocity of the sweeping robot is 0, and the low level indicates that the linear velocity of the sweeping robot is 0. Velocity is not 0. Therefore, the falling edge of the composite signal can represent two situations: the moment when the robot turns from turning to walking in a straight line, and the moment when the robot starts from stopping; the rising edge of the composite signal can represent two situations: the moment when the robot goes from going straight to turning Moment, the moment from start to stop of the robot. The moment corresponding to the signal rising edge or signal falling edge of the synthesized signal is taken as the moment of state change. At this time, the linear velocity of the robot is theoretically 0, so the velocity measured at this moment is the velocity drift.

S13. Obtain two consecutive state change moments t _n-1 and t _n when the robot is turning.

S2. Obtain the velocity values v _n-1 and v _n of the robot at two state change moments t _n-1 and t _n , and calculate the change slope v _d of the robot’s velocity drift:

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; ;</span>$

S3, according to the change slope v _d of the robot speed drift, correct the speed in the time period from t _n-1 to t _n , and obtain the corrected speed v _i :

v _i =v _t -v _d (tt _n-1 ),

Among them, v _t is the speed before correction, t∈[t _n ,t _n-1 ]. Wherein, the corrected velocity v _i includes the velocity components of the X-axis, Y-axis and Z-axis in the navigation coordinate system. FIG. 3 is an effect diagram of the speed before and after correction on the X-axis, Y-axis and Z-axis in the embodiment of the present invention. As shown in FIG. 3 , the corrected speed in each direction basically eliminates the speed drift.

S4. Integrate each velocity component in the corrected velocity v _i in the time dimension to obtain the position coordinates P _i of the robot at each moment:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; +</span>\underset{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; \&Element;</span><span\; class="notranslate">\; \&lsqb;</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; \&rsqb;</span>}{<span\; class="notranslate">\; \&Sigma;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}$

in, is the coordinates of the sweeping robot at time t _n-1 .

Connect the position coordinates P _i at each time to obtain the trajectory of the robot. Fig. 4 is the trajectory diagram of the inertial navigation system of the sweeping robot in the embodiment of the present invention after speed correction. As shown in Fig. 4, according to the speed measured by the inertial navigation system, the driving trajectory of the robot is obtained. The angle is basically a right angle, which is very close to the bow-shaped motion path of the real sweeping robot, indicating that there is basically no speed drift after the measured speed is corrected.

The specific embodiments described above have further described the purpose, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above descriptions are only specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims (10)

1. A drift calibration method of a robot inertial navigation system, used to calibrate the robot speed measured by an inertial navigation system, is characterized in that the method comprises: S1, acquiring adjacent robot motion state change moments t _n-1 and t _n , wherein the robot motion state includes a straight line motion state, an original turning state, and a static state; S2. Obtain the speed v _n-1 and v _n measured by the inertial navigation system at the two state change moments t _n-1 and t _n , and calculate the change slope v _d : ${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; ;</span>$ S3, according to the change slope v _d , calibrate the speed measured by the inertial navigation system during the period t _n-1 ~ t _n , and obtain the calibrated speed v _i : v _i =v _t -v _d (tt _n-1 ), Among them, v _t is the velocity measured by the inertial navigation system at time t before calibration, t∈[t _n ,t _n-1 ]. 2. the drift calibration method of robot inertial navigation system according to claim 1, is characterized in that, described step S1 comprises: S11, acquiring the angular velocity signal and the acceleration signal of the robot and performing preprocessing; S12. Combining the preprocessed angular velocity signal and the acceleration signal to obtain a composite signal, and the moment corresponding to the signal rising edge or the signal falling edge of the composite signal is a state change moment; S13. Obtain two consecutive state change moments. 3. the drift calibration method of robot inertial navigation system according to claim 2, is characterized in that, in the described step S11, preprocessing comprises: Comparing the angular velocity signal with a first threshold, when the angular velocity signal is greater than the first threshold, the angular velocity signal is at a high level, otherwise, the angular velocity signal is at a low level; The acceleration signal is compared with a second threshold, and when the acceleration signal is greater than the second threshold, the acceleration signal is set at a high level, otherwise, the acceleration signal is set at a low level. 4. The drift calibration method of the robot inertial navigation system according to claim 3, wherein the first threshold is 50 degrees/s. 5 . The drift calibration method of the robot inertial navigation system according to claim 3 , wherein the second threshold is 0.001 m/s ² . 6. The drift calibration method of the robot inertial navigation system according to claim 3, characterized in that, in the step S2, an OR operation is performed on the preprocessed angular velocity signal and the acceleration signal to obtain a composite signal. 7. the drift calibration method of robot inertial navigation system according to claim 1, is characterized in that, the velocity v after described _correction comprises the velocity component of X-axis, Y-axis and Z-axis under navigation coordinate system, described method Also includes: S4. Integrate each velocity component in the corrected velocity v _i in the time dimension to obtain the position coordinates P _{i of the robot at each time, and connect the position coordinates P i at} each time to obtain the trajectory of the robot. 8. the drift calibration method of robot inertial navigation system according to claim 1, is characterized in that, also comprises: S0, obtain the original acceleration signal [a _{x, b} a _{y, b} a _{z, b} ] of the robot in its own coordinate system, and perform coordinate system conversion to obtain the acceleration signal [a _{x, k} a _{y, k} a _{z, k} ]: $\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\\ \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; h</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; P</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; R</span>}\end{array}\right]\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; b</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ Among them, H, P, R represent heading angle, pitch angle and roll angle respectively, and k represents the kth moment of signal sampling of inertial navigation system; Integrate the acceleration signal [a _{x, k} a _{y, k} a _{z, k} ] in the time dimension under the navigation coordinate system to obtain the velocity v _t before correction: ${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\\ \underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\\ \underset{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; t</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; t</span>}\end{array}\right]$ Among them, t represents the current moment of signal sampling of the inertial navigation system, and Δt represents the sampling time interval. 9. The drift calibration method of robot inertial navigation system according to claim 2, is characterized in that, described inertial navigation system comprises gyroscope and accelerometer, utilizes the angular velocity signal of described gyroscope sampling robot, utilizes described accelerometer Sample the acceleration signal of the robot. 10. The drift calibration method of robot inertial navigation system according to claim 9, is characterized in that, described gyroscope and accelerometer are MEMS devices.