A multi-modal inertial navigation method and system for a pool robot

Abstract

The application relates to the technical field of navigation and positioning, and discloses a multi-mode inertial navigation method and system for a swimming pool robot, which comprises the following steps: constructing a multi-sensor network of the swimming pool robot, collecting multi-mode data of the swimming pool robot in real time, constructing a three-dimensional environment map of a swimming pool of the swimming pool robot, determining an initial position, an underwater target position and a path coverage range of the swimming pool robot, calculating an initial underwater path of the swimming pool robot, calculating motion adjustment parameters and an attitude adjustment angle of the swimming pool robot, updating the initial underwater path, determining a water surface target position of the swimming pool robot corresponding to a water surface object, calculating a position distance between the initial position and the water surface target position, determining an initial water surface path of the swimming pool robot, analyzing an object flow path of the water surface object, adjusting the initial water surface path, obtaining a water surface service path, and generating a final service path of the swimming pool robot in an application scene. The application can improve the navigation accuracy of the swimming pool robot.

Description

TECHNICAL FIELD

[0001] The application relates to a multi-modal inertial navigation method and system of a swimming pool robot, and belongs to the technical field of navigation and positioning. BACKGROUND

[0002] Multi-modal inertial navigation refers to a high-precision and high-robustness autonomous navigation method realized by combining an inertial measurement unit (IMU) with other heterogeneous sensors (such as vision, sonar, depth gauge, GPS / Beidou, magnetometer, etc.) and a data fusion algorithm. Multi-modal inertial navigation overcomes the limitations of a single sensor to adapt to complex underwater environments (such as dynamic water flow, low light, turbid water quality).

[0003] A traditional navigation method of a swimming pool robot is infrared / ultrasonic obstacle avoidance navigation, which is performed by a pre-edited working path and an infrared / ultrasonic sensor. This method can only detect nearby obstacles and make simple obstacle avoidance adjustments, and cannot comprehensively perceive the three-dimensional structure of a swimming pool and long-distance obstacles, so as to realize accurate path planning and autonomous navigation, and it is difficult to realize efficient and comprehensive service. Therefore, a solution is urgently needed to improve the navigation accuracy of a swimming pool robot. SUMMARY

[0004] The application provides a multi-modal inertial navigation method and system of a swimming pool robot, which mainly aims to improve the navigation accuracy of a swimming pool robot.

[0005] To achieve the above-mentioned purpose, the application provides a multi-modal inertial navigation method of a swimming pool robot, which comprises the following steps:

[0006] The application scenario of a swimming pool robot is determined, a multi-sensor network of the swimming pool robot is constructed based on the application scenario, and multi-modal data of the swimming pool robot is collected in real time based on the multi-sensor network, wherein the multi-modal data comprises inertial measurement data, underwater sonar positioning data and vision sensor data.

[0007] The multi-modal data is fused to obtain fusion data, a swimming pool three-dimensional environment map of the swimming pool robot is constructed based on the fusion data, an initial position, an underwater target position and a path coverage range of the swimming pool robot are determined based on an underwater application scenario in the application scenario, and the initial position, the underwater target position and the path coverage range are mapped into the swimming pool three-dimensional environment map to calculate an initial underwater path of the swimming pool robot.

[0008] Based on the initial underwater path, obstacle data of the pool robot is collected in real time, based on the obstacle data, motion adjustment parameters and posture adjustment angles of the pool robot are calculated, based on the motion adjustment parameters and the posture adjustment angles, the initial underwater path is updated to obtain an underwater service path;

[0009] Based on the water application scenario in the application scenario, a water target position of the pool robot corresponding to a water object is determined, a position distance between the initial position and the water target position is calculated, and based on the position distance, an initial water path of the pool robot is determined;

[0010] The flow speed and flow direction of the water object are collected in real time, based on the flow speed and the flow direction, the object flow path of the water object is analyzed, and based on the object flow path, the initial water path is adjusted to obtain a water service path.

[0011] Optionally, based on the application scenario, a multi-sensor network of the pool robot is constructed, comprising:

[0012] Based on the application scenario, the multi-sensor of the pool robot is configured;

[0013] Determine the connection mode and installation layout between the multi-sensor, and determine the sensor network architecture of the multi-sensor according to the connection mode and the installation layout;

[0014] A communication network of the sensor network architecture is constructed;

[0015] Determine the sensor circuit and signal adjustment circuit of the sensor network architecture;

[0016] The multi-sensor, the communication network, the sensor circuit and the signal adjustment circuit are integrated into the sensor network architecture to obtain a multi-sensor network.

[0017] Optionally, the multi-modal data is fused to obtain fusion data, comprising:

[0018] The multi-modal data is denoised to obtain denoised multi-source data;

[0019] The time stamps of the denoised multi-source data are unified, and based on the time stamps, the denoised multi-source data is time-aligned to obtain aligned multi-source data;

[0020] A spatial coordinate system of the aligned multi-source data is constructed, and the aligned multi-source data is mapped into the spatial coordinate system to obtain registered multi-source data;

[0021] Extract a multi-source feature matrix of the registered multi-source data, perform feature fusion on the registered multi-source data based on the multi-source feature matrix, and obtain fused data.

[0022] Optionally, the initial position, the underwater target position, and the path coverage range of the pool robot are determined based on the underwater application scenario in the application scenario, including:

[0023] Collecting environmental data of the underwater application scenario, analyzing the relative position of the pool robot in the underwater application scenario based on the environmental data;

[0024] Matching the relative position and the corresponding pool three-dimensional environment map of the pool robot to obtain an initial position;

[0025] Analyzing the service demand of the pool robot, identifying a target area of the underwater application scenario based on the service demand;

[0026] Determining an underwater target position of the target area;

[0027] Calculating a target area volume of the target area, and determining a path coverage range of the pool robot according to the target area volume.

[0028] Optionally, the initial position, the underwater target position, and the path coverage range are mapped into the pool three-dimensional environment map to calculate an initial underwater path of the pool robot, including:

[0029] Dividing the pool three-dimensional environment map into a plurality of map areas;

[0030] Based on the initial position, the underwater target position, and the path coverage range, the plurality of map areas are divided into an initial position area, a target area, a coverage area, and a path planning area;

[0031] Calculating a robot passable probability of the path planning area;

