Underwater positioning method, self-moving equipment and computer readable storage medium

Abstract

The embodiment of the invention provides an underwater positioning method, a self-moving device and a computer readable storage medium. The method comprises the following steps: constructing an environment boundary map of an underwater operation area; when the construction of the environment boundary map is completed, acquiring motion information of the self-moving equipment at the end point position of the environment boundary map as initial pose information; in the running process of the self-moving equipment, obtaining motion information increment and initial pose information of the current time period, and determining predicted pose information of the self-moving equipment; determining predicted distance information of a distance sensor based on the predicted pose information, and calculating an observation error of the distance information based on the predicted distance information and actual distance information sensed by the distance sensor; and determining actual pose information of the self-moving equipment based on the observation error. According to the embodiment of the invention, the actual pose information of the self-moving equipment is determined by combining the pose information and the distance information, the positioning error is effectively reduced, and the positioning accuracy is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic navigation, and in particular to an underwater positioning method, a self-moving device, and a computer readable storage medium. BACKGROUND

[0002] With the continuous progress of science and technology and the rapid development of artificial intelligence, using self-moving devices (for example, lawn mowing robots, cleaning robots, food delivery robots, etc.) to replace manual work can greatly improve work efficiency.

[0003] Underwater cleaning robots usually use odometers for positioning. Since the odometer is formed by fusing sensors such as wheel speed meters and inertial measurement units, collaborative errors are easily generated among multiple sensors, thereby causing positioning errors, and as the mileage accumulates, the positioning errors also gradually increase, thereby affecting cleaning efficiency. SUMMARY

[0004] Therefore, embodiments of the present application provide an underwater positioning method, a self-moving device, and a computer readable storage medium to solve the problem that when a self-moving device uses an odometer for positioning, there is a positioning error, thereby affecting work efficiency.

[0005] In a first aspect, embodiments of the present application provide an underwater positioning method applied to a self-moving device, and the method comprises: constructing an environmental boundary map of an underwater work area; when the environmental boundary map is constructed, obtaining motion information of the self-moving device at an end position of the environmental boundary map as initial pose information; during operation of the self-moving device, obtaining a motion information increment of a current time period and the initial pose information to determine predicted pose information of the self-moving device; determining predicted distance information of a distance sensor based on the predicted pose information, and calculating an observation error of distance information based on the predicted distance information and actual distance information sensed by the distance sensor; determining actual pose information of the self-moving device based on the observation error.

[0006] In a possible implementation, the constructing an environmental boundary map of an underwater work area comprises: controlling the self-moving device to move along the environmental boundary of the underwater work area, and constructing an environmental boundary map of the underwater work area based on displacement data sensed by an odometer and distance data sensed by a lateral distance sensor, the environmental boundary map comprising point cloud data of multiple positions of the environmental boundary.

[0007] In a possible implementation, the determining the predicted pose information of the self-moving device comprises: obtaining the predicted pose information based on a sum value between the motion information increment and the initial pose information.

[0008] In a possible implementation, the determining the predicted distance information of the distance sensor based on the predicted pose information comprises: determining a distance between the distance sensor and a wheel of the self-moving device based on an extrinsic parameter of the distance sensor; calculating positioning information of the distance sensor based on the predicted pose information and the distance between the distance sensor and the wheel; determining, as the predicted distance information, minimum distance information within a sensing range of the distance sensor based on the positioning information of the distance sensor, and determining positioning information corresponding to the predicted distance information.

[0009] In a possible implementation, the calculating an observation error of distance information based on the predicted distance information and actual distance information sensed by the distance sensor comprises: calculating a difference value between the actual distance information and the predicted distance information to obtain the observation error.

[0010] In a possible implementation, the determining the actual pose information of the self-moving device based on the observation error comprises: calculating an observed Jacobian matrix based on the predicted distance information; calculating a Kalman gain parameter of a Kalman filtering algorithm based on a preset process noise, a preset observation noise, and the Jacobian matrix; performing operation on the observation error, the predicted pose information, and the Kalman gain parameter by using the Kalman filtering algorithm to obtain the actual pose information.

[0011] In a possible implementation, the preset process noise of the Kalman filtering algorithm is updated based on the Kalman gain parameter and the Jacobian matrix to obtain a process noise at a next time of pose updating.

