Navigation system and method of photovoltaic panel cleaning robot

Abstract

The invention discloses a navigation system and method for a photovoltaic panel cleaning robot. The method comprises the implementation steps that a sensing module collects multi-sensor data; the three-dimensional point cloud mapping module constructs a three-dimensional point cloud map of the working environment of the robot; the positioning module obtains the accurate pose of the robot based on the sensor data and the three-dimensional point cloud map; the path planning module obtains a navigation path based on the precise pose of the robot; the motion control module controls the robot to navigate to the target pose. The positioning module performs extended Kalman filtering on an environment three-dimensional point cloud map and multi-sensor data, integrates multi-source data, and overcomes the influence of positioning drift on navigation reliability; and meanwhile, the path planning module selects the navigation path of the robot from a structured road network map generated by the accurate pose of the robot when the robot runs along the preset working path according to the current pose of the robot and a navigation task issued by the remote upper computer platform, so that the calculation redundancy is reduced, and the overall efficiency of navigation is improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of intelligent robots, and relates to a navigation system and method of a photovoltaic panel cleaning robot, which can be applied to automatic cleaning work of a photovoltaic power station cleaning robot. BACKGROUND

[0002] The photovoltaic panel cleaning robot is a mobile robot platform for periodically cleaning dust, bird droppings and other pollutants accumulated on the surface of photovoltaic panels to improve power generation efficiency. The chassis of the robot is usually of Ackerman steering structure to adapt to the flat and regular path commonly seen between photovoltaic arrays. The robot body is integrated with a cleaning mechanism (such as a roller brush, a water spraying device, etc.) and various sensor systems. Among them, the navigation system is the "brain" of the robot, responsible for realizing autonomous, safe and accurate movement of the robot from the current position to the target position. Specifically, navigation refers to the complete technical process of the robot perceiving the environment and its own state through sensors, positioning (clearly knowing the position and attitude of itself in the environment), path planning (determining the optimal or feasible route from the starting point to the ending point) and motion control (driving the chassis to execute the planned path).

[0003] The navigation system of the existing photovoltaic panel cleaning robot, such as the patent application with publication number CN117968694A, discloses a path navigation system for a photovoltaic cleaning robot. The invention realizes navigation of the robot through the cooperation of sensing, positioning, path planning, obstacle avoidance and control modules, and further integrates autonomous charging, fault diagnosis, intelligent scheduling and other extension modules to improve the automation level of the system. However, the positioning module of the invention realizes positioning by directly obtaining multi-sensor data of the sensing module, which is prone to positioning drift in an outdoor open and structured photovoltaic array environment, thereby affecting the reliability of navigation. At the same time, since the working route of the photovoltaic panel cleaning robot is usually a pre-set fixed path, the path planning module of the system adopts a free planning strategy that does not match the characteristics of the working scene, resulting in redundant planning calculation and affecting the improvement of navigation efficiency. SUMMARY

[0004] The purpose of the present application is to overcome the defects of the prior art, and to provide a navigation system and method of a photovoltaic panel cleaning robot, which solves the technical problems of poor navigation reliability and low navigation efficiency in the prior art.

[0005] To achieve the above purpose, the technical solution adopted by the present application is as follows:

[0006] A navigation system of a photovoltaic panel cleaning robot, comprising a cascade of a perception module, a localization module, a path planning module and a motion control module; the perception module and the localization module are loaded with a three-dimensional point cloud mapping module, and the input end and the output end of the localization module are respectively connected with the output end of the perception module and the input end of the motion control module; the three-dimensional point cloud mapping module is used to construct an environment three-dimensional point cloud map based on sensor data collected by the perception module; the localization module is used to output an accurate pose of the robot based on sensor data and the environment three-dimensional point cloud map; the path planning module is used to select a navigation path of the robot based on a structured road network map generated based on the accurate pose of the robot.

[0007] As an optimization, the perception module comprises a satellite positioning sensor for outputting position data and motion inertia data of the robot itself, an inertial measurement sensor, and a laser radar sensor for outputting three-dimensional point cloud data of the working environment of the robot.