[0032] According to the passable probability, the initial position area, the target area, the coverage area, and the path planning area are combined to obtain a path combination set;

[0033] Calculating a path length of each path in the path combination set;

[0034] According to the path length, an initial underwater path in the path combination set is selected.

[0035] Optionally, the motion adjustment parameter and the attitude adjustment angle of the pool robot are calculated based on the obstacle data, including:

[0036] determining a current position, a current speed and a current attitude of the pool robot, extracting an obstacle position and an obstacle speed of an obstacle corresponding to the obstacle data;

[0037] determining a safety radius of the pool robot;

[0038] calculating a motion adjustment parameter and an attitude adjustment angle of the pool robot according to the current position, the current speed, the current attitude, the obstacle position, the obstacle speed and the safety radius by using the following formula:

[0039]

[0040] wherein, the S represents the motion adjustment parameter, the θ represents the attitude adjustment angle, the S0 represents a current speed vector of the current speed, the Y represents a current position vector of the current position, the Z represents an obstacle position vector of the obstacle position, the R represents a safety radius vector of the safety radius, the S Z represents an obstacle speed vector of the obstacle speed, and the θ0 represents an attitude angle of the current attitude relative to a horizontal plane.

[0041] Optionally, the water target position of the pool robot corresponding to the water object is determined based on the water application scenario in the application scenario, including:

[0042] determining a water service demand of the pool robot based on the water application scenario;

[0043] collecting water object data of the water object;

[0044] identifying an object category of the water object according to the water object data;

[0045] labeling the water object based on the object category and the water service demand to obtain a water target object;

[0046] determining a water target position of the water target object.

[0047] Optionally, the initial water path of the pool robot is determined based on the position distance, including:

[0048] determining a path point sequence of the pool robot according to the position distance;

[0049] determining a water path set of the pool robot according to the path point sequence;

[0050] determining a path point quantity of the path point sequence and analyzing a turning energy consumption coefficient of the path point sequence;

[0051] According to the number of path points and the turning energy consumption coefficient, a path cost function of the pool robot is defined, wherein the path cost function comprises:

[0052]

[0053] wherein D(B) represents a path cost, n represents the number of path points, b i+1 represents a path point position vector of an i+1th path point in a path point sequence, b i represents a path point position vector of an ith path point in the path point sequence, μ represents the turning energy consumption coefficient, arctan represents an inverse tangent function, q i+1 represents a longitudinal coordinate of a path point position corresponding to the i+1th path point in the path point sequence, q i represents a longitudinal coordinate of a path point position corresponding to the ith path point in the path point sequence, q i-1 represents a longitudinal coordinate of a path point position corresponding to an i-1th path point in the path point sequence, p i+1 represents a transverse coordinate of a path point position corresponding to the i+1th path point in the path point sequence, p i represents a transverse coordinate of a path point position corresponding to the ith path point in the path point sequence, p i-1 represents a transverse coordinate of a path point position corresponding to the i-1th path point in the path point sequence.

[0054] According to the path cost function, a path cost of the set of water paths is calculated.

[0055] According to the path cost, an initial water path in the set of water paths is screened out.

[0056] Optionally, the analysis of the object flow path of the water object based on the flow speed and the flow direction comprises:

[0057] The analysis of the discrete time sequence positions of the water object at discrete time points based on the flow speed and the flow direction comprises:

[0058] The analysis of the environmental influence factor of the water object comprises:

[0059] The analysis of the influence parameter of the discrete time sequence positions based on the environmental influence factor comprises:

[0060] The optimization of the discrete time sequence positions according to the influence parameter comprises:

[0061] The curve fitting of the optimized discrete time sequence positions in chronological order comprises the object flow path.

[0062] To solve the above problems, the application further provides a multi-modal inertial navigation system of a pool robot, the system comprising:

[0063] A multi-modal data acquisition module is configured to determine an application scenario of the pool robot, construct a multi-sensor network of the pool robot based on the application scenario, and acquire multi-modal data of the pool robot in real time based on the multi-sensor network, wherein the multi-modal data comprises inertial measurement data, underwater sonar positioning data, and visual sensor data.

[0064] A three-dimensional map construction module is configured to fuse the multi-modal data to obtain fused data, construct a pool three-dimensional environment map of the pool robot based on the fused data, determine an initial position, an underwater target position, and a path coverage range of the pool robot based on an underwater application scenario in the application scenario, and map the initial position, the underwater target position, and the path coverage range to the pool three-dimensional environment map to calculate an initial underwater path of the pool robot.

[0065] An underwater path optimization module is configured to acquire obstacle data of the pool robot in real time based on the initial underwater path, calculate motion adjustment parameters and attitude adjustment angles of the pool robot based on the obstacle data, update the initial underwater path based on the motion adjustment parameters and the attitude adjustment angles to obtain an underwater service path.

[0066] An above-water path construction module is configured to determine an above-water target position of an above-water object corresponding to the pool robot based on an above-water application scenario in the application scenario, calculate a position distance between the initial position and the above-water target position, and determine an initial above-water path of the pool robot based on the position distance.

[0067] An above-water path optimization module is configured to acquire a flow speed and a flow direction of the above-water object in real time, analyze an object flow path of the above-water object based on the flow speed and the flow direction, and adjust the initial above-water path based on the object flow path to obtain an above-water service path.

[0068] A final service path generation module is configured to generate a final service path of the pool robot in the application scenario according to the underwater service path and the above-water service path.

[0069] Compared with the problems described in the background art, the embodiment of the application can more accurately perceive the environment of the swimming pool by collecting multi-modal data of the swimming pool robot in real time, providing a data basis for subsequent dynamic planning of a cleaning path and optimizing work efficiency. Optionally, the embodiment of the application can help the robot better adapt to the environment and plan a safe and efficient path by constructing a three-dimensional environment map of the swimming pool robot based on the fusion data. The embodiment of the application can help the robot better adapt to the environment and plan a safe and efficient path by constructing a three-dimensional environment map of the swimming pool robot based on the fusion data. The embodiment of the application can adjust the path in real time, effectively avoid obstacles, adapt to the changing underwater environment, and reduce the probability of being trapped by updating the initial underwater path based on the motion adjustment parameter and the attitude adjustment angle to obtain an underwater service path. The embodiment of the application can plan the shortest or optimal path from the initial position to the target position for the robot by determining the initial overwater path of the swimming pool robot based on the position distance, thereby reducing the travel distance and time consumption and improving work efficiency. Finally, the embodiment of the application can adapt to the dynamically changing overwater environment and always maintain the best service state by adjusting the initial overwater path based on the object flow path to obtain an overwater service path, thereby improving work efficiency. Therefore, the multi-modal inertial navigation method and system for the swimming pool robot provided by the embodiment of the application can improve the navigation accuracy of the swimming pool robot. BRIEF DESCRIPTION OF DRAWINGS