[0012] In a possible implementation, the method further comprises: publishing the actual pose information by broadcasting the actual pose information to a shared message channel.

[0013] In a second aspect, an embodiment of the present application provides a self-moving device, which comprises a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor implements the underwater positioning method as described above when executing the computer program.

[0014] In a third aspect, an embodiment of the present application provides a computer readable storage medium, and the computer readable storage medium stores a computer program. The computer program is executed by a processor in a self-moving device, so that the self-moving device implements the underwater positioning method as described above.

[0015] The underwater positioning method, the self-moving device and the computer readable storage medium provided by the embodiments of the present application can determine the actual pose information of the self-moving device by combining the pose information sensed by the odometry sensor and the distance information sensed by the distance sensor, instead of only relying on the pose information sensed by the odometry sensor for positioning, thereby effectively reducing the positioning error and improving the positioning accuracy, so as to improve the operation efficiency of the self-moving device in the underwater operation environment. BRIEF DESCRIPTION OF DRAWINGS

[0016] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description are only embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of the provided drawings.

[0017] Figure 1 FIG. 1 is a schematic diagram of the hardware structure of the self-moving device provided by an embodiment of the present application.

[0018] Figure 2 FIG. 2 is a flowchart of the underwater positioning method provided by an embodiment of the present application.

[0019] Figure 3 FIG. 3 is a schematic diagram of the sensor of the self-moving device provided by an embodiment of the present application.

[0020] Figure 4 FIG. 4 is a flowchart of determining the predicted distance information provided by an embodiment of the present application.

[0021] Figure 5 FIG. 5 is a schematic diagram of the pose information of the self-moving device provided by an embodiment of the present application.

[0022] Figure 6 FIG. 6 is a schematic diagram of the three-dimensional structure of the underwater cleaning robot provided by an embodiment of the present application. DETAILED DESCRIPTION

[0023] In order to make the purpose, technical solutions and advantages of the present application more clear, the following will describe the present application in detail with reference to the drawings and specific embodiments.

[0024] It should be noted that, in the present application, "at least one" means one or more, and "multiple" means two or more than two. The "and / or" describes the association between the associated objects, which means that there can be three kinds of relationships, for example, A and / or B can represent: A exists alone, A and B exist together, and B exists alone, where A and B can be singular or plural. The terms "first", "second", "third", "fourth" and the like (if any) in the specification and claims of the present application and the drawings are used to distinguish similar objects, and are not used to describe a specific order or sequence.

[0025] In the embodiments of the present application, the words such as "exemplary" or "for example" are used to mean an example, illustration, or description. Any embodiment or design scheme described as "exemplary" or "for example" in the embodiments of the present application should not be interpreted as more preferred or more advantageous than other embodiments or design schemes. Rather, the words such as "exemplary" or "for example" are intended to present the relevant concept in a specific manner. The embodiments described below and the features in the embodiments can be combined with each other without conflict.

[0026] With the continuous progress of technology and the rapid development of artificial intelligence, using self-moving devices (such as lawn mowing robots, cleaning robots, food delivery robots, etc.) to replace manual work can greatly improve work efficiency.

[0027] The underwater cleaning robot usually uses an odometer for positioning. Since the odometer is formed by fusing sensors such as a wheel speed meter and an inertial measurement unit, collaborative errors are easily generated between multiple sensors, thereby causing positioning errors. And with the accumulation of mileage, the positioning error also gradually increases, causing the underwater cleaning robot to deviate from the preset work path during the cleaning work, thereby affecting the cleaning work efficiency.

[0028] In order to solve the problem that the self-moving device uses the odometer for positioning, thereby affecting the work efficiency, the embodiments of the present application provide an underwater positioning method, which determines the actual pose information of the self-moving device by combining the pose information sensed by the odometer sensor and the distance information sensed by the distance sensor, instead of relying only on the pose information sensed by the odometer sensor for positioning, thereby effectively reducing the positioning error, improving the positioning accuracy, and thereby improving the work efficiency of the self-moving device in the underwater work environment.