[0008] A navigation method of a photovoltaic panel cleaning robot, comprising the following steps:

[0009] (1) The perception module collects sensor data:

[0010] The perception module collects position data and motion inertia data of the robot itself, and three-dimensional point cloud data of the working environment;

[0011] (2) The three-dimensional point cloud mapping module constructs a three-dimensional point cloud map of the working environment of the robot:

[0012] The three-dimensional point cloud mapping module constructs a three-dimensional point cloud map of the working environment of the robot based on sensor data collected by the perception module:

[0013] (3) The localization module obtains an accurate pose of the robot based on sensor data and the three-dimensional point cloud map:

[0014] The localization module matches the three-dimensional point cloud data with the three-dimensional point cloud map, and performs extended Kalman filtering on the relative pose data of the robot obtained by matching, and the position data and motion inertia data of the robot itself, to obtain an accurate pose of the robot;

[0015] (4) The path planning module obtains a navigation path based on the accurate pose of the robot:

[0016] The path planning module generates a structured road network map based on the accurate pose of the robot when driving along a preset working route, and selects a navigation path of the robot from the structured road network map according to the current pose of the robot and a navigation task issued by a remote host computer platform;

[0017] (5) The motion control module controls the robot to navigate to a target pose:

[0018] The motion control module controls the robot to travel from the current pose to the target pose according to the accurate pose of the robot output by the positioning module and the navigation path given by the path planning module.

[0019] Compared with the prior art, the present application has the following advantages:

[0020] (1) The positioning module in the present application performs extended Kalman filtering on the environment three-dimensional point cloud map constructed by the three-dimensional point cloud mapping module and the multi-sensor data obtained by the perception module, fuses multi-source data, avoids the defect that the existing technology only uses the multi-sensor data of the perception module for positioning, which easily leads to positioning drift, and effectively improves the reliability of navigation.

[0021] (2) The path planning module of the present application collects the accurate pose of the robot when traveling along the preset working route to generate a structured road network map, and selects the navigation path of the robot from the structured road network map according to the current pose of the robot and the navigation task issued by the remote host computer platform, so that the path planning strategy is more closely matched with the fixed route characteristics of the photovoltaic panel cleaning operation, and the overall efficiency of navigation is improved. BRIEF DESCRIPTION OF DRAWINGS

[0022] Figure 1 It is a structural schematic diagram of the navigation system of the present application.

[0023] Figure 2 It is a flowchart of the implementation of the navigation method of the present application. DETAILED DESCRIPTION

[0024] The present application will be further described in detail below in combination with the drawings and specific embodiments.

[0025] Referring to Figure 1 , the navigation system of the photovoltaic panel cleaning robot of the present application comprises a cascaded perception module, a three-dimensional point cloud mapping module, a positioning module, a path planning module and a motion control module; the input end and the output end of the positioning module are respectively connected with the output end of the perception module and the input end of the motion control module; the perception module is used for collecting sensor data; the three-dimensional point cloud mapping module is used for constructing an environment three-dimensional point cloud map based on the sensor data; the positioning module is used for outputting the accurate pose of the robot based on the sensor data and the environment three-dimensional point cloud map; the path planning module is used for outputting the navigation path of the robot based on the accurate pose of the robot and the navigation task issued by the remote host computer platform; and the motion control module is used for controlling the robot to navigate to the target pose based on the accurate pose of the robot and the navigation path.

[0026] The perception module comprises a satellite positioning sensor for outputting the position data and motion inertia data of the robot itself, an inertial measurement sensor, and a laser radar sensor for outputting the three-dimensional point cloud data of the working environment of the robot.