[0070] Figure 1 A flowchart of a multi-modal inertial navigation method for a swimming pool robot provided by an embodiment of the application is shown.

[0071] Figure 2 A module diagram for implementing the multi-modal inertial navigation method for the swimming pool robot provided by an embodiment of the application is shown.

[0072] The object implementation, functional features and advantages of the application will be further described with reference to the embodiments and the accompanying drawings. DETAILED DESCRIPTION

[0073] It should be understood that the specific embodiments described herein are only used to explain the application and do not limit the application.

[0074] The embodiment of the application provides a multi-modal inertial navigation method for a swimming pool robot. The execution subject of the multi-modal inertial navigation method for the swimming pool robot includes but is not limited to at least one of electronic devices capable of being configured to execute the method provided by the embodiment of the application, such as a server, a terminal, etc. In other words, the multi-modal inertial navigation method for the swimming pool robot can be executed by software or hardware installed in a terminal device or a server device. The server includes but is not limited to a single server, a server cluster, a cloud server or a cloud server cluster, etc.

[0075] Embodiment 1:

[0076] Referring to Figure 1 Fig. 1 shows a flowchart of a multi-modal inertial navigation method of a pool robot according to an embodiment of the present application. In this embodiment, the multi-modal inertial navigation method of the pool robot comprises the following steps:

[0077] S1, determining an application scenario of the pool robot, constructing a multi-sensor network of the pool robot based on the application scenario, and collecting multi-modal data of the pool robot in real time based on the multi-sensor network, wherein the multi-modal data comprises inertial measurement data, underwater sonar positioning data, and visual sensor data.

[0078] According to the application scenario of the pool robot, the embodiment of the present application can lay a foundation for subsequent different path planning for different work tasks. The application scenario refers to a specific environment and task in which the pool robot can exert its functions and effects, such as a family pool, a public pool, a water park, underwater photography, etc.

[0079] According to the application scenario, the embodiment of the present application can realize accurate navigation and positioning in the pool, and avoid collision and disorientation by constructing a multi-sensor network of the pool robot. The multi-sensor network refers to a network system composed of multiple different types of sensors for collaborative perception and monitoring of various information in a specific environment.

[0080] As an embodiment of the present application, constructing the multi-sensor network of the pool robot based on the application scenario comprises:

[0081] configuring the multi-sensor of the pool robot based on the application scenario;

[0082] determining a connection mode and an installation layout among the multi-sensor, and determining a sensor network architecture of the multi-sensor according to the connection mode and the installation layout;

[0083] constructing a communication network of the sensor network architecture;

[0084] determining a sensor circuit and a signal adjustment circuit of the sensor network architecture;

[0085] integrating the multi-sensor, the communication network, the sensor circuit, and the signal adjustment circuit into the sensor network architecture to obtain a multi-sensor network.

[0086] The multi-sensor refers to various types of sensors equipped on the pool robot for sensing and collecting various information in the pool environment, such as an inertial measurement unit, an underwater sonar positioning system, a visual sensor, etc. The connection mode refers to the communication and data transmission mode between sensors and sensors, between sensors and controllers, and between controllers and external systems (such as user interfaces, remote monitoring platforms, etc.), such as wired connection, wireless connection, hybrid connection, etc. The installation layout refers to the physical installation position and arrangement of components such as sensors and controllers on the robot body. The sensor network architecture refers to the organization structure and interaction mode between sensors, controllers and communication module devices. The communication network refers to the network used for transmitting data between various sensors, controllers and external systems on the robot. The sensor circuit refers to an electronic circuit for connecting, controlling and processing electrical signals from various sensors. The signal adjustment circuit refers to an electronic circuit for processing and optimizing sensor output signals.

[0087] Optionally, the sensor network architecture can be determined by a topology optimization algorithm, such as a genetic algorithm, simulated annealing, ant colony algorithm, etc.

[0088] Optionally, the communication network can be constructed by a low-power wide-area network technology, such as NB-IoT, LoRaWAN, etc.

[0089] The embodiment of the application can more accurately perceive the environment of the pool by collecting multi-modal data of the pool robot in real time, providing a data basis for subsequent dynamic planning of cleaning paths and optimizing work efficiency. The multi-modal data refers to a collection of data collected by various types of sensors. The inertial measurement data refers to data collected by an inertial measurement unit, such as linear acceleration and deceleration information, rotation rate, magnetic force data, etc. The underwater sonar positioning data refers to data collected by an underwater sonar system, such as distance measurement data, environment mapping data, etc. The visual sensor data refers to image or video data collected by a visual sensor (such as a camera) carried by the pool robot.

[0090] S2, fuse the multi-modal data to obtain fused data, construct a three-dimensional pool environment map of the pool robot based on the fused data, determine an initial position, an underwater target position and a path coverage range of the pool robot based on the underwater application scenario in the application scenario, map the initial position, the underwater target position and the path coverage range to the three-dimensional pool environment map to calculate an initial underwater path of the pool robot.

[0091] The embodiment of the present application can obtain fusion data by fusing the multi-modal data, so as to overcome the limitation of a single mode and provide more comprehensive and accurate environmental perception. The fusion data refers to the result obtained by integrating and processing data from different sensor modalities.

[0092] As an embodiment of the present application, the fusing the multi-modal data to obtain fusion data comprises:

[0093] de-noising the multi-modal data to obtain de-noised multi-source data;

[0094] unifying the time stamps of the de-noised multi-source data, and time-aligning the de-noised multi-source data based on the time stamps to obtain aligned multi-source data;

[0095] constructing a spatial coordinate system of the aligned multi-source data, mapping the aligned multi-source data into the spatial coordinate system to obtain registered multi-source data;