[0029] Referring to Figure 1 Fig. 1 shows a hardware structure schematic diagram of a self-moving device provided by an embodiment of the present application. The underwater positioning method in the embodiments of the present application can be applied to, for example, Figure 1The self-moving device 1 shown in the figure includes a body, and a memory 11, a processor 12, a power supply 13, a sensor 14, a working mechanism 15, a communication module 16, a positioning module 17, a driving wheel 18 and a bus 19 arranged on the body. The processor 12 is coupled to the memory 11, the power supply 13, the sensor 14, the working mechanism 15, the communication module 16, the positioning module 17 and the driving wheel 18 through the bus 19 respectively. The self-moving device 1 is an electronic device with a self-moving function, such as a lawn mower robot, a cleaning robot, a food delivery robot, etc.

[0030] The memory 11 can include one or more random access memories (RAMs) and one or more non-volatile memories (NVMs). The random access memory can be directly read and written by the processor 12, and can be used to store executable programs (such as machine instructions) of operating systems or other running programs, and can also be used to store data of users and applications, etc. The random access memory can include static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), etc.

[0031] The non-volatile memory can also store executable programs and store data of users and applications, etc., which can be loaded into the random access memory in advance for direct reading and writing by the processor 12. The non-volatile memory can include a magnetic disk storage device, a flash memory.

[0032] The memory 11 is used to store one or more computer programs. The one or more computer programs are configured to be executed by the processor 12. The one or more computer programs include a plurality of instructions which, when executed by the processor 12, can implement an underwater positioning method executed on the self-moving device 1.

[0033] In other embodiments, the self-moving device 1 further includes an external memory interface for connecting an external memory to expand the storage capacity of the self-moving device 1.

[0034] The processor 12 can include one or more processing units, for example: the processor 12 can include an application processor (AP), a modem processor, a graphics processing unit (GPU), an image signal processor (ISP), a controller, a video codec, a digital signal processor (DSP), a baseband processor, and / or a neural-network processing unit (NPU), etc. Among them, different processing units can be independent devices, or can be integrated in one or more processors.

[0035] The processor 12 provides computing and control capabilities, for example, the processor 12 is used to execute the computer program stored in the memory 11 to realize the above-mentioned underwater positioning method.

[0036] The power supply 13 is used to supply power for the self-moving device. In an embodiment of the present application, the power supply 13 can include any one or more of the following power supply devices: a battery, a fuel generator, a solar power module, a wind power module, etc.

[0037] The sensor 14 is used to obtain information for the self-moving device 1, such as obtaining environmental information for the self-moving device 1 and movement information of the self-moving device 1. In an embodiment of the present application, the sensor 14 can include one or more of the following types of sensors: a laser radar, a camera, an infrared sensor, an encoder, etc.

[0038] The working mechanism 15 is used to perform corresponding work tasks, for example, mowing, patrolling, cleaning, and spraying pesticides, etc. In some embodiments of the present application, the working mechanism 15 can include a motor, a transmission mechanism, and a cutter head, etc. In some embodiments of the present application, the motor can drive the cutter head to rotate through the transmission mechanism to realize the mowing function. The motor can also control the movement of the cutter blade to adjust the mowing height and the mowing area.

[0039] The communication module 16 is used to realize the communication between the self-moving device and other devices. In an embodiment of the present application, the communication module 16 can interact with other devices based on wired communication and / or wireless communication. The above-mentioned wireless communication can include one or a combination of the following communication modes: Bluetooth communication, Wi-Fi communication, Near Field Communication (NFC), etc.

[0040] The positioning module 17 is configured to determine the position of the self-moving device. In some embodiments of the present application, the positioning module 17 can include one or more of a Global Positioning System (GPS), an inertial navigation system, a Real-time kinematic (RTK) carrier phase differential system, and the like.

[0041] The driving wheels 18 are configured to enable the self-moving device to move. In some embodiments of the present application, the driving wheels 18 can enable the self-moving device to move according to the control of the processor 12. In some embodiments of the present application, the driving wheels 18 can include a left driving wheel and a right driving wheel.

[0042] The bus 19 is configured to provide a communication channel between the memory 11, the processor 12, the power supply 13, the sensor 14, the working mechanism 15, the communication module 16, the positioning module 17, and the driving wheels 18 in the self-moving device 1.

[0043] In other embodiments of the present application, the self-moving device 1 can further include a collision avoidance component and a steering component, etc. The collision avoidance component can be configured to prevent the driving wheels 18 from colliding with an obstacle in front of the self-moving device. The steering component can be configured to enable the driving wheels 18 to adjust the driving direction.