[0027] A satellite positioning sensor is a Global Positioning System (GPS) receiver that receives satellite signals, calculates, and outputs the robot's absolute position data in the WGS-84 geographic coordinate system. An inertial measurement unit (IMU) typically integrates a three-axis accelerometer and a three-axis gyroscope to measure the robot's three-axis acceleration and angular velocity in the robot's coordinate system. A lidar sensor emits a laser beam and receives the echoes reflected from the surfaces of objects in the environment, directly measuring the three-dimensional coordinates of a large number of discrete points around the robot, thus forming a frame of three-dimensional point cloud data. In this embodiment, considering cost, application scope, and adaptability to outdoor photovoltaic array environments, a BeiDou differential navigation and positioning sensor is used for the satellite positioning sensor, a 9-axis IMU sensor is used for the inertial measurement sensor, and a 32-line mechanical lidar sensor is used.

[0028] Reference Figure 2 The present invention discloses a navigation method for a photovoltaic panel cleaning robot, comprising the following steps:

[0029] Step 1) The sensing module collects sensor data:

[0030] The perception module collects the robot's own position data and motion inertial data, as well as the three-dimensional point cloud data of the working environment;

[0031] Step 2) The 3D point cloud mapping module constructs a 3D point cloud map of the robot's working environment. This is based on the LIO-SAM algorithm, and the implementation steps are as follows:

[0032] (2a) The 3D point cloud mapping module preprocesses the 3D point cloud data of the working environment continuously collected by the perception module. The preprocessing uses a voxel mesh filter to downsample the 3D point cloud data, which reduces the amount of data while preserving the geometric features of the environment. Then, the edge point features and planar point features extracted from each frame of 3D point cloud data after preprocessing are accumulated. Then, the accumulated local point cloud feature map is matched with a newly collected frame of 3D point cloud data through the iterative nearest point algorithm, so as to minimize the geometric distance residual between the features of the current frame of 3D point cloud data and the corresponding features in the local point cloud feature map, thereby obtaining the matching pose of the photovoltaic panel cleaning robot.

[0033] (2b) The 3D point cloud mapping module pre-integrates the robot's own motion inertial data, uses the pre-integrated robot motion prediction information, the robot's matching pose and its own position data as constraint factors, and stitches together all the collected 3D point cloud data based on the factor graph optimization algorithm to generate a globally consistent 3D point cloud map.

[0034] This step generates a high-precision, globally consistent 3D point cloud map, providing a high-quality lidar observation benchmark for the extended Kalman filter of the robot's localization module. This helps avoid positioning drift and thus improves navigation reliability.

[0035] Step 3) The localization module obtains the robot's precise pose based on sensor data and a 3D point cloud map:

[0036] The localization module first matches the current frame's 3D point cloud data with the 3D point cloud map using an iterative nearest-point algorithm to obtain the robot's relative pose data. Since the map has been optimized by factor graph to eliminate accumulated errors and incorporates the robot's own position information, the robot's relative pose data calculated by the matching is a highly accurate observation with small drift and strong geometric constraints.

[0037] The localization module then performs extended Kalman filtering on the robot's relative pose data, as well as the robot's own position data and motion inertia data. The steps are as follows:

[0038] (3a) Initialize the number of iterations to be The maximum number of iterations is , No. The robot state vector in the next iteration The covariance matrix is and order ;

[0039] (3b) The positioning module performs positioning based on the current motion inertial data of the robot. The posterior state vector of the next iteration and its covariance matrix Perform prior state prediction to obtain the prior state vector. and its covariance matrix :

[0040] ;

[0041] ;

[0042] in , These are the state transition matrix and control input matrix based on the robot's kinematics model, respectively. This is the control input vector for the robot; It is Gaussian white noise. for The covariance matrix;

[0043] (3c) The positioning module uses the covariance matrix Calculate Kalman gain and through The observation vector consists of the robot's own position data and the robot's relative pose data. and prior state vector By fusing the data, we obtain the posterior state vector for this operation. and its covariance matrix ,in:

[0044] ;

[0045] ;

[0046] ;

[0047] in, For the observation function, For observation function Jacobian matrix; To observe the noise covariance matrix;