[0096] extracting a multi-source feature matrix of the registered multi-source data, and fusing features of the registered multi-source data based on the multi-source feature matrix to obtain fusion data.

[0097] The de-noised multi-source data refers to a data set from multiple different sensor modalities after de-noising processing. The time stamp refers to accurate time information marked for a data sample, which is used to record the specific time of data acquisition and generation. The aligned multi-source data refers to data obtained by synchronizing and integrating the de-noised multi-source data in time. The spatial coordinate system refers to a reference frame used to describe and represent the position and relationship of the aligned multi-source data in space. The registered multi-source data refers to a data set from different sensor modalities after spatial registration processing. The multi-source feature matrix refers to a matrix set used to represent data features after feature extraction of the registered multi-source data.

[0098] Optionally, the de-noised multi-source data can be obtained by a filtering de-noising algorithm, such as a low-pass filtering algorithm, a median filtering algorithm, a bilateral filtering algorithm, etc.

[0099] Optionally, the registered multi-source data can be registered by a multi-modal registration network, such as constructing a multi-modal registration network of the aligned multi-source data, analyzing the corresponding relationship of the aligned multi-source data through the multi-modal registration network, and mapping the aligned multi-source data into the spatial coordinate system according to the corresponding relationship to obtain registered multi-source data.

[0100] The embodiment of the application can help the robot to better adapt to the environment and plan a safe and efficient path by constructing a pool three-dimensional environment map of the pool robot based on the fusion data. The pool three-dimensional environment map refers to a three-dimensional digital model that can comprehensively and accurately describe the internal and peripheral environment of the pool, which is constructed by fusing various sensor data (such as inertial measurement data, underwater sonar positioning data, and visual sensor data).

[0101] Optionally, the pool three-dimensional environment map can be constructed by a three-dimensional reconstruction algorithm, such as a point-based reconstruction algorithm, a grid-based reconstruction algorithm, etc.

[0102] The embodiment of the application can provide accurate starting point information and target information for the robot by determining the initial position, underwater target position, and path coverage range of the pool robot based on the underwater application scenario in the application scenario, which is the basis for subsequent path planning and navigation. The initial position refers to the position of the pool robot when it starts to perform a task. The underwater target position refers to the underwater location that the robot needs to reach or perform a specific task. The path coverage range refers to the range of water area that the robot can reach and perform a task.

[0103] As an embodiment of the application, the determination of the initial position, underwater target position, and path coverage range of the pool robot based on the underwater application scenario in the application scenario comprises:

[0104] Collecting environmental data of the underwater application scenario, analyzing the relative position of the pool robot in the underwater application scenario based on the environmental data;

[0105] Matching the relative position and the corresponding pool three-dimensional environment map of the pool robot to obtain the initial position;

[0106] Analyzing the service demand of the pool robot, identifying the target area of the underwater application scenario based on the service demand;

[0107] Determining the underwater target position of the target area;

[0108] Calculating the target area volume of the target area, and determining the path coverage range of the pool robot according to the target area volume.

[0109] The environmental data refers to information about the underwater environment of the swimming pool collected by a sensor network carried by the robot, such as the shape, size, material of the pool wall and pool bottom, and the inclination angle of the swimming pool. The relative position refers to the position of the swimming pool robot relative to a reference point or a reference object (such as a swimming pool wall) in the swimming pool environment. The service requirement refers to the service content and function expected by the user for the swimming pool robot. The target area refers to a specific water area where the robot needs to perform a specific task or service, such as the bottom of the swimming pool, the pool wall, etc. The target area volume refers to the size of the space occupied by the target area.

[0110] Optionally, the relative position of the swimming pool robot in the underwater application scene can be analyzed by an environmental feature matching algorithm, such as extracting environmental features of the environmental data, matching the environmental features with scene features in the underwater application scene, obtaining a feature matching coefficient, and determining the relative position of the swimming pool robot based on the matching coefficient.

[0111] Optionally, the service requirement can be obtained by obtaining an execution instruction received by the swimming pool robot, and analyzing the execution instruction to obtain the service requirement of the swimming pool robot.

[0112] The initial position, the underwater target position and the path coverage range are mapped into the three-dimensional environmental map of the swimming pool to calculate the initial underwater path of the swimming pool robot, which can accurately plan the path from the starting point to the ending point, avoid unnecessary repetition or omission, and reduce navigation error. The initial underwater path refers to the first motion trajectory covering the swimming pool area planned according to the constructed three-dimensional environmental map.

[0113] As an embodiment of the present application, the initial position, the underwater target position and the path coverage range are mapped into the three-dimensional environmental map of the swimming pool to calculate the initial underwater path of the swimming pool robot, which includes:

[0114] The three-dimensional environmental map of the swimming pool is equally divided into multiple map areas;

[0115] Based on the initial position, the underwater target position and the path coverage range, the multiple map areas are divided into an initial position area, a target area, a coverage area and a path planning area;

[0116] The robot passable probability of the path planning area is calculated;

[0117] According to the passable probability, the initial position area, the target area, the coverage area and the path planning area are combined to obtain a path combination set;

[0118] calculating a path length of each path in the path combination set;

[0119] screening an initial underwater path in the path combination set according to the path length.

[0120] The multi-map area refers to a three-dimensional environment map of the entire swimming pool divided into multiple smaller and regular areas. The initial position area refers to an area containing the initial position in the multi-map area. The target area refers to an area containing the underwater target position in the multi-map area. The coverage area refers to an area containing the path coverage range in the multi-map area. The path planning area refers to a remaining area in the multi-map area for planning a moving path. The robot passable probability refers to a possibility size of the path planning area being passable for the swimming pool robot. The path combination set refers to a set composed of multiple possible paths, each of which represents a potential route from the initial position to the target position. The path length refers to a total length of a path passed by the robot from the initial position to the target position.

[0121] Optionally, the robot passable probability can be calculated by identifying a passable range and an obstacle range of each area in the path planning area, calculating a range ratio of the passable range and the obstacle range, and determining the robot passable probability of the path planning area according to the range ratio.

[0122] Optionally, the path combination set has three paths of A, B and C, and the path lengths of the paths of A, B and C are 200 meters, 300 meters and 400 meters respectively. The path A with the shortest path length is selected as the initial underwater path of the swimming pool robot.