[0044] It can be understood that the structure shown in the embodiments of the present application does not constitute a specific limitation on the self-moving device 1. In other embodiments of the present application, the self-moving device 1 can include more or fewer components than shown, or combine certain components, or split certain components, or different component arrangements. The components shown can be implemented in hardware, software, or a combination of software and hardware.

[0045] Referring to Figure 2 FIG. 1 shows a flowchart of an underwater positioning method provided by an embodiment of the present application. The underwater positioning method can be applied to a self-moving device 1 as shown in Figure 1 The underwater positioning method includes the following steps: S101, constructing an environmental boundary map of the underwater working area.

[0046] Referring to Figure 3 FIG. 1 shows a sensor diagram of a self-moving device provided by an embodiment of the present application. The sensors of the self-moving device include, but are not limited to, a distance sensor, an inertial measurement unit, and a wheel speed meter, Figure 3 The triangle in FIG. 1 represents a distance sensor, the rectangle represents an inertial measurement unit, the circle represents a wheel speed meter, and the dashed box represents the environmental boundary of the underwater working area. The distance sensor includes a front distance sensor and a side distance sensor, and the distance sensor is an ultrasonic sensor or a laser sensor.

[0047] In an embodiment of the present application, the inertial measurement unit and the wheel speed meter can be fused to form a distance sensor. Specifically, the inertial measurement unit includes a three-axis gyroscope for measuring the angular velocity of the mobile device and a three-axis accelerometer for measuring the linear acceleration of the mobile device. The errors of the inertial measurement unit are exponentially accumulated due to sensor zero bias, noise and gravity interference. The wheel speed meter measures the number of wheel rotations of the mobile device through an encoder to calculate the linear speed. The speed measurement may be distorted due to factors such as tire slip, uneven ground, mechanical clearance, etc. In addition, the wheel speed meter cannot directly provide attitude information. Therefore, in order to improve the positioning accuracy, the inertial measurement unit and the wheel speed meter are fused to form a distance sensor.

[0048] In an embodiment of the present application, the self-moving device is controlled to move along the environmental boundary of the underwater work area for one circle, and the point cloud information of multiple positions of the environmental boundary is determined based on the pose information sensed by the distance sensor during the movement and the distance information sensed by the lateral distance sensor, so as to obtain the environmental boundary map. Specifically, the displacement increment of the mobile device is sensed by the distance sensor, the pose information of the mobile device is determined based on the displacement increment, and the distance information sensed by the distance sensor is converted into point coordinates in the global coordinate system. The point cloud information of multiple positions of the environmental boundary is determined based on the pose information and the point coordinates, and the point cloud information of multiple positions is fused to form a point cloud map of the environmental boundary.

[0049] Based on the sensing data of the distance sensor and the sensing data of the distance sensor, the positioning accuracy of the self-moving device can be improved, so as to accurately construct the point cloud map of the environmental boundary of the underwater work area.

[0050] S102, when the environmental boundary map is constructed, the motion information of the self-moving device at the end position of the environmental boundary map is obtained as the initial pose information.

[0051] In an embodiment of the present application, after the self-moving device completes the movement along the environmental boundary of the underwater work area for one circle, it is determined that the environmental boundary map is constructed, the motion information of the self-moving device at the end position of the environmental boundary map is obtained, and the motion information of the self-moving device at the end position of the environmental boundary map is determined as the initial pose information. The motion information is the pose information, and the pose information includes the x-axis coordinate value, the y-axis coordinate value of the self-moving device in the global coordinate system and the orientation angle of the self-moving device. For example, the initial pose information is P0(x0, y0, α0).

[0052] By taking the pose information sensed by the distance sensor of the self-moving device at the end position of the environmental boundary map as the basis for subsequent pose prediction of the self-moving device during operation, the accuracy of the predicted pose information can be improved.

[0053] S103, acquiring the motion information increment and the initial pose information of the current time period during the running of the self-moving device, and determining the predicted pose information of the self-moving device.

