Inertial attitude data correction method

Abstract

The invention relates to an inertial attitude data correction method, which comprises the following steps: S1, continuously detecting the pitch angle of a carrier, and judging whether the carrier is located on a vertical plane in real time; s2, when it is detected that the carrier is located on the vertical plane, recording a first course angle A1 of the carrier relative to a reference coordinate system; s3, continuously detecting the pitch angle of the carrier, and judging whether the carrier is separated from the vertical plane or not in real time; s4, when it is detected that the carrier is separated from the vertical plane, recording a second course angle A2 of the carrier relative to the reference coordinate system at the moment of separation; and S5, correcting the second course angle A2 into the first course angle A1. The method provided by the invention can effectively limit and reduce the course angle error drift of the carrier on the vertical plane.

Description

Technical Field

[0001] The present invention relates to the fields of inertial measurement and intelligent robots, and particularly to a method for correcting inertial attitude data. Background Art

[0002] Inertial measurement units (IMUs) based on micro-electromechanical systems (MEMS) have been widely used in the field of mobile robots. Based on MEMS IMU sensors, lawn mowing robots, pool cleaning robots, and window cleaning robots can estimate their own attitude angles such as yaw, pitch, and roll, so as to further plan and control relevant movements and operation tasks.

[0003] Among these robots, pool cleaning robots have certain particularities. Compared with the application scenarios of lawn mowing robots and window cleaning robots that operate on a single plane, modern advanced pool cleaning robots need to clean the bottom, surface, walls, and steps in the pool, and need to frequently switch between different planes such as horizontal and vertical to complete three-dimensional operation tasks. Therefore, the inertial attitude measurement of pool cleaning robots is more difficult and is more easily affected by the errors caused by the switching between different operation planes. A common situation is that when a pool cleaning robot returns to the bottom of the pool after long-term operation on the pool wall, due to the continuous accumulation of inertial measurement errors and the change of the robot's working plane, a significant yaw angle offset will occur, seriously affecting the operation trajectory planning and motion control of the robot. This technical difficulty has become a widespread pain point problem in the pool cleaning robot industry. Summary of the Invention

[0004] In view of this, the present invention proposes a method for correcting inertial attitude data to solve the following problems in the prior art: when a pool cleaning robot returns to the bottom of the pool after long-term operation on the vertical pool wall, due to the influence of error accumulation, a significant yaw angle drift will occur, seriously affecting the operation trajectory planning and motion control of the robot.

[0005] In view of the above problems in the prior art, the inventors noticed that when a pool robot is adsorbed on a vertical pool wall surface for cleaning operations, although its heading angle relative to the East-North-Up (ENU) coordinate system has been continuously estimated and changed, since the azimuth angle of the vertical wall itself is fixed and unchanged, the heading angle of the robot should actually always be approximately equal to the azimuth angle of the vertical wall and should not change or drift significantly. Therefore, the heading angle value of the robot on the vertical wall can be recorded in advance and corrected when it leaves the vertical wall, thereby solving the technical problem of excessive heading angle error drift when the pool cleaning robot operates on the pool wall for a long time. Based on this, the present invention is proposed.

[0006] In a first aspect, an embodiment of the present invention provides an inertial attitude data correction method, including:

[0007] S1, continuously detecting the pitch angle of the carrier and determining in real time whether the carrier is on a vertical plane;

[0008] S2, when it is detected that the carrier is on a vertical plane, recording the first heading angle A of the carrier relative to the reference coordinate system 1 ;

[0009] S3, continuing to continuously detect the pitch angle of the carrier and determining in real time whether the carrier has left the vertical plane;

[0010] S4, when it is detected that the carrier has left the vertical plane, recording the second heading angle A of the carrier relative to the reference coordinate system at the moment of separation 2 ;

[0011] S5, correcting the second heading angle A 2 to the first heading angle A 1 .

[0012] Preferably, in S1, when the pitch angle of the carrier is greater than θ or less than -θ, it is considered that the carrier is on a vertical plane;

[0013] where, 80° < θ < 90°.

[0014] Preferably, in S2, the reference coordinate system is the East-North-Up coordinate system.

[0015] Preferably, in S3, when the pitch angle of the carrier is less than θ or greater than -θ, it is considered that the carrier has left the vertical plane;

[0016] where, 80° < θ < 90°.

[0017] Preferably, in S1 and S3, the pitch angle is detected by an IMU sensor in the carrier.

[0018] In a second aspect, an embodiment of the present invention further provides a cleaning robot configured to operate on a vertical plane and a non-vertical plane. The cleaning robot includes a control unit configured to execute the method according to any one of the above first aspects.

[0019] The inertial attitude data correction method provided by the embodiment of the present invention cleverly utilizes the physical fact that "the heading angle of a vertical wall itself is fixed and unchanged". When the robot climbs the wall, its initial heading angle is recorded, and when the robot descends the wall, its cumulative heading angle is corrected, thereby effectively limiting and reducing the heading angle error drift of the robot on the vertical wall. Using the inertial attitude data correction method provided by the present invention, even if the pool robot performs cleaning operations on the vertical pool wall for several hours or even longer, when the robot leaves the pool wall, its heading angle will not have a large error drift, and the heading angle error can be controlled within an acceptable range, thereby greatly improving the three-dimensional motion control and operation performance of the pool cleaning robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention, and those of ordinary skill in the art can obtain other drawings without creative efforts based on these drawings.