[0123] S3, based on the initial underwater path, collecting obstacle data of the swimming pool robot in real time, calculating a motion adjustment parameter and a posture adjustment angle of the swimming pool robot based on the obstacle data, updating the initial underwater path based on the motion adjustment parameter and the posture adjustment angle, and obtaining an underwater service path.

[0124] The embodiment of the present application can detect the obstacles in front or around in real time and respond quickly to avoid collision by collecting the obstacle data of the swimming pool robot in real time based on the initial underwater path. The obstacle data refers to information about objects in the surrounding environment that may interfere with the motion or task execution of the robot collected by the sensor carried by the robot, such as obstacle size and obstacle motion data.

[0125] The embodiment of the present application can help the robot to select the optimal obstacle avoidance path and reduce unnecessary detours by calculating the motion adjustment parameters and the posture adjustment angle of the pool robot based on the obstacle data, thereby improving the efficiency of task execution. The motion adjustment parameters refer to a series of parameters for controlling the motion behavior of the robot, such as angular velocity, linear velocity, acceleration, etc. The posture adjustment angle refers to the angle of rotation required for the robot to adjust its current posture to adapt to environmental changes or perform specific tasks relative to its original posture or desired posture.

[0126] As an embodiment of the present application, the calculation of the motion adjustment parameters and the posture adjustment angle of the pool robot based on the obstacle data comprises:

[0127] determining the current position, current speed and current posture of the pool robot, and extracting the obstacle position and obstacle speed of the obstacle corresponding to the obstacle data;

[0128] determining the safety radius of the pool robot;

[0129] According to the current position, the current speed, the current posture, the obstacle position, the obstacle speed and the safety radius, the motion adjustment parameters and the posture adjustment angle of the pool robot are calculated by the following formula:

[0130]

[0131] wherein S represents the motion adjustment parameter, θ represents the posture adjustment angle, S0 represents the current speed vector of the current speed, Y represents the current position vector of the current position, Z represents the obstacle position vector of the obstacle position, R represents the safety radius vector of the safety radius, S Z represents the obstacle speed vector of the obstacle speed, and θ0 represents the posture angle of the current posture relative to the horizontal plane.

[0132] The current position refers to the specific position coordinates of the pool robot in the three-dimensional space at a specific time. The current speed refers to the motion speed of the pool robot at a specific time, including the size and direction of the speed. The current posture refers to the orientation or posture of the pool robot in the three-dimensional space at a specific time. The obstacle position refers to the specific position coordinates of the obstacle detected by the pool robot in its working environment. The obstacle speed refers to the motion speed of the obstacle detected by the pool robot in its working environment. The safety radius refers to a safety distance set by the pool robot to ensure its safety and avoid collision with surrounding obstacles when performing tasks.

[0133] The embodiment of the application can update the initial underwater path based on the motion adjustment parameter and the posture adjustment angle to obtain an underwater service path, which can adjust the path in real time, effectively avoid obstacles, adapt to changing underwater environments, and reduce the probability of being trapped. The underwater service path is the best path obtained by updating based on the initial underwater path and combining real-time collected obstacle data, the current position, speed, and posture of the robot.

[0134] S4, based on the water application scenario in the application scenario, determining a water target position of the water object corresponding to the pool robot, calculating a position distance between the initial position and the water target position, and determining an initial water path of the pool robot based on the position distance.

[0135] The embodiment of the application can identify and avoid water obstacles, accurately navigate to the target area, and avoid blind coverage by determining the water target position of the water object corresponding to the pool robot based on the water application scenario in the application scenario. The water target position refers to a specific position point that the pool robot needs to reach or operate when performing a water task.

[0136] As an embodiment of the application, determining the water target position of the water object corresponding to the pool robot based on the water application scenario in the application scenario comprises:

[0137] Determining a water service demand of the pool robot based on the water application scenario;

[0138] Collecting water object data of the water object;

[0139] Identifying an object category of the water object according to the water object data;

[0140] Labeling the water object based on the object category and the water service demand to obtain a water target object;

[0141] Determining a water target position of the water target object.

[0142] The water service demand refers to a specific task that the pool robot needs to perform in the water environment, such as cleaning water leaves, transporting fruits, etc. The water object data refers to various information about the water object collected by the sensor carried by the pool robot in the water. The object category refers to the classification of the water object identified based on the water object data, such as water garbage (leaves, plastic bottles, etc.), lifesaving equipment (swimming rings, life rods, etc.), swimming aids, etc. The water target object refers to a specific water object that the pool robot needs to identify, locate, approach, operate, or avoid when performing a specific task.

[0143] Optionally, the object category can be identified by a target detection algorithm, such as YOLO, SSD, Faster R-CNN, etc.

[0144] The embodiment of the application can calculate the position distance between the initial position and the water target position to plan an optimal path for the pool robot from the initial position to the water target position, avoiding unnecessary detours and energy waste. Wherein, the position distance refers to the straight-line distance between the current position of the robot and the target cleaning position.

[0145] Optionally, the position distance can be obtained by mapping the initial position and the water target position to a two-dimensional coordinate system to obtain the initial position coordinates and the target position coordinates, calculating the coordinate difference between the initial position coordinates and the target position coordinates, and determining the position distance between the initial position and the water target position based on the coordinate difference.

[0146] The embodiment of the application can determine the initial water path of the pool robot based on the position distance to plan the shortest or optimal path for the robot from the initial position to the target position, reduce the travel distance and time consumption, and improve the work efficiency. Wherein, the initial water path refers to a path from the initial position to the target position that is initially planned by the pool robot when starting to perform a task, according to the position distance calculated between the initial position and the water target position, and considering the application scenario, environmental factors and its own performance (such as speed, maneuverability, etc.).

[0147] As an embodiment of the application, the determination of the initial water path of the pool robot based on the position distance comprises:

[0148] determining a path point sequence of the pool robot according to the position distance;

[0149] determining a water path set of the pool robot according to the path point sequence;

[0150] determining the number of path points of the path point sequence and analyzing the turning energy consumption coefficient of the path point sequence;

[0151] defining a path cost function of the pool robot according to the number of path points and the turning energy consumption coefficient, wherein the path cost function comprises:

[0152]