[0054] In an embodiment of the present application, during the running, the self-moving device acquires the motion information increment every preset period, and the motion information increment of the current time period is the motion information increment of the current period, for example, the preset period is 50 ms, 100 ms, 200 ms or other time length. The motion information increment includes the x-axis coordinate value increment Δx, the y-axis coordinate value increment Δy and the orientation angle increment Δα in the global coordinate system. The sum value between the motion information increment and the initial pose information is calculated to obtain the predicted pose information. For example, the predicted pose information is P(x, y, α), wherein x=x0+Δx, y=y0+Δy, and α=α0+Δα. predict predict predict predict predict predict predict

[0055] In an embodiment of the present application, during the running of the self-moving device, the displacement of the self-moving device can be sensed by the wheel speed meter, the displacement of the self-moving device is decomposed into the displacement in the x-axis direction of the global coordinate system to obtain the x-axis coordinate value increment Δx, and the displacement of the self-moving device is decomposed into the displacement in the y-axis direction of the global coordinate system to obtain the y-axis coordinate value increment Δy. The orientation angle of the self-moving device during the running can be sensed by the inertial measurement unit, and the orientation angle can be used as the orientation angle in the predicted pose information.

[0056] The odometry sensor formed by fusing the inertial measurement unit and the wheel speed meter can sense the displacement and the orientation angle of the self-moving device during the running in real time and accurately, and further accurately predict the predicted pose information of the self-moving device.

[0057] S104, determining the predicted distance information of the distance sensor based on the predicted pose information, and calculating the observation error of the distance information based on the predicted distance information and the actual distance information sensed by the distance sensor.

[0058] Referring to FIG. 11, a flowchart for determining the predicted distance information is provided in an embodiment of the present application. Figure 4

[0059] S1041, determining the distance between the distance sensor and the wheel of the self-moving device based on the extrinsic parameter of the distance sensor.

[0060] Referring to FIG. 11, a flowchart for determining the predicted distance information is provided in an embodiment of the present application. Figure 5 ​​​​​​​​As shown in FIG, a schematic diagram of the posture information of the self-mobile device provided in an embodiment of the present application. For example, the current posture information of the self-mobile device is P predict (x predict , y predict , α predict ), α predict is the orientation angle of the mobile device, which is based on the north direction being 0 degrees, the west direction being 90 degrees, the south direction being 180 degrees, and the east direction being 270 degrees. The sensing range of the distance sensor is β degrees, and the external parameter of the forward distance sensor is e front (ex1, ey1), where the extrinsic parameter is the extrinsic parameter from the distance sensor to the rear wheel of the self-moving device. The calculation formula (1) for determining the distance L1 between the forward distance sensor and the wheel (e.g., the rear wheel) of the self-moving device is: (1).

[0061] In one embodiment of the present application, the external parameter of the lateral (eg left) distance sensor is e left (ex2, ey2), the same method is used to determine the distance L2 between the lateral distance sensor and the wheel (e.g., rear wheel) of the self-moving device. The calculation formula (2) for determining the distance L2 between the lateral distance sensor and the wheel (e.g., rear wheel) of the self-moving device is: (2).

[0062] S1042: Calculate the positioning information of the distance sensor based on the predicted posture information and the distance between the distance sensor and the wheel.

[0063] In one embodiment of the present application, the orientation angle of the forward distance sensor is the same as the orientation angle of the mobile device, and the positioning information of the forward distance sensor is the posture information D1 (x _sonar1 , y _sonar1 , α1), where α1=α predict , x _sonar1 = x predict +L1cos(α predict ), y _sonar1 = y predict + L1sin(α predict ).

[0064] In one embodiment of the present application, the orientation angle of the lateral distance sensor is the orientation angle of the mobile device plus 90 degrees, and the positioning information of the lateral distance sensor is the posture information D2 (x _sonar2 , y _sonar2 , α2), where α2=α predict +90°, x _sonar2 = xpredict + L2cos(a predict + 90°), y _sonar2 = y predict + L2sin(a predict + 90°).

[0065] S1043, determine the minimum distance information in the sensing range of the distance sensor as the predicted distance information based on the positioning information of the distance sensor, and determine the positioning information corresponding to the predicted distance information.

[0066] In an embodiment of the present application, all map points in the sensing range of the front distance sensor are determined based on the positioning information and the sensing range of the front distance sensor, and the map points are position points in the point cloud map. The distance between each map point in the sensing range of the front distance sensor and the front distance sensor is calculated , the coordinates of each map point are (x_map, y_map), wherein The calculation formula (3) of d_front is: (3).