[0048] (3d) Judgment Is it true? If so, then... As the robot's precise pose, otherwise, let Then proceed with step (3b).

[0049] In this step, the localization module fuses the robot's own position data, motion inertial data, and robot relative pose data based on a 3D point cloud map using an extended Kalman filter algorithm. The robot relative pose data obtained from the globally consistent 3D point cloud map plays a core role in the fusion. It effectively corrects the short-term errors in the prediction of motion inertial data and complements the robot's own position data observation (the robot's own position data constrains the global position, while the robot relative pose data provides fine relative attitude and position), together suppressing the localization drift that may be introduced by any single sensor.

[0050] The localization module solves the localization drift problem caused by directly using raw, un-fused and optimized sensor data in the prior art by iteratively executing the extended Kalman filter algorithm described above. It can continuously output the robot's accurate pose, directly supporting the reliable operation of the path planning and motion control modules and effectively improving the reliability of navigation.

[0051] Step 4) The path planning module obtains the navigation path based on the robot's precise pose:

[0052] This step aims to provide the robot with an efficient and feasible route. Unlike existing technologies that typically involve time-consuming searches within complex global maps, this invention proposes a path planning scheme based on prior operational knowledge. By constructing a structured road network graph, the path planning problem is transformed into an efficient search within a fixed topology network, thus improving navigation efficiency. The specific implementation process is as follows:

[0053] (4a) When the robot performs its first navigation or path learning, the path planning module controls the robot to travel along a fixed working route preset on the photovoltaic array. During the travel, when the straight distance traveled by the robot exceeds the preset distance threshold, or when the change in the angle of the robot's rotation exceeds the preset angle threshold, the robot records the precise pose of the robot output by the positioning module at this time and uses the pose as the path key point. After the robot has traveled the entire working route, all the recorded path key points are used to form a path key point sequence. This sequence is essentially a digital and structured abstraction of the preset fixed working route.

[0054] (4b) The path planning module constructs a topology graph with each path key point in the path key point sequence as a topology node and the actual passable paths connecting adjacent path key points as undirected edges. It defines the length (representing the physical distance of the path segment) and the passage weight (a value that integrates empirical information such as road surface smoothness, turning difficulty, and historical passage efficiency; a higher weight indicates a greater "cost" or "price" of passage) of each undirected edge as its attributes. Then, all node information, undirected edges, and attribute information in the topology graph are serialized and stored to obtain a structured road network graph. This road network graph has a small data volume and a clear structure, serving as the foundation for subsequent rapid path planning.

[0055] (4c) In the structured road network graph, the path planning module takes the topological node closest to the robot's current precise pose as the path start point S and the topological node closest to the target pose specified by the navigation task as the path end point G. Using a graph theory-based path search algorithm, it systematically explores all possible edge connections from the start point S until it reaches the end point G, thereby traversing and finding all feasible paths from S to G. The feasible path is a path formed by sequentially connecting the undirected edges that are interconnected in the topological graph.

[0056] (4d) The path planning module calculates the cost of each feasible path. The feasible path with the lowest cost among all costs is selected as the optimal path.

[0057] in The calculation formula is:

[0058] ;

[0059] in For the summation operation, , Let be the number of undirected edges on the feasible path. , The first The length and passage weight of an undirected edge. These are the weighting coefficients, and .

[0060] The cost calculation formula incorporates the length of undirected edges and the passage weight attribute, which allows the path selection to not only consider the geometric shortest path, but also incorporate empirical judgments on path quality (such as prioritizing flat and straight roads and avoiding slippery or complex turning sections), thereby planning a more efficient and stable path in actual driving.

[0061] The structured road network map constructed in this invention is directly derived from the collection and abstraction of preset fixed work routes. Therefore, it inherently contains the optimal work passage information. The path planning is constrained within this defined high-quality road network, avoiding meaningless searches in non-work areas or unreasonable areas. This ensures that the planned path naturally meets the actual work requirements and improves navigation efficiency.