[0153] wherein D(B) represents the path cost, n represents the number of path points, b i+1 represents the path point position vector of the i+1th path point in the path point sequence, b ia path point position vector representing the i-th path point in the path point sequence, μ represents a turning energy consumption coefficient, arctan represents an inverse tangent function, q i+1 a vertical coordinate of a path point position corresponding to the i+1-th path point in the path point sequence, q i a vertical coordinate of a path point position corresponding to the i-th path point in the path point sequence, q i-1 a vertical coordinate of a path point position corresponding to the i-1-th path point in the path point sequence, p i+1 a horizontal coordinate of a path point position corresponding to the i+1-th path point in the path point sequence, p i a horizontal coordinate of a path point position corresponding to the i-th path point in the path point sequence, p i-1 a horizontal coordinate of a path point position corresponding to the i-1-th path point in the path point sequence;

[0154] calculating path costs of the set of water paths according to the path cost function;

[0155] screening an initial water path in the set of water paths according to the path cost.

[0156] The path point sequence refers to a series of ordered path points defining a path to be followed by the pool robot from an initial position to a target position. The set of water paths refers to a set composed of multiple different water paths. The turning energy consumption coefficient refers to a ratio of energy consumed by the pool robot when performing a turning operation to energy consumed by the pool robot when performing a straight-line travel for the same distance. The path cost function refers to a mathematical function used to evaluate the pros and cons of different paths. The path cost refers to a total cost paid by the pool robot when moving from an initial position to a target position along a certain path.

[0157] Optionally, the turning energy consumption coefficient can be analyzed by a machine learning algorithm, such as constructing a relationship model between turning actions of the pool robot and energy consumption by a machine learning algorithm, and analyzing the turning energy consumption coefficient of the path point sequence according to the relationship model.

[0158] For example, there are three paths A, B and C in the set of water paths, and the path costs of A, B and C are 70, 120 and 65 respectively after calculation by the path cost function, then path C is selected as the initial water path of the pool robot.

[0159] S5, collecting the flow speed and flow direction of the water object in real time, analyzing an object flow path of the water object based on the flow speed and the flow direction, adjusting the initial water path based on the object flow path to obtain a water service path.

[0160] The embodiment of the present application can dynamically adjust the driving path of the water object by collecting the flow speed and flow direction of the water object in real time, so as to avoid collision with the flowing object or hinder the normal work. The flow speed refers to the speed of the water object moving in the water body. The flow direction refers to the direction of the water object moving in the water body.

[0161] Optionally, the flow speed and the flow direction can be collected by sonar technology, such as emitting sound waves to the water object in real time by sonar technology, receiving reflected sound waves of the water object based on the emitted sound waves, analyzing the frequency and phase change value of the reflected sound waves, and determining the flow speed and flow direction of the water object based on the frequency and the phase change value.

[0162] The embodiment of the present application can help the pool robot to analyze the position of the obstacle by analyzing the object flow path of the water object based on the flow speed and the flow direction, so as to plan the path in advance, avoid collision, and better adapt to the dynamically changing water environment. The object flow path refers to the motion trajectory of the water object under the action of the water flow.

[0163] As an embodiment of the present application, the analysis of the object flow path of the water object based on the flow speed and the flow direction includes:

[0164] Analyzing the discrete time sequence positions of the water object at discrete time points based on the flow speed and the flow direction;

[0165] Analyzing the environmental influence factors of the water object;

[0166] Analyzing the influence parameters of the discrete time sequence positions according to the environmental influence factors;

[0167] Optimizing the discrete time sequence positions according to the influence parameters to obtain optimized discrete time sequence positions;

[0168] Curve fitting the optimized discrete time sequence positions in time sequence to obtain the object flow path.

[0169] The discrete time sequence positions refer to the set of position coordinates of the water object at a series of specific time points. The environmental influence factors refer to various environmental factors that can affect the flow speed, flow direction and final flow path of the water object, such as wind speed, wind direction, water flow speed, water flow direction, etc. The influence parameters refer to the influence values of the environmental influence factors on the discrete time sequence positions of the pool robot. The optimized discrete time sequence positions refer to the positions of the object at each discrete time point after the original calculated positions are corrected and improved.

[0170] Optionally, the discrete time sequence positions can be analyzed by machine learning and deep learning algorithms, such as recurrent neural networks, long short-term memory networks, etc.

[0171] Optionally, the environmental impact factors can be analyzed by multi-factor analysis algorithms, such as multiple linear regression, logistic regression, Poisson regression, etc.

[0172] Optionally, the impact parameters can be analyzed by breakpoint design algorithms, such as evaluating the causal effect of the environmental impact factors by the breakpoint design algorithm, and determining the impact parameters of the discrete time sequence positions according to the causal effect.

[0173] The embodiment of the present application can adapt to the dynamically changing water environment by adjusting the initial water path based on the object flow path to obtain the water service path, and always maintain the best service state to improve work efficiency. The water service path refers to the route of the pool robot moving above or near the water surface when performing tasks.

[0174] S6, according to the underwater service path and the water service path, generating the final service path of the pool robot in the application scene.

[0175] The embodiment of the present application can ensure that the service work of the pool robot covers every corner of the pool, optimize the moving path of the robot, reduce repeated coverage and unnecessary movement, thereby improving work efficiency, saving time and energy, by generating the final service path of the pool robot in the application scene according to the underwater service path and the water service path.

[0176] Compared with the problems described in the background art, the embodiment of the application can more accurately perceive the environment of the swimming pool by collecting multi-modal data of the swimming pool robot in real time, providing a data basis for subsequent dynamic planning of a cleaning path and optimizing work efficiency. Optionally, the embodiment of the application can help the robot better adapt to the environment and plan a safe and efficient path by constructing a three-dimensional environment map of the swimming pool of the swimming pool robot based on the fusion data. The embodiment of the application can help the robot better adapt to the environment and plan a safe and efficient path by constructing a three-dimensional environment map of the swimming pool of the swimming pool robot based on the fusion data. The embodiment of the application can adjust the path in real time, effectively avoid obstacles, adapt to the changing underwater environment, and reduce the probability of being trapped by updating the initial underwater path based on the motion adjustment parameter and the attitude adjustment angle to obtain an underwater service path. The embodiment of the application can plan the shortest or optimal path from the initial position to the target position for the robot by determining the initial overwater path of the swimming pool robot based on the position distance, thereby reducing the travel distance and time consumption and improving the work efficiency. Finally, the embodiment of the application can adapt to the dynamically changing overwater environment and always maintain the best service state by adjusting the initial overwater path based on the object flow path to obtain an overwater service path, thereby improving the work efficiency. Therefore, the multi-modal inertial navigation method and system for the swimming pool robot provided by the embodiment of the application can improve the navigation accuracy of the swimming pool robot.