[0067] Determine the minimum d_front in a plurality of , that is, determine the map point (x_map1, y_map1) closest to the front distance sensor and the distance between the map point (x_map1, y_map1) closest to the front distance sensor and the front distance sensor is the predicted distance information of the front distance sensor.

[0068] In an embodiment of the present application, all map points in the sensing range of the side distance sensor are determined based on the positioning information and the sensing range of the side distance sensor, and the map points are position points in the point cloud map. The distance between each map point in the sensing range of the side distance sensor and the side distance sensor is calculated , the coordinates of each map point are (x_map, y_map), wherein The calculation formula (4) of d_side is: (4).

[0069] Determine the minimum d_side in a plurality of , that is, determine the map point (x_map2, y_map2) closest to the side distance sensor and the distance between the map point (x_map2, y_map2) closest to the side distance sensor and the side distance sensor the distance between the lateral distance sensor and the map point (x_map2, y_map2) closest to the lateral distance sensor is the predicted distance information of the lateral distance sensor.

[0070] Optionally, the front distance sensor and the lateral distance sensor can be ultrasonic sensors, sonar sensors, laser radars, or sensors for underwater distance measurement.

[0071] In an embodiment of the present application, a difference between the actual distance information and the predicted distance information is calculated to obtain an observation error. For example, the predicted distance information is , the actual distance information is , and the observation error is .

[0072] The above method can accurately calculate the predicted distance information of the front distance sensor and the lateral distance sensor, and further accurately calculate the observation error of the distance information.

[0073] S105, based on the observation error, determining actual pose information of the self-moving device.

[0074] In an embodiment of the present application, a Jacobian matrix of the observation is calculated, and the Jacobian matrix is a partial derivative matrix for describing the relationship between the observation data and the system state. The calculation formula (5) of the Jacobian matrix H1 of the front distance sensor is: (5).

[0075] wherein the predicted distance information The calculation formula (6) of the partial derivative with respect to x is: (6).

[0076] The predicted distance information The calculation formula (7) of the partial derivative with respect to y is: (7).

[0077] The predicted distance information The calculation formula (8) of the partial derivative with respect to the angle a is: (8).

[0078] In an embodiment of the present application, the calculation formula (9) of the Jacobian matrix H2 of the lateral distance sensor is: (9).

[0079] wherein the predicted distance information The calculation formula (10) of the partial derivative of x is: (10).

[0080] Predicted distance information The calculation formula (11) of the partial derivative of y is: (11).

[0081] Predicted distance information The calculation formula (12) of the partial derivative of angle a is: (12).

[0082] In an embodiment of the present application, the Jacobian matrix H determined based on the Jacobian matrix H1 of the front distance sensor and the Jacobian matrix H2 of the side distance sensor is: (13).

[0083] In an embodiment of the present application, the Kalman gain parameter of the Kalman filter algorithm is calculated based on the preset process noise, the preset observation noise and the Jacobian matrix. The calculation formula (14) of the Kalman gain parameter K is: (14).

[0084] In the calculation formula (14), P is the preset process noise, Q is the preset observation noise, and H is the Jacobian matrix.

[0085] In an embodiment of the present application, the observation error, the predicted pose information and the Kalman gain parameter are operated by using the Kalman filter algorithm to obtain the actual pose information. For example, the predicted distance information is , the actual distance information is , and the actual pose information is P update (x update , y update , a predict ). The calculation formula (15) and (16) of x update and y update in the actual pose information are: (15) ; (16).

[0086] Through the above embodiments of the present application, first, the point cloud map of the environment boundary of the left and right regions underwater is obtained by mapping along the edge, the predicted distance information of the distance sensor is calculated based on the point cloud data of the point cloud map, the actual distance information actually sensed by the distance sensor is taken as the observation value, the Kalman filtering algorithm is used to update the pose information of the self-moving device, so as to determine the actual pose information of the self-moving device in combination with the pose information sensed by the mileage sensor and the distance information sensed by the distance sensor, effectively reduce the positioning error, improve the positioning accuracy, and thus improve the operation efficiency of the self-moving device in the underwater operation environment.

[0087] In an embodiment of the present application, the method further comprises: updating the preset process noise of the Kalman filtering algorithm based on the actual pose information and the predicted pose information to obtain the process noise at the next time of pose updating. The calculation formula (17) of the updated process noise is: (17).