[0062] Step 5) The motion control module controls the robot to navigate to the target pose, which is implemented based on the Model Predictive Control (MPC) algorithm. The specific steps are as follows:

[0063] (5a) The initialization control cycle is The maximum control cycle is , No. The robot's state vector for each control cycle is: The control vector is The navigation task target state vector is The control sequence to be optimized is and order ;

[0064] (5b) The motion control module in the first Each control cycle constructs a preset state vector. threshold , and control vector threshold , As a constraint, using the state vector and control vector Cost function for the objective The optimal control sequence is obtained by solving for the minimum value of the cost function. ,in:

[0065] ;

[0066] ;

[0067] ;

[0068] in To control the time domain, This is the reference state vector on the navigation path. , and The weight matrix is ​​positive definite. and This indicates the operation of taking the weighted norm;

[0069] (5c) The motion control module will use the optimal control sequence The first control quantity in As actual control commands, they drive the robot's movement and calculate... and Euclidean distance :

[0070] ;

[0071] in This represents the norm operation;

[0072] (5d) Judgment or If the condition is met, then complete the robot navigation task; otherwise, set... and perform step (5b), where This is the preset navigation tolerance threshold.

Claims

1. A navigation system for a photovoltaic panel cleaning robot, comprising cascaded perception module, localization module, path planning module and motion control module; characterized in that, The perception module and the positioning module are loaded with a three-dimensional point cloud mapping module, and the input end and the output end of the positioning module are respectively connected with the output end of the perception module and the input end of the motion control module; the three-dimensional point cloud mapping module is used for constructing an environment three-dimensional point cloud map based on sensor data collected by the perception module; the positioning module is used for outputting the precise pose of the robot based on the sensor data and the environment three-dimensional point cloud map; and the path planning module is used for selecting the navigation path of the robot based on the structured road network map generated based on the precise pose of the robot.

2. The system of claim 1, wherein, The perception module comprises a satellite positioning sensor for outputting position data and motion inertia data of the robot itself, an inertial measurement sensor, and a laser radar sensor for outputting three-dimensional point cloud data of the working environment of the robot.

3. The method of navigation of the system of claim 1, wherein, The method comprises the following steps: (1) collecting sensor data by the perception module: The perception module collects position data and motion inertia data of the robot itself, and three-dimensional point cloud data of the working environment; (2) constructing a three-dimensional point cloud map of the working environment of the robot by the three-dimensional point cloud mapping module: The three-dimensional point cloud mapping module constructs a three-dimensional point cloud map of the working environment of the robot based on the sensor data collected by the perception module; (3) obtaining the precise pose of the robot based on the sensor data and the three-dimensional point cloud map by the positioning module: The positioning module matches the three-dimensional point cloud data with the three-dimensional point cloud map, and performs extended Kalman filtering on the relative pose data of the robot obtained by matching, and the position data and motion inertia data of the robot itself, to obtain the precise pose of the robot; (4) obtaining the navigation path based on the precise pose of the robot by the path planning module: The path planning module collects the precise pose of the robot when driving along the preset working route to generate a structured road network map, and selects the navigation path of the robot from the structured road network map according to the current pose of the robot and the navigation task issued by the remote host computer platform; (5) controlling the robot to navigate to the target pose by the motion control module: The motion control module controls the robot to drive from the current pose to the target pose according to the precise pose of the robot output by the positioning module and the navigation path given by the path planning module.

4. The method of claim 3, wherein, The three-dimensional point cloud map in step (2) is realized based on the LIO-SAM algorithm, and the realization steps are as follows: (2a) the three-dimensional point cloud mapping module pre-processes the three-dimensional point cloud data of the working environment collected by the perception module continuously, accumulates the edge point features and plane point features extracted from each frame of pre-processed three-dimensional point cloud data, and then matches the accumulated local point cloud feature map with a newly collected frame of three-dimensional point cloud data to obtain the matching pose of the photovoltaic panel cleaning robot; (2b) the three-dimensional point cloud mapping module pre-integrates the motion inertia data of the robot, takes the motion prediction information of the robot obtained by pre-integration, the matching pose of the robot and the position data of the robot as constraint factors, and performs splicing on all the collected three-dimensional point cloud data based on a factor graph optimization algorithm to generate a globally consistent three-dimensional point cloud map.