[0177] Embodiment 2:

[0178] As Figure 2 shown, it is a functional module diagram of a multi-modal inertial navigation system for a swimming pool robot.

[0179] The multi-modal inertial navigation system 200 for the swimming pool robot can be installed in an electronic device. According to the functions to be implemented, the multi-modal inertial navigation system for the swimming pool robot can include a multi-modal data acquisition module 201, a three-dimensional map construction module 202, an underwater path optimization module 203, an overwater path construction module 204, an overwater path optimization module 205, and a final service path generation module 206. The modules of the application can also be referred to as units, which refer to a series of computer program segments that can be executed by an electronic device processor and can complete a fixed function, and are stored in the memory of the electronic device.

[0180] In the embodiment of the application, the functions of each module / unit are as follows:

[0181] The multi-modal data acquisition module 201 is configured to determine an application scenario of the pool robot, construct a multi-sensor network of the pool robot based on the application scenario, and acquire multi-modal data of the pool robot in real time based on the multi-sensor network, wherein the multi-modal data includes inertial measurement data, underwater sonar positioning data, and visual sensor data.

[0182] The three-dimensional map construction module 202 is configured to fuse the multi-modal data to obtain fused data, construct a pool three-dimensional environment map of the pool robot based on the fused data, determine an initial position, an underwater target position, and a path coverage range of the pool robot based on an underwater application scenario in the application scenario, and map the initial position, the underwater target position, and the path coverage range to the pool three-dimensional environment map to calculate an initial underwater path of the pool robot.

[0183] The underwater path optimization module 203 is configured to acquire obstacle data of the pool robot in real time based on the initial underwater path, calculate a motion adjustment parameter and an attitude adjustment angle of the pool robot based on the obstacle data, update the initial underwater path based on the motion adjustment parameter and the attitude adjustment angle to obtain an underwater service path.

[0184] The above-water path construction module 204 is configured to determine an above-water target position of a corresponding above-water object of the pool robot based on an above-water application scenario in the application scenario, calculate a position distance between the initial position and the above-water target position, and determine an initial above-water path of the pool robot based on the position distance.

[0185] The above-water path optimization module 205 is configured to acquire a flow speed and a flow direction of the above-water object in real time, analyze an object flow path of the above-water object based on the flow speed and the flow direction, and adjust the initial above-water path based on the object flow path to obtain an above-water service path.

[0186] The final service path generation module 206 is configured to generate a final service path of the pool robot in the application scenario according to the underwater service path and the above-water service path.

[0187] In detail, the modules in the multi-modal inertial navigation system 200 of the pool robot in the embodiment of the present application use the same technical means as the multi-modal inertial navigation method of the pool robot in the above-mentioned Figure 1 and can produce the same technical effects, which will not be described here again.

[0188] It is apparent for a person skilled in the art that the present application is not limited to the details of the above exemplary embodiments, but can be implemented in other concrete forms without departing from the spirit or essential characteristics of the present application.

[0189] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application but not limit the present application, and although the present application has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present application can be modified or equivalently replaced without departing from the spirit and scope of the present application.

Claims

1. A multi-modal inertial navigation method for a pool robot, characterized by, The method comprises: The application scene of the pool robot is determined, a multi-sensor network of the pool robot is constructed based on the application scene, and multi-modal data of the pool robot is collected in real time based on the multi-sensor network, wherein the multi-modal data comprises inertial measurement data, underwater sonar positioning data and visual sensor data; The multi-modal data is fused to obtain fused data, a pool three-dimensional environment map of the pool robot is constructed based on the fused data, an initial position, an underwater target position and a path coverage range of the pool robot are determined based on an underwater application scene in the application scene, and the initial position, the underwater target position and the path coverage range are mapped into the pool three-dimensional environment map to calculate an initial underwater path of the pool robot; Obstacle data of the pool robot is collected in real time based on the initial underwater path, motion adjustment parameters and an attitude adjustment angle of the pool robot are calculated based on the obstacle data, the initial underwater path is updated based on the motion adjustment parameters and the attitude adjustment angle to obtain an underwater service path; An overwater target position of a corresponding overwater object of the pool robot is determined based on an overwater application scene in the application scene, a position distance between the initial position and the overwater target position is calculated, and an initial overwater path of the pool robot is determined based on the position distance; The flow speed and the flow direction of the overwater object are collected in real time, an object flow path of the overwater object is analyzed based on the flow speed and the flow direction, the initial overwater path is adjusted based on the object flow path to obtain an overwater service path; A final service path of the pool robot in the application scene is generated according to the underwater service path and the overwater service path.

2. The multi-modal inertial navigation method of a pool robot of claim 1, wherein, The multi-sensor network of the pool robot is constructed based on the application scene, comprising: The multi-sensor of the pool robot is configured based on the application scene; The connection mode and the installation layout among the multi-sensor are determined, and the sensor network architecture of the multi-sensor is determined according to the connection mode and the installation layout; A communication network of the sensor network architecture is constructed; A sensor circuit and a signal adjustment circuit of the sensor network architecture are determined; The multi-sensor, the communication network, the sensor circuit and the signal adjustment circuit are integrated into the sensor network architecture to obtain a multi-sensor network.

3. The multi-modal inertial navigation method of a pool robot of claim 1, wherein, The multi-modal data is fused to obtain fused data, comprising: The multi-modal data is denoised to obtain denoised multi-source data; The timestamps of the denoised multi-source data are unified, the denoised multi-source data are time-aligned based on the timestamps to obtain aligned multi-source data; A spatial coordinate system of the aligned multi-source data is constructed, the aligned multi-source data are mapped into the spatial coordinate system to obtain registered multi-source data; A multi-source feature matrix of the registered multi-source data is extracted, the registered multi-source data are feature-fused based on the multi-source feature matrix to obtain fused data.