[0088] In the calculation formula (17), I is a unit matrix, K is a Kalman gain parameter, H is a Jacobian matrix, is a prediction error covariance matrix.

[0089] In an embodiment of the present application, after determining the actual pose information of the self-moving device, the method further comprises: publishing the actual pose information. The actual pose information is published by broadcasting the updated actual pose information to a shared message channel. For example, the updated actual pose information is sent to a visual device such as a user terminal and an FPV (first person view) device in the form of a message, so that the visual device displays the real-time position and motion trajectory of the self-moving device. For example, the updated actual pose information is sent to a path planning module in the form of a message, and the path planning module performs path planning based on the updated actual pose. For example, the updated actual pose information is sent to an obstacle avoidance module in the form of a message, and the obstacle avoidance module performs obstacle avoidance based on the updated actual pose.

[0090] The underwater self-moving device introduces an environmental boundary map as a static spatial reference benchmark, and combines an IMU motion incremental inference mechanism, so that high-frequency positioning update and correction of the pool robot can be realized without relying on GPS or visual calibration points. The system can continuously run in a low drift state, greatly reduces the trajectory deviation of the underwater robot caused by inertial error accumulation, effectively solves the problem of inaccurate underwater positioning of the underwater cleaning robot, dynamically adjusts the current pose estimation through actual observation deviation, realizes a robust and adaptive error correction mechanism, effectively improves the adaptability of the underwater self-moving device to complex environments, and can complete autonomous mapping and accurate positioning of the pool operation environment without deploying underwater base stations or auxiliary positioning points, thereby significantly reducing deployment and maintenance costs.

[0091] The computer readable storage medium stores a computer program, and the computer program includes program instructions. The program instructions are executed to implement the method described in the embodiments of the present application.

[0092] Referring to Figure 6 The self-moving device provided by the embodiments of the present application can be an underwater cleaning robot 100, and the underwater cleaning robot 100 can be a pool cleaning robot. The underwater cleaning robot 100 at least includes a shell 10, a walking assembly 20, a working assembly 30, a front distance sensor 40, a side distance sensor 50, and an odometer (not shown in the figure). The odometer is formed by fusion of an IMU and a wheel speed meter. The walking assembly 20 can be driven by a motor, thereby driving the underwater cleaning robot 100 to move. The working assembly 30 can be driven by a motor, thereby performing underwater cleaning work. The underwater cleaning robot 100 further includes a memory and a processor. The memory of the underwater cleaning robot is used to store one or more computer programs configured to be executed by the processor of the underwater cleaning robot. The one or more computer programs include a plurality of instructions, and the plurality of instructions are executed by the processor of the underwater cleaning robot to implement an underwater positioning method executed on the underwater cleaning robot.

[0093] Specifically, the underwater cleaning robot 100 moves slowly and uniformly inside the to-be-cleaned area (for example, a pool) before initial work, detects the pool wall and the bottom boundary through the front distance sensor 40 and the side distance sensor 50, and completes boundary point cloud mapping of the to-be-cleaned area.

[0094] After the boundary point cloud mapping, the pose data of the underwater cleaning robot 100 at the end point position is recorded. During the cleaning operation of the underwater cleaning robot 100, the motion increment of the current period is calculated in combination with the IMU and the wheel speed meter to obtain the predicted pose information. By comparing the distance information predicted by the forward distance sensor 40 and / or the lateral distance sensor 50 at the current time and the actual sensed distance information, the observation error in each direction is obtained, and the predicted pose information is updated based on the observation error. After updating, the predicted pose information is updated to the actual pose information at the current time to ensure the accuracy of the positioning position of the underwater robot. By embedding the prediction-measurement-correction mechanism into the running cycle, a closed-loop estimation system is constructed, which not only reduces the dependence on a single sensor, but also enhances the adaptability of the system to sudden disturbances and environmental changes, and realizes completely autonomous positioning without deploying an underwater base station or auxiliary positioning point, thereby ensuring the accuracy of the positioning of the underwater cleaning robot.

[0095] The computer readable storage medium can be an internal storage of the self-moving device, such as a hard disk or a memory of the self-moving device. The computer readable storage medium can also be an external storage device of the self-moving device, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc.