5. The method of claim 3, wherein, The realization steps of the extended Kalman filtering in step (3) are as follows: (3a) initialize the iteration number as , the maximum iteration number as , the covariance matrix of the robot state vector at the th iteration as , and let ; (3b) the positioning module performs a prior state prediction based on the motion inertia data of the current robot to obtain a prior state vector and its covariance matrix for the next iteration and its covariance matrix and its covariance matrix : ； ； wherein , are respectively a state transition matrix and a control input matrix based on a kinematic model of the robot, is a control input vector of the robot; is a Gaussian white noise, is a covariance matrix of (3c) the localization module fuses the observation vector calculates the Kalman gain and fuses it with the observation vector consisting of the robot's own position data and the robot's relative pose data and the prior state vector to obtain the current posterior state vector and its covariance matrix wherein: ； ； ； wherein is the observation function, is the observation function is the Jacobian matrix of the observation function; is the observation noise covariance matrix; (3d) determining whether it is true; if so, setting the precise pose of the robot, otherwise, setting and performing step (3b).

6. The method of claim 3, wherein, The realization steps of the navigation path in step (4) are as follows: (4a) When the robot performs first navigation or path learning, the path planning module controls the robot to travel along the preset working route of the photovoltaic panel array. During the travel, when the straight-line distance traveled by the robot exceeds the preset distance threshold, or the angle change of the rotation of the robot exceeds the preset angle threshold, the robot records the accurate pose of the robot output by the positioning module at this time as a path key point. After the robot travels the entire working route, all recorded path key points form a path key point sequence; (4b) The path planning module constructs a topological graph with each path key point in the path key point sequence as a topological node, and the actual passable path between the adjacent two path key points as an undirected edge. The length and passable weight of each undirected edge are defined as the attributes of the undirected edge. Then, all node information, undirected edges and attribute information in the topological graph are serialized and stored to obtain a structured road network graph; (4c) In the structured road network graph, the path planning module takes the topological node closest to the current accurate pose of the robot as the path starting point S, and takes the topological node closest to the target pose specified by the navigation task as the path ending point G. A path search algorithm based on graph theory is used to start from the starting point S to systematically explore all possible edge connections until the ending point G is reached, so as to traverse and find all feasible paths from S to G. The feasible path is a path formed by sequentially connecting the interconnected undirected edges in the topological graph; (4d) The path planning module calculates the cost of each feasible path and selects the feasible path with the minimum cost as the optimal path.

7. The method of claim 6, wherein, The cost described in step (4d) The calculation formula is: ； wherein is a summation operation, , is the number of undirected edges on a feasible path, , are the length, the transit weight, respectively, of the th undirected edge, is a weight coefficient, and .

8. The method of claim 3, wherein, The step (5) of controlling the robot to travel from the current pose to the target pose is realized based on a model predictive control algorithm MPC, and the specific steps are as follows: (5a) initializing a control period as , a maximum control period as , a state vector of the robot at the th control period as , a control vector as , a navigation task target state vector as , a control sequence to be optimized as , and letting ; (5b) The motion control module in the first Each control cycle constructs a preset state vector. threshold , and control vector threshold , As a constraint, using the state vector and control vector Cost function for the objective and to Solve the problem to obtain the optimal control sequence. ,in: ； ； ； wherein is a control time domain, is a reference state vector on a navigation path, , and are positive definite weight matrices, and denotes a take weighted norm operation; (5c) The motion control module drives the robot motion with the first control quantity of the optimal control sequence as actual control commands and computes the Euclidean distance to :​ ； wherein denotes a take norm operation; (5d) determining or is true, the robot navigation task is completed, otherwise, let , and performing step (5b), wherein is a preset navigation tolerance threshold.

Images