4. The multi-modal inertial navigation method of a pool robot of claim 1, wherein, The initial position, the underwater target position, and the path coverage range of the pool robot are determined based on the underwater application scenario in the application scenario, including: environmental data of the underwater application scenario is collected, and a relative position of the pool robot in the underwater application scenario is analyzed based on the environmental data; the relative position is matched with a corresponding pool three-dimensional environment map of the pool robot to obtain an initial position; service demand of the pool robot is analyzed, and a target area of the underwater application scenario is identified based on the service demand; an underwater target position of the target area is determined; a target area volume of the target area is calculated, and a path coverage range of the pool robot is determined according to the target area volume.

5. The multi-modal inertial navigation method of a pool robot of claim 1, wherein, The initial position, the underwater target position, and the path coverage range are mapped into the pool three-dimensional environment map to calculate an initial underwater path of the pool robot, including: The pool three-dimensional environment map is equally divided into a plurality of map areas; The plurality of map areas are divided into an initial position area, a target area, a coverage area, and a path planning area based on the initial position, the underwater target position, and the path coverage range; A robot passable probability of the path planning area is calculated; The initial position area, the target area, the coverage area, and the path planning area are combined according to the passable probability to obtain a path combination set; A path length of each path in the path combination set is calculated; An initial underwater path in the path combination set is selected according to the path length.

6. The multi-modal inertial navigation method of a pool robot of claim 1, wherein, The motion adjustment parameter and the attitude adjustment angle of the pool robot are calculated based on the obstacle data, including: A current position, a current speed, and a current attitude of the pool robot are determined, and an obstacle position and an obstacle speed of an obstacle corresponding to the obstacle data are extracted; A safety radius of the pool robot is determined; The motion adjustment parameter and the attitude adjustment angle of the pool robot are calculated by using the following formula according to the current position, the current speed, the current attitude, the obstacle position, the obstacle speed, and the safety radius: wherein S represents a motion adjustment parameter, θ represents an attitude adjustment angle, S0 represents a current speed vector of a current speed, Y represents a current position vector of a current position, Z represents an obstacle position vector of an obstacle position, R represents a safety radius vector of a safety radius, S Z represents an obstacle speed vector of an obstacle speed, and θ0 represents an attitude angle of a current attitude with respect to a horizontal plane.

7. The multi-modal inertial navigation method of a pool robot of claim 1, wherein, The underwater target position of the pool robot corresponding to the water object is determined based on the water application scenario in the application scenario, including: The water service demand of the pool robot is determined based on the water application scenario; Water object data of the water object is collected; An object category of the water object is identified according to the water object data; The water object is marked based on the object category and the water service demand to obtain a water target object; The underwater target position of the water target object is determined.

8. The multi-modal inertial navigation method of a pool robot of claim 1, wherein, The initial water path of the pool robot is determined based on the position distance, including: A path point sequence of the pool robot is determined according to the position distance; A water path set of the pool robot is determined according to the path point sequence; A path point quantity of the path point sequence is determined, and a turning energy consumption coefficient of the path point sequence is analyzed; According to the path point quantity and the turning energy consumption coefficient, a path cost function of the pool robot is defined, wherein the path cost function comprises: wherein D(B) represents a path cost, n represents a path point number, b i+1 represents a path point position vector of an i+1th path point in a path point sequence, b i represents a path point position vector of an ith path point in a path point sequence, μ represents a turning energy consumption coefficient, arctan represents an inverse tangent function, q i+1 represents a vertical coordinate of a path point position corresponding to an i+1th path point in a path point sequence, q i represents a vertical coordinate of a path point position corresponding to an ith path point in a path point sequence, q i-1 represents a vertical coordinate of a path point position corresponding to an i-1th path point in a path point sequence, p i+1 represents a horizontal coordinate of a path point position corresponding to an i+1th path point in a path point sequence, p i represents a horizontal coordinate of a path point position corresponding to an ith path point in a path point sequence, p i-1 represents a horizontal coordinate of a path point position corresponding to an i-1th path point in a path point sequence; According to the path cost function, a path cost of the set of water surface paths is calculated; According to the path cost, an initial water surface path in the set of water surface paths is screened out.

9. The multi-modal inertial navigation method of a pool robot of claim 1, wherein, According to the flow speed and the flow direction, the object flow path of the water surface object is analyzed, comprising: According to the flow speed and the flow direction, the discrete time sequence positions of the water surface object at discrete time points are analyzed; An environmental influence factor of the water surface object is analyzed; According to the environmental influence factor, an influence parameter of the discrete time sequence positions is analyzed; According to the influence parameter, the discrete time sequence positions are optimized to obtain optimized discrete time sequence positions; The optimized discrete time sequence positions are curve fitted in time sequence to obtain the object flow path.

10. A multi-modal inertial navigation system for a pool robot, characterized in that, The system comprises: A multi-modal data acquisition module is configured to determine an application scenario of a pool robot, construct a multi-sensor network of the pool robot based on the application scenario, and acquire multi-modal data of the pool robot in real time based on the multi-sensor network, wherein the multi-modal data comprises inertial measurement data, underwater sonar positioning data, and visual sensor data; A three-dimensional map construction module is configured to fuse the multi-modal data to obtain fused data, construct a pool three-dimensional environment map of the pool robot based on the fused data, determine an initial position, an underwater target position, and a path coverage range of the pool robot based on an underwater application scenario in the application scenario, and map the initial position, the underwater target position, and the path coverage range to the pool three-dimensional environment map to calculate an initial underwater path of the pool robot; An underwater path optimization module is configured to acquire obstacle data of the pool robot in real time based on the initial underwater path, calculate a motion adjustment parameter and an attitude adjustment angle of the pool robot based on the obstacle data, and update the initial underwater path based on the motion adjustment parameter and the attitude adjustment angle to obtain an underwater service path; An above-water path construction module is configured to determine an above-water target position of an above-water object corresponding to the pool robot based on an above-water application scenario in the application scenario, calculate a position distance between the initial position and the above-water target position, and determine an initial above-water path of the pool robot based on the position distance; An above-water path optimization module is configured to acquire a flow speed and a flow direction of the above-water object in real time, analyze an object flow path of the above-water object based on the flow speed and the flow direction, and adjust the initial above-water path based on the object flow path to obtain an above-water service path; A final service path generation module is configured to generate a final service path of the pool robot in the application scenario based on the underwater service path and the above-water service path.

Images