[0096] In an embodiment of the present application, the computer readable storage medium can include a program storage area and a data storage area. The program storage area can store an operating system, application programs required by at least one function, etc. The data storage area can store data created according to the use of the self-moving device, etc.

[0097] In the above embodiments, the description of each embodiment has its own focus. The parts not described or recorded in detail in a certain embodiment can be referred to the relevant description of other embodiments.

[0098] Those skilled in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be realized by electronic hardware or a combination of computer software and electronic hardware. Whether the functions are realized in hardware or software depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods to realize the described functions for each specific application, but such implementation should not be considered beyond the scope of the present application.

[0099] In the embodiments provided by the present application, it should be understood that the disclosed apparatus / terminal device and method can be implemented in other manners. For example, the embodiments of the apparatus / terminal device described above are merely schematic, and the division of the modules or units is merely logical function division, and there can be another division manner in actual implementation. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed mutual couplings or direct couplings or communication connections between the units can be indirect couplings or communication connections through some interfaces, devices or units, and can be electrical, mechanical or in other forms.

[0100] The units described as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, i.e., can be located in one place, or can be distributed on a plurality of network units. Part or all of the units can be selected according to actual needs to achieve the purpose of the embodiments.

[0101] The above-described embodiments are merely used to illustrate the technical solutions of the present application, but not limit the present application; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: the technical solutions recorded in the foregoing embodiments can still be modified, or some technical features can be replaced by equivalent replacements; and these modifications or replacements do not make the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should be included in the protection scope of the present application.

Claims

1. An underwater positioning method applied to a self-moving device, characterized in that, The method comprises: constructing an environmental boundary map of an underwater operation area; when the environmental boundary map is constructed, acquiring motion information of the self-moving device at the end position of the environmental boundary map as initial pose information; during operation of the self-moving device, acquiring motion information increment of a current time period and the initial pose information, and determining predicted pose information of the self-moving device; based on the predicted pose information, determining predicted distance information of a distance sensor, and calculating observation error of distance information based on the predicted distance information and actual distance information sensed by the distance sensor; based on the observation error, determining actual pose information of the self-moving device.

2. The underwater positioning method of claim 1, wherein, The method comprises: controlling the self-moving device to move along the environmental boundary of the underwater operation area, and constructing an environmental boundary map of the underwater operation area based on displacement data sensed by a mileage sensor and distance data sensed by a lateral distance sensor, wherein the environmental boundary map comprises point cloud data of multiple positions of the environmental boundary.

3. The underwater positioning method of claim 1, wherein, The method comprises: based on the sum of the motion information increment and the initial pose information, obtaining the predicted pose information.

4. The underwater positioning method of claim 3, wherein, The method comprises: based on extrinsic parameters of the distance sensor, determining the distance between the distance sensor and the wheels of the self-moving device; based on the predicted pose information and the distance between the distance sensor and the wheels, calculating positioning information of the distance sensor; based on the positioning information of the distance sensor, determining minimum distance information within the sensing range of the distance sensor as the predicted distance information, and determining positioning information corresponding to the predicted distance information.

5. The underwater positioning method of claim 4, wherein, The method comprises: calculating the difference between the actual distance information and the predicted distance information to obtain the observation error.

6. The underwater positioning method of claim 5, wherein, The method comprises: based on the predicted distance information, calculating a Jacobian matrix of observation; based on preset process noise, preset observation noise, and the Jacobian matrix, calculating a Kalman gain parameter of a Kalman filtering algorithm; using the Kalman filtering algorithm to operate the observation error, the predicted pose information, and the Kalman gain parameter to obtain the actual pose information.

7. The underwater positioning method of claim 6, wherein, The method further comprises: updating the preset process noise of the Kalman filtering algorithm based on the Kalman gain parameter and the Jacobian matrix to obtain preset process noise for the next pose update.

8. The underwater positioning method according to any one of claims 1 to 7, characterized in that, The method further comprises: broadcasting the actual pose information to a shared message channel to publish the actual pose information.

9. A self-moving device, characterized in that The self-moving device comprises a memory, a processor, and a computer program stored on the memory and executable on the processor, and the processor executes the computer program to enable the self-moving device to implement the underwater positioning method according to any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that, The computer program is stored on the computer readable storage medium and executable on the processor in the self-moving device to implement the underwater positioning method according to any one of claims 1 to 8.

Images