[0021] Figure 1 is a schematic flow chart of the inertial attitude data correction method according to an embodiment of the present invention;

[0022] Figure 2 is a schematic diagram of the heading angle of the carrier according to an embodiment of the present invention;

[0023] Figure 3 is a schematic diagram of the running trajectory of the carrier according to an embodiment of the present invention;

[0024] Figure 4 is a schematic diagram of a test scenario of a six-axis IMU sensor. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0025] The description of the embodiments of this specification should be combined with the corresponding drawings, and the drawings should be regarded as an integral part of the complete specification. Elements not shown in the drawings or not described in words are in forms known to those of ordinary skill in the art. The following description of the preferred embodiments will involve combinations of features that may exist independently or in combination. The present invention is not particularly limited to the preferred embodiments. The scope of the present invention is defined by the claims.

[0026] AsFigure 1 As shown in the figure, it is a flowchart of the inertial attitude data correction method according to an embodiment of the present invention. In the following, this embodiment will be described by taking the scenario of a pool cleaning robot and its cleaning of a pool (including cleaning the pool bottom and pool walls) as an example. The method of this embodiment includes:

[0027] S1. Continuously detect the pitch angle of the carrier, and real-time determine whether the carrier is on a vertical plane.

[0028] In this embodiment, the pool robot can be regarded as the carrier, and the pool wall can be regarded as the vertical plane. An IMU sensor is provided inside the carrier. A common IMU sensor is usually composed of a three-axis accelerometer and a three-axis gyroscope. Among them, the accelerometer measures and outputs the acceleration measurement value, and the gyroscope measures and outputs the angular rate measurement value. Using the acceleration and angular rate measurement values output by the IMU sensor, the pitch angle of the carrier is calculated, so as to determine whether the carrier is above the vertical plane.

[0029] Generally, when the pitch angle θ of the carrier is nearly close to 90°, it can be considered that the carrier is currently on a vertical plane. For the pool cleaning robot, this means that the robot is currently adsorbed on the vertical pool wall for operation. Considering the jitter during the vertical operation of the robot and the fact that the wall is not necessarily strictly vertical, the requirement for the pitch angle θ to be close to 90° can be appropriately relaxed. For example, it can be set that when θ > 85° or θ < -85°, or when θ > 80° or θ < -80°, it is considered that the carrier is in a vertical state and above the vertical plane of the pool wall.

[0030] It should be noted that when calculating the pitch angle θ of the carrier, if the IMU sensor is not horizontally installed relative to the carrier coordinate system, the installation angle error of the IMU sensor also needs to be taken into account.

[0031] It should also be noted that in step S1, when continuously detecting the pitch angle of the carrier but not detecting that the carrier is on a vertical plane, it usually means that the pool cleaning robot is cleaning the pool bottom.

[0032] S2. When it is detected that the carrier is on a vertical plane, record the first heading angle A of the carrier relative to the reference coordinate system 1 .

[0033] As Figure 2 shown, when the pool cleaning robot works on non-vertical planes such as the pool bottom and the water surface, the north-east-up coordinate system (ENU) is used. At this time, the three attitude angles of the IMU sensor are defined and output as follows:

[0034] Yaw: Rotate around the Z-axis. It is zero when the carrier is facing due north, positive when it is north by west, and the angle range is -180° to 180°.

[0035] Pitch: Rotate around the X-axis. It is positive when the carrier is looking up, and the angle range is -90° to 90°.

Claims

1. A method for correcting inertial attitude data, characterized in that: The method comprises: S1, continuously detecting the pitch angle of the carrier, and determining in real time whether the carrier is in a vertical plane; S2, when it is detected that the carrier is on a vertical plane, recording a first heading angle A1 of the carrier relative to a reference coordinate system; S3, continue to detect the pitch angle of the carrier, and determine in real time whether the carrier is out of the vertical plane; S4, when it is detected that the carrier deviates from the vertical plane, recording a second heading angle A2 of the carrier relative to the reference coordinate system at the moment of departure; S5: Correct the second heading angle A2 to the first heading angle A1.

2. The inertial attitude data correction method according to claim 1, characterized in that: In S1, when the pitch angle of the carrier is greater than θ or less than -θ, it is considered that the carrier is in a vertical plane; Among them, 80°<θ<90°.

3. The inertial attitude data correction method according to claim 1, characterized in that: In S2, the reference coordinate system is the northeast celestial coordinate system.

4. The inertial attitude data correction method according to claim 1, characterized in that: In S3, when the pitch angle of the carrier is less than θ or greater than -θ, it is considered that the carrier is out of the vertical plane; Among them, 80°<θ<90°.

5. The inertial attitude data correction method according to claim 1, characterized in that: In S1 and S3, the pitch angle detection is performed by an IMU sensor in the carrier.

6. A cleaning robot, the cleaning robot being configured to operate on a vertical plane and a non-vertical plane, characterized in that: The cleaning robot comprises a control unit, and the control unit is used to execute the method according to any one of claims 1-5.

Images