An autonomous navigation method for underwater robots based on energy consumption perception

Abstract

The present invention discloses an underwater robot autonomous navigation method based on energy consumption perception, comprising: obtaining underwater environment parameters and robot state parameters by fusing multi-sensor data, adopting a graph-based SLAM algorithm, constructing a factor graph model including posture nodes and energy consumption nodes, obtaining a local area environmental energy consumption map by fusing the maximum a posteriori estimation of posture and energy consumption with the robot state parameters, and updating the local energy consumption map in real time according to the expansion of the robot exploration area; if the current local energy consumption map covers the global target position, calculating the global energy consumption optimal path according to the path length and energy consumption; if the current local energy consumption map has not yet covered the global target position, triggering local path re-planning, generating an energy consumption compensation strategy; if the environment undergoes a major change that causes the current path to become invalid, the global planning needs to be re-executed.

Description

Technical Field

[0001] The invention relates to the field of information technology, and in particular to an autonomous navigation method for an underwater robot based on energy consumption perception. Background Art

[0002] With the development of underwater robot technology, the ability of robots to perform tasks in complex seabed environments has become particularly critical. These environments usually contain uncertain water flow speed, direction and water pressure dynamic factors. In order to navigate and perform tasks effectively, robots must be able to accurately perceive and adapt to these changes. In addition, the energy consumption management of robots has also become a challenge because underwater operations are usually accompanied by high energy consumption and limited energy supply.

[0003] Therefore, it is particularly important to build a multi-dimensional spatiotemporal energy consumption perception model that can predict and optimize energy consumption in real time. The model needs to combine water flow velocity, water flow direction, water pressure environmental parameters and the robot's speed and posture state parameters to achieve accurate energy consumption prediction and path planning.

[0004] The technical problems solved by the present invention include how to make effective navigation decisions when the local energy consumption map has not yet covered the global target position. In this case, the robot needs to generate a random sampling tree with the current position as the root node within the local map and select the path with the lowest energy consumption cost. In addition, the technical problem also involves how to accurately predict the energy consumption and water flow change trends in the branch extension direction in a dynamically changing underwater environment to guide the growth direction of the sampling tree. This requires the robot's perception system to be able to obtain environmental parameters in real time and accurately, and effectively integrate these parameters into the energy consumption perception model. At the same time, another key issue that needs to be addressed is how to ensure that the local energy consumption map can be updated in real time during global path planning, and on this basis, calculate the global path with the optimal energy consumption. These technical problems are interrelated, and solving them is crucial to improving the autonomy and efficiency of underwater robots, but it is also full of challenges.

[0005] Environmental modeling technology in the autonomous navigation of underwater robots based on energy consumption perception requires accurate modeling of the underwater environment. Traditional environmental modeling technologies such as SLAM technology mainly focus on the geometric characteristics of the environment and ignore the energy consumption characteristics of the environment. By introducing the energy consumption perception mechanism to optimize the navigation strategy, considering the impact of water flow and water pressure factors on the robot's energy consumption, an underwater environment model including energy consumption characteristics is constructed to provide environmental information support for the energy consumption perception and autonomous navigation of underwater robots. Summary of the invention

[0006] In order to solve the above-mentioned technical problems, the present invention provides an underwater robot autonomous navigation method based on energy consumption perception.

[0007] The technical solution of the present invention is implemented as follows: an underwater robot autonomous navigation method based on energy consumption perception mainly includes:

[0008] Based on the multi-sensor fusion data carried by the robot, underwater environmental parameters are obtained, including water flow velocity vector field, water pressure distribution and obstacle distribution map. At the same time, combined with the robot state parameters, including speed, posture and energy consumption data, a multi-dimensional spatiotemporal energy consumption perception model is constructed;

[0009] For the energy consumption perception model, a graph-based SLAM algorithm is used to construct a factor graph model containing pose nodes and energy consumption nodes. The pose nodes represent the position and direction of the robot in the environment, the energy consumption nodes represent the energy consumption value under the pose, and the edges between the nodes represent the pose constraints and energy consumption constraints.

[0010] The factor graph model and energy consumption perception model are combined to calculate the energy consumption increment between postures and form an energy consumption increment factor. Together with the traditional vision and odometer factors, the factor graph method is optimized to solve the maximum a posteriori estimation of posture and energy consumption.

[0011] The maximum a posteriori estimate of posture and energy consumption is integrated with the robot state parameters to obtain a local area environmental energy consumption map that includes the spatial distribution of environmental parameters and the estimated energy consumption of each position. The local energy consumption map is updated in real time according to the expansion of the robot's exploration area.

[0012] If the current local energy consumption map covers the global target position, global path planning is performed, using an octree structure to search for the optimal energy consumption in the tree space, and the global energy consumption optimal path is calculated based on the path length and energy consumption;

[0013] If the current local energy consumption map has not yet covered the global target position, navigation decisions are made only within the scope of the local energy consumption map. The current position of the robot is used as the root node, and a random sampling tree is generated in the neighborhood. According to the maximum a posteriori estimate of the water flow direction, water flow speed, robot posture and energy consumption, the branch with the lowest energy cost is selected as the local energy consumption optimal path. At the same time, the energy consumption and water flow change trends in the branch extension direction are predicted to guide the growth direction of the tree.

[0014] The above global or local energy consumption optimal path is discretized into a control waypoint sequence, and control instructions are generated and executed through the motion control module. The motion control adopts an adaptive PID controller. While tracking the desired waypoint, the control parameters are adjusted online according to the deviation between the actual energy consumption and the expected energy consumption;

[0015] During the robot's movement, it continuously perceives the environment and builds energy consumption maps. When the actual energy consumption deviates seriously from expectations, local path replanning is triggered and an energy consumption compensation strategy is generated. If the environment changes significantly and the current path becomes invalid, global planning needs to be re-executed. Beneficial Effects

[0016] The present invention acquires underwater environmental parameters and robot state parameters by fusing multi-sensor data, and constructs a multi-dimensional energy consumption perception model, which solves the problems of insufficient comprehensive underwater environmental perception and inaccurate energy consumption modeling in traditional methods. An innovative factor graph model is adopted to optimize the posture and energy consumption nodes, improve the accuracy of energy consumption estimation and the rationality of path planning, and ensure the efficient and low-energy operation of the robot in a complex underwater environment through dynamically updated local energy consumption maps and real-time navigation decisions. In addition, the path can be replanned and energy consumption compensation can be performed in time when the environment changes, so as to solve the impact of environmental changes on path planning and ensure the stability and reliability of task execution. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is a structural block diagram of an underwater robot autonomous navigation method based on energy consumption perception in an embodiment of the present invention;

[0018] Figure 2 This is a flowchart of a method for autonomous navigation of an underwater robot based on energy consumption perception in an embodiment of the present invention. DETAILED DESCRIPTION

[0019] In order to make the objects and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with embodiments; it should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0020] The preferred implementation methods of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these implementation methods are only used to explain the technical principles of the present invention and are not intended to limit the protection scope of the present invention.

[0021] It should be noted that when an element is considered to be "connected" to another element, it can be directly connected to the other element, or connected to the other element through an intermediate element. In addition, the "connection" in the following embodiments should be understood as "electrical connection", "communication connection", etc. if there is transmission of electrical signals or data between the connected objects.

[0022] When used herein, the singular forms "a", "an", and "said / the" may also include plural forms, unless the context clearly indicates otherwise. It should also be understood that the terms "include / comprise" or "have" and the like specify the presence of stated features, wholes, steps, operations, components, parts, or combinations thereof, but do not exclude the possibility of the presence or addition of one or more other features, wholes, steps, operations, components, parts, or combinations thereof. At the same time, the term "and / or" used in this specification includes any and all combinations of the relevant listed items.

[0023] See also Figure 1-2 As shown, an underwater robot autonomous navigation method based on energy consumption perception includes:

[0024] S101. Based on the multi-sensor fusion data carried by the robot, underwater environmental parameters are obtained, including water flow velocity vector field, water pressure distribution and obstacle distribution map. At the same time, the robot state parameters, including speed, posture and energy consumption data, are combined to construct a multi-dimensional spatiotemporal energy consumption perception model.

[0025] Specifically, based on the multi-sensor fusion data carried by the robot, Kalman filtering and Bayesian estimation multi-sensor data fusion algorithms are used to fuse the sensor data to obtain underwater environmental parameters, which include water flow velocity vector field, water pressure distribution and obstacle distribution map; the robot's state parameters are obtained, which include speed, posture and energy consumption data, and the state parameters are associated with the underwater environmental parameters to provide input information for the construction of an energy consumption perception model; a spatiotemporal dynamic Bayesian network and a hidden Markov model multi-dimensional spatiotemporal modeling method are used, combined with the underwater environmental parameters and robot state parameters, and an energy consumption perception model is constructed by modeling and analyzing the energy consumption data at different spatiotemporal positions. The energy consumption perception model describes the energy consumption change law when the robot moves between positions; support vector machines are introduced, The random forest machine learning algorithm establishes an energy consumption prediction model by learning and training historical data, and integrates the energy consumption prediction model with the energy consumption perception model to optimize and improve the energy consumption perception model; according to the prediction results of the energy consumption perception model, reinforcement learning and adaptive dynamic programming intelligent decision-making algorithms are used to autonomously adjust the robot's motion strategy and parameters, which include speed, posture and path planning. By optimizing the robot's motion control strategy, the robot's energy consumption in an underwater environment is minimized; during the robot's movement, the underwater environment parameters and robot state parameters are continuously acquired, and the parameters are input into the energy consumption perception model in real time. By collecting data in real time and updating and correcting the energy consumption perception model, an interactive closed loop between the robot and the environment is established, and the adaptability and robustness of the system are continuously improved.

[0026] In one embodiment, based on the multi-sensor fusion data carried by the robot, including Doppler sonar, pressure sensor, lidar and camera, detailed underwater environmental parameters are obtained through data fusion algorithm, including water flow velocity vector field, water pressure distribution and obstacle distribution map, where the water flow velocity vector field shows the direction and size of water flow velocity at different depths and positions, the water pressure distribution map shows the water pressure changes at different depths and areas, and the obstacle distribution map details the shape, position and size of underwater obstacles. These parameters are fused and processed through Kalman filtering and Bayesian estimation multi-sensor data fusion algorithm to improve the accuracy and reliability of the data, and obtain real-time dynamic information of the underwater environment for subsequent energy consumption. The perception model is constructed to provide basic data. At the same time, the robot state parameters, including speed, posture and energy consumption data, are obtained. Through real-time monitoring and analysis of these parameters, the robot's motion state and energy consumption level are judged, and they are associated with environmental parameters to provide important input information for the subsequent construction of the energy consumption perception model. On this basis, the spatiotemporal dynamic Bayesian network and hidden Markov model multi-dimensional spatiotemporal modeling methods are adopted, combined with the water flow velocity vector field, water pressure distribution, obstacle distribution map, and robot speed, posture and energy consumption information in multiple dimensions. By modeling and analyzing the energy consumption data of the robot at different time and space positions, the energy consumption change law of the robot when moving between postures is characterized, and an energy consumption perception model is constructed.

[0027] Furthermore, support vector machines and random forest machine learning algorithms are introduced. Through learning and training of historical data, an energy consumption prediction model is established, and it is integrated with the spatiotemporal energy consumption perception model to continuously optimize and improve the energy consumption perception model, improve the accuracy and stability of the model, and enable it to more accurately predict the energy consumption level of the robot under different underwater environmental conditions. According to the prediction results of the energy consumption perception model, reinforcement learning and adaptive dynamic programming intelligent decision-making algorithms are used to autonomously adjust the robot's motion strategy and parameters, including speed, posture and path planning. By optimizing the robot's motion control strategy, the robot's energy consumption in the underwater environment is minimized. During the robot's movement, the underwater environmental parameters and robot state parameters are continuously obtained, and these parameters are input into the energy consumption perception model in real time. By collecting data in real time and using it to update and correct the perception model, an interactive closed loop between the robot and the environment is established, and the adaptability and robustness of the system are continuously improved to ensure that the energy consumption perception model always maintains high accuracy and reliability, and outputs accurate energy consumption prediction information for the robot's motion control.

[0028] In terms of multi-sensor data fusion technology, the Kalman filter algorithm is used to fuse the underwater environmental parameters. By performing Kalman filtering on the data collected by multiple sensors (sonar, pressure sensor, flow meter) carried by the robot, the water velocity vector field, water pressure distribution and obstacle distribution map environmental parameters are obtained. At each time step, the state is predicted based on the state estimation and state transfer matrix of the previous moment, and then the prediction result is corrected according to the observation value and observation matrix of the current moment to obtain the optimal state estimation of the current moment. By continuously iterating this process, the multi-sensor data is effectively fused to improve the accuracy of environmental parameter estimation. At a certain moment, the water velocity vector field within 5 meters around the robot is obtained by fusing the data of the sonar and pressure sensor through Kalman filtering. The average flow velocity is 0.5 m / s and the flow direction is northeast. At the same time, it is estimated that the water depth of the robot is 50 meters and there are 2 obstacles with a diameter greater than 0.3 meters nearby. In terms of energy consumption perception model construction, the hidden Markov model (HMM) is used to model and analyze the energy consumption change law of the robot. In the robot energy consumption perception, the energy consumption level of the robot is abstracted as a hidden state, and the speed is , posture, and environmental parameters as observation values. By training the HMM model, the distribution characteristics of observation values ​​under different hidden states and the transition rules between hidden states are learned. Through the study of a large amount of historical data, it is found that the robot is usually in a lower hidden state (0.1 kWh) when it is low speed (0.2 m / s), stable (pitch angle less than 5 degrees), and without obstacles. The Gaussian distribution parameters of the observation values ​​in this state are mean 0.2 m / s (speed), 0 degrees (pitch angle), 0 (number of obstacles), and the variance is 0.05, 1, and 0.1. At the same time, the transition probability matrix between different hidden states is also learned, such as the probability of transferring from a low energy consumption state to a high energy consumption state (0.3 kWh) is 0.2, and the probability of staying in a low energy consumption state is 0.8. Through the HMM model, the real-time prediction and perception of the robot's energy consumption level is realized. In the robot energy consumption optimization, the robot's state is defined as the current speed, posture, and energy consumption level, the action is defined as the adjustment amount of speed and posture, and the reward function is based on the goal of minimizing energy consumption.

[0029] S102. For the energy consumption perception model, a graph-based SLAM algorithm is used to construct a factor graph model including posture nodes and energy consumption nodes. The posture nodes represent the position and direction of the robot in the environment, the energy consumption nodes represent the energy consumption value under the posture, and the edges between the nodes represent the posture constraints and energy consumption constraints.

[0030] Specifically, a graph-based SLAM algorithm is used to construct a factor graph model containing pose nodes and energy consumption nodes, in which the pose node represents the position and orientation information of the robot in the environment, the energy consumption node represents the energy consumption value under the current pose, and the edges between the pose node and the energy consumption node are used as the motion factor and energy consumption factor in the factor graph respectively. The motion factor represents the motion constraint relationship between the pose nodes, and the energy consumption factor represents the energy consumption constraint relationship between the pose node and the energy consumption node. By recording the robot's pose and using the energy consumption perception model to calculate the constraint relationship between adjacent pose nodes, the pose factor is constructed to obtain The actual energy consumption observation value is taken, and the energy consumption perception model is used to calculate the constraint relationship between adjacent energy consumption nodes, and the energy consumption factor is constructed. The energy consumption information is introduced into the factor graph model to realize the synchronous representation and estimation of positioning and energy consumption perception, and overcome the problem of separation of positioning and energy consumption perception in traditional methods. If the robot's posture changes, the motion factor and energy consumption factor between the posture node and the energy consumption node are updated to realize the dynamic update of the factor graph model. For the constructed factor graph model, an optimization algorithm is used to solve it, and the optimal posture estimation and energy consumption estimation of the robot are obtained, so as to realize efficient positioning and energy consumption perception of the underwater robot.

[0031] In one embodiment, for the energy consumption perception model, a graph-based SLAM algorithm is used to construct a factor graph model including posture nodes and energy consumption nodes, and the position change and energy consumption of the robot in the underwater environment are described in detail, wherein the posture node represents the position and direction information of the robot in the environment, and the energy consumption node represents the energy consumption value under the current posture. The edges between the posture node and the energy consumption node are respectively used as motion factors and energy consumption factors in the factor graph, which represent the motion constraint relationship between the posture nodes and the energy consumption constraint relationship between the energy consumption nodes. The energy consumption information is introduced into the factor graph model to realize the synchronous representation and estimation of positioning and energy consumption perception, overcome the problem of separation of positioning and energy consumption perception in traditional methods, and improve the efficiency and accuracy of perception. wherein, by recording the posture of the robot, the constraint relationship between adjacent posture nodes is calculated using the energy consumption perception model, and the posture factor is constructed. By using the actual energy consumption observation value, the constraint relationship between adjacent energy consumption nodes is calculated using the energy consumption perception model, and the energy consumption factor is constructed.

[0032] When constructing a factor graph model containing pose nodes and energy consumption nodes, the g2o graph optimization library is used to implement it. First, the data structure of the pose nodes and energy consumption nodes is defined, where the pose node contains the robot's three-dimensional position (x, y, z), the direction represented by the quaternion (qw, qx, qy, qz) and the robot's speed, and the energy consumption node contains the energy consumption value (in joules) under the pose. Then, according to the energy consumption perception model, the relative motion constraints between adjacent pose nodes are calculated to construct the motion factor. The preset movement distance of the robot between two pose nodes is 1 meter, and the rotation angle is 30 degrees. The observed value of the motion factor is (1, 0, 0, cos (π / 12), sin (π / 12), 0, 0). The noise covariance matrix is ​​set according to the sensor accuracy characteristics. At the same time, the energy consumption factor between each pose node value and the adjacent pose node and the corresponding energy consumption node is calculated according to the energy consumption perception model. The observed value is the energy consumption difference. The noise covariance reflects the uncertainty of the energy consumption estimation. By minimizing the sum of square errors of all factors, the LM algorithm is used to iterate. Optimize and obtain the optimal estimated values ​​of the pose nodes and energy consumption nodes. For example, in the optimization process, the initial value of the pose node is the result of the odometer integration, and the initial value of the energy consumption node is 0. In each iteration, the error and Jacobian matrix of each factor are calculated according to the current estimated value, and the incremental equation is constructed. The incremental equation is solved to obtain the optimized increment, and the current estimated value is updated until the increment is less than the threshold or the maximum number of iterations is reached. The pose node sequence finally obtained forms the motion trajectory of the robot, and the energy consumption node sequence gives the estimated energy consumption value under each pose. When introducing the energy consumption factor, according to the physical energy consumption model of the robot, such as considering the current, voltage, and speed parameters of the motor, the observation equation of the energy consumption factor is established, and the energy consumption is explicitly associated with the pose state. For example, the energy consumption of the preset robot is quadratically related to the speed, and the observation model of the energy consumption factor is expressed as: E=kv^2, where E is the energy consumption value, v is the speed size, and k is the proportional coefficient. By optimizing the energy consumption factor, the speed estimation value and energy consumption estimation value under each pose are obtained while the pose is estimated, so as to realize the fusion of positioning and energy consumption perception.

[0033] S103. Combine the factor graph model and the energy consumption perception model to calculate the energy consumption increment between postures and form an energy consumption increment factor. Together with the traditional vision and odometer factors, the factor graph method is optimized to solve the maximum a posteriori estimation of posture and energy consumption.

[0034] Specifically, based on the factor graph model and the energy consumption perception model, the energy consumption increment of the robot between different postures is predicted, and the energy consumption increment factor is constructed; the spatiotemporal dynamic Bayesian network and the hidden Markov model multi-dimensional spatiotemporal modeling method are used to integrate the water flow velocity vector field, water pressure distribution, obstacle distribution map, and information in multiple dimensions of robot speed, posture and energy consumption for modeling; the energy consumption data of the robot is collected at different time and space positions, and the energy consumption perception model is trained using the energy consumption data, and the residual between the predicted value of the energy consumption perception model and the actual energy consumption observation value is used as the observation value of the factor. And the noise covariance of the factor is set according to the uncertainty of the residual; if the likelihood probability construction method is adopted, the likelihood probability of different energy consumption increments is calculated according to the prediction results of the energy consumption perception model, and the likelihood probability is used as the observation value of the factor; the constructed energy consumption increment factor is incorporated into the factor graph model together with the visual factor and the odometer factor, and a factor graph of multivariate information fusion is formed by sharing the pose nodes; using the graph optimization library, the maximum a posteriori estimate of the pose nodes and the energy consumption nodes is obtained by minimizing the sum of squared errors of all factors, thereby achieving high-precision estimation of the robot's motion trajectory and energy consumption increment prediction.

[0035] In one embodiment, a factor graph model and an energy consumption perception model are combined to predict the energy consumption increment of the robot between different postures, and to construct an energy consumption increment factor. A spatiotemporal dynamic Bayesian network and a hidden Markov model multi-dimensional spatiotemporal modeling method are used. The water velocity vector field, water pressure distribution, obstacle distribution map, and information on the robot's speed, posture, and energy consumption in multiple dimensions are combined. The energy consumption increment factor prediction result is obtained by constructing an energy consumption perception model based on the energy consumption data of the robot at different time and space positions. According to the prediction result of the energy consumption perception model, the energy consumption increment factor is constructed by using a residual construction method or a likelihood probability construction method. The energy consumption increment prediction value is used as the observed value of the factor, and the noise covariance of the factor is set according to the uncertainty of the prediction. The energy consumption increment factor is incorporated into the factor graph model together with the visual factor and the odometer factor to form a factor graph of multivariate information fusion. The visual factor calculates the posture change between adjacent frames by extracting image features, performing feature matching and posture estimation. The odometer factor measures the robot's motion information through the encoder, including wheel speed and heading; the energy consumption factor reflects the energy change relationship between postures. These different types of factors form a closely related factor graph model by sharing posture nodes. The constructed multivariate information fusion factor graph is solved using the graph optimization method. The maximum a posteriori estimate of the posture node and energy consumption node is obtained by minimizing the sum of square errors of all factors. In each iterative optimization process, the error and Jacobian matrix of each factor are calculated based on the current posture node and energy consumption node estimates. For the visual factor, the error is the reprojection error under the current posture estimation. For the odometer factor, the error is the difference between the current posture estimation and the odometer measurement value. For the energy consumption factor, the error is the difference between the energy consumption prediction value under the current posture estimation and the actual energy consumption observation value. The error and Jacobian matrix of each factor are constructed into an incremental equation, and robust kernel functions and prior information are added to improve the accuracy and robustness of the optimization.

[0036] The energy consumption perception model is applied to the construction of energy consumption increment factors in factor graph optimization, and the energy consumption increment of the robot between different postures is predicted. The weight and noise covariance of the factor are adjusted according to the difference between the actual energy consumption and the predicted value. For example, when the actual energy consumption is much higher than the predicted value, the weight of the energy consumption factor is appropriately increased to make the optimization more inclined to minimize the energy consumption; when the actual energy consumption is close to the predicted value, the weight is reduced to make the optimization more inclined to minimize the errors of other factors. At the same time, the noise covariance is set according to the confidence of the energy consumption prediction. The higher the confidence, the smaller the noise and the stronger the constraint of the factor. By dynamically adjusting the weight and noise of the factor, the adaptive fusion of energy consumption perception and other perception information is realized in the process of factor graph optimization, and the accuracy and robustness of positioning and energy consumption estimation are improved. For example, when the battery is sufficient, the weight of the energy consumption factor is appropriately reduced to give priority to ensuring the positioning accuracy; when the battery is insufficient, the weight of the energy consumption factor is increased to give priority to reducing energy consumption and extending the battery life. By flexibly adjusting the role of multi-perception information in factor graph optimization, the adaptive operation optimization of the robot under different task requirements and environmental conditions is realized.

[0037] S104, integrating the maximum a posteriori estimate of the posture and energy consumption with the robot state parameters to obtain a local area environmental energy consumption map including the spatial distribution of environmental parameters and the estimated energy consumption of each position, and updating the local energy consumption map in real time according to the expansion of the robot's exploration area.

[0038] Specifically, according to the state parameters and environmental parameters of the robot, a Gaussian process regression method is used to establish a nonlinear mapping relationship between the state parameters and the environmental parameters, and the parameter space distribution that comprehensively describes the robot's environment is obtained. According to the spatial distribution of the environmental parameters, combined with a physical energy consumption model or a data-driven energy consumption model, an estimated energy consumption value at the current position and state is predicted. The estimated energy consumption value reflects the energy consumption of the robot when it is running at this position. By integrating the estimated energy consumption values ​​of each position and smoothing the energy consumption using the spatial correlation and temporal continuity of the energy consumption, an energy consumption map that reflects the energy consumption characteristics of the local area environment is constructed, and a sliding window size that dynamically adjusts the window size based on the robot's movement speed and the size of the exploration area is used. The window strategy dynamically adjusts the size of the local map window by setting thresholds for speed and area size to balance computational efficiency and map coverage. Environmental factors are introduced into the factor graph as prior factors. By constructing constraint relationships between environmental factors and posture and energy consumption factors, the fusion of environmental perception, positioning, and energy consumption estimation is achieved to obtain a more accurate and comprehensive environmental energy consumption map. A multi-resolution map is used to maintain a high-resolution local energy consumption map near the robot and a low-resolution global energy consumption map in areas far away from the robot. By dynamically adjusting the scope and resolution of the local map, while ensuring coverage of the robot's current exploration area, the area that has been far away is promptly eliminated to reduce computational and storage overhead and improve the real-time performance of map updates.

[0039] In one embodiment, the maximum a posteriori estimation of the posture and the maximum a posteriori estimation of the energy consumption obtained by factor graph optimization are fused with the state parameters of the robot. The state parameters include the robot's speed, motor speed, motor current, and battery voltage, which reflect the real-time operating status of the robot. The low-dimensional representation of the state parameters is extracted by the principal component analysis method, and the nonlinear mapping relationship between the state parameters and the environmental parameters is established by the Gaussian process regression method to obtain a parameter space distribution that comprehensively describes the robot's environment. On the basis of the obtained environmental parameter space distribution, the maximum a posteriori estimation of the energy consumption is further used in combination with a physical energy consumption model or a data-driven energy consumption model to predict the energy consumption at the current position and state according to the robot's state parameters and environmental parameters. The energy consumption estimate reflects the energy consumption of the robot when it is running at that position. By integrating the energy consumption estimates of each position and using the spatial correlation and time continuity of energy consumption for smooth optimization, an energy consumption map that reflects the energy consumption characteristics of the local area environment is constructed. According to the robot's motion trajectory and exploration area, the constructed local area environmental energy consumption map is updated in real time. A sliding window strategy that dynamically adjusts the window size based on the robot's motion speed and the size of the exploration area is adopted to balance the computational efficiency and map coverage. When the robot enters a new unknown area, the pose estimation and energy consumption estimation of the area are obtained according to the results of factor graph optimization, and are integrated into the existing environmental energy consumption map. Through continuous expansion and updating, New, get an environmental energy consumption map covering the entire exploration area of ​​the robot, improve the prediction accuracy and real-time performance of the environmental energy consumption map, in the process of factor graph optimization, not only the posture factor and energy consumption factor are combined, but also the environmental factor is introduced, the environmental factor includes terrain undulation, obstacle density, light intensity, parameters that reflect environmental characteristics, by extracting these features from sensor data, the environmental factor is introduced as a priori factor, by constructing the constraint relationship between environmental factors and posture and energy consumption factors, the fusion of environmental perception and positioning, energy consumption estimation is realized, and a more accurate and comprehensive environmental energy consumption map is obtained. In the process of constructing the environmental energy consumption map, the correlation between the robot state parameters and energy consumption is fully utilized, and the energy consumption of the robot in different states is analyzed. The energy consumption characteristics under different speeds and load conditions are included. The nonlinear mapping relationship between state parameters and energy consumption is established by using machine learning algorithms, including support vector machines and neural networks. The mapping relationship is used to predict the energy consumption change trend according to the real-time state parameters of the robot, and to provide prior information and constraints for the real-time update of the local area environmental energy consumption map. In the expansion of the robot's exploration area, a multi-resolution map method is used to maintain a high-resolution local energy consumption map near the robot, and a low-resolution global energy consumption map is maintained in the area far away from the robot. By dynamically adjusting the range and resolution of the local map, while ensuring that the robot's current exploration area is covered, the area that has been far away is timely eliminated to reduce the computing and storage overhead and improve the real-time performance of the map update.At the same time, it realizes the seamless integration and update of local and global energy consumption information, introduces other environmental perception and energy consumption estimation methods, including semantic maps and energy consumption prediction networks, and uses semantic maps to identify and classify objects and areas in the environment to obtain higher-level environmental information; through the energy consumption prediction network, it directly learns energy consumption characteristics from raw sensor data to improve the accuracy and real-time performance of energy consumption prediction, further expands and optimizes the construction method of energy consumption maps, and improves the robot's ability to operate autonomously for a long time in complex environments;

[0040] In the fusion of robot state parameters and environmental parameters, principal component analysis (PCA) is used to extract low-dimensional representation of state parameters. For example, for a 6-DOF robot, its state parameters include 3 position coordinates (x, y, z) and 3 Euler angles (roll, pitch, yaw), a total of 6 dimensions. Through PCA analysis, it is found that the first 3 principal components explain 95% of the variance of state parameters. Therefore, a 3D vector is used to represent the state of the robot. On this basis, Gaussian process regression (GPR) is used to establish a mapping relationship between state parameters and environmental parameters. The training data is the environmental parameters (temperature, humidity, and light) collected by the robot under different states. By optimizing the kernel function hyperparameters of GPR, A Gaussian process model is obtained to predict the distribution of environmental parameters under any state. For the calculation of energy consumption estimation, a physical energy consumption model is used. For example, for wheeled robots, energy consumption mainly comes from the mechanical work and heat dissipation of the motor, which is expressed by the formula P = Tm·ω+I^2·R, where P is the total power, Tm is the motor output torque, ω is the motor speed, I is the motor current, and R is the motor internal resistance. According to the robot's speed, load, and motor parameter state, the energy consumption estimation at different positions is calculated and represented by an energy consumption map. At the same time, considering the spatial correlation of energy consumption, the energy consumption map is smoothly optimized using Markov random field (MRF). By minimizing the MRF energy function, the energy consumption estimation of adjacent positions is minimized. The estimation is as close as possible. When introducing environmental factors, laser radar and visual sensors are used to extract environmental features. For example, for laser point cloud data, the terrain undulation characteristics are obtained by calculating the normal vector and curvature of each point; for visual images, the obstacle density characteristics are obtained through semantic segmentation algorithms. These characteristics are constructed into environmental factors and included in the factor graph optimization together with the posture and energy consumption factors. By minimizing the overall error of the factor graph, the joint estimation of the robot state and environmental parameters is obtained. For example, in a 100m×100m exploration area, 10 environmental factor nodes are extracted, each node contains 5 different environmental features, then there are a total of 10×(5+6)=110 variable nodes in the factor graph. To optimize the solution, finally, in the construction of the local energy consumption map, the octree structure is used to dynamically adjust the map resolution according to the position of the robot. The area close to the robot uses a high resolution of 0.1m, and the area far away from the robot uses a low resolution of 1m. The leaf nodes of the octree store the local energy consumption map data. As the robot moves, the local map is updated in real time through dynamic cropping and expansion of the octree nodes, while maintaining topological consistency with the global map. When the robot enters a new area, the energy consumption estimate of the area is calculated according to the results of factor graph optimization and inserted into the local map. At the same time, the update of the local map is passed to the global map to achieve seamless splicing of multi-scale energy consumption maps.

[0041] S105. If the current local energy consumption map covers the global target position, perform global path planning, use an octree structure, search for the optimal energy consumption in the tree space, and calculate the global energy consumption optimal path based on the path length and energy consumption.

[0042] Specifically, a local energy consumption map corresponding to the current position of the robot is obtained, wherein the local energy consumption map includes environmental energy consumption information; based on the local energy consumption map, an octree structure is used to spatially divide the global map to obtain multiple leaf nodes, each of which corresponds to a local energy consumption map block; with the global target position as the root node, an optimal path from the starting point to the root node is recursively searched in the octree space, and during the search process of the optimal path, a scalarized cost function is formed by combining the path length and the accumulated energy consumption along the way; based on the cost function, an A* heuristic search algorithm is used for path planning, and during the A* search process, each expansion A leaf node is expanded, and the local energy consumption map corresponding to the node is dynamically loaded into the memory. The local energy consumption map is used to evaluate the optimal path length and cumulative energy consumption from the node to the global target as the heuristic function value of the node; if the local energy consumption map is updated, it is determined whether the updated map affects the current path. If it does, path replanning is triggered; the searched global energy consumption optimal path is used as the robot's motion reference, and the robot is controlled to move along the planned path through the trajectory tracking control algorithm; according to the actual speed and energy consumption data of the robot, the local energy consumption map and the global path are corrected and updated in real time to form a closed-loop feedback, and the accuracy of path planning and energy consumption prediction is continuously improved.

[0043] In one embodiment, the coverage of the local area environmental energy consumption map is continuously expanded by real-time updating and expansion. When it is detected that the local energy consumption map has covered the global target position, global path planning is carried out based on the local map. The environmental energy consumption information contained in the local energy consumption map is used to obtain a global energy consumption optimal path. The energy consumption optimal path search is performed on a global scale. The global map is spatially divided using an octree structure. Each leaf node corresponds to a local energy consumption map block, and non-leaf nodes represent the aggregation of adjacent local map blocks. The local map block corresponding to the target area is quickly located through the tree structure, and the path search range is narrowed to the global target position. The root node recursively searches for an optimal path from the starting point to the root node in the octree space. During the search process, the two optimization goals of path length and cumulative energy consumption along the way are combined by weighted summation to form a scalar cost function. The selection of weight coefficients is determined according to the robot's task requirements and energy constraints. For long-distance cruising tasks, the energy consumption weight is increased; for tasks with high timeliness requirements, the path length weight is increased. Based on the cost function, the A* heuristic search algorithm is used for path planning. Among them, the evaluation function combines the local energy consumption map information and the Euclidean distance heuristic to estimate the lower bound of energy consumption from the current node to the target node.The cost function is calculated based on the length of the path that has been traveled and the cumulative energy consumption. During the A* search process, each time a leaf node is expanded, the local energy consumption map corresponding to the node is dynamically loaded into the memory. The local energy consumption map is used to evaluate the optimal path length and cumulative energy consumption from the node to the global target, which is used as the heuristic function value of the node to guide the search direction and realize the dynamic fusion of global path planning and local energy consumption map. The calculation of cumulative energy consumption adopts a recursive method. Each time a new node is expanded, its energy consumption value is added to the energy consumption value of the parent node, and the energy consumption prediction value of the current node is added to obtain the cumulative energy consumption from the starting point to the current node. When the local energy consumption map is updated, the cumulative energy consumption value must also be updated accordingly. During the search process, the position of the robot needs to be monitored in real time. If the robot deviates from the current optimal path, the path needs to be replanned, and the current position of the robot is used as the new starting point. Repeat the above octree search process to generate a new energy-optimal path. Through this dynamic update strategy, the real-time position changes of the robot can be adapted. At the same time, considering the complexity and uncertainty of the actual environment, a certain amount of energy consumption redundancy needs to be reserved during path planning, that is, when calculating the cumulative When calculating energy consumption, multiply the energy consumption prediction value of the local map by a coefficient greater than 1 to cope with possible unexpected situations. When the environment changes and there is a deviation between the energy consumption prediction value of the local map and the actual value, the robustness of the path planning is improved. The energy consumption redundancy coefficient is selected according to the battery capacity of the robot and the energy consumption parameters per unit mileage, and an energy consumption safety margin is set. Finally, the searched global energy consumption optimal path is used as the movement reference of the robot. The robot is controlled to move along the planned path through the trajectory tracking control algorithm. The trajectory tracking control algorithm includes the PurePursuit algorithm and the Stanley algorithm. The PurePursuit algorithm calculates a tracking point based on the current position and preview distance of the robot, and generates a steering control instruction based on the azimuth angle of the tracking point relative to the robot; the Stanley algorithm calculates the front wheel turning angle through the lateral deviation and heading deviation of the robot relative to the path to achieve trajectory tracking control. During the trajectory tracking process, the local energy consumption map and the global path are corrected and updated in real time according to the actual speed and energy consumption data of the robot to form a closed-loop feedback, and the accuracy of path planning and energy consumption prediction is continuously improved.

[0044] In global path planning, the global energy consumption map is spatially divided and hierarchically managed through an octree structure. Each leaf node corresponds to a local energy consumption map block of 10m×10m in size. Non-leaf nodes aggregate the energy consumption information of child nodes by averaging. The global target position is taken as the root node, and the A algorithm is used to search for the optimal path in the tree space. Among them, the evaluation function f(n)=g(n)+h(n), g(n) is the actual cost from the starting point to the current node n, including the path length and cumulative energy consumption, h(n) is the estimated cost from the current node n to the target node, which consists of two items: the lower bound of the Euclidean distance and the lower bound of the energy consumption. The lower bound of the energy consumption is obtained by querying the local energy consumption map block to which the current node belongs; in the actual cost g(n), the weights of the path length and the cumulative energy consumption are set to 0.6 and 0.4 respectively to balance the driving distance and energy consumption. During the search process, each time a node is expanded, its corresponding local energy consumption map block is dynamically loaded. According to the map data and the coordinates of the current node, through The energy consumption prediction value of the node is obtained by the trilinear interpolation method, and then it is added to the accumulated energy consumption of the parent node to update the actual cost g(n) of the current node. At the same time, considering the uncertainty of the environment, when calculating the accumulated energy consumption, the local map energy consumption prediction value is multiplied by an energy consumption redundancy coefficient of 1.2 to reserve a 20% energy consumption margin to improve the robustness of path planning. After obtaining the global planning path, the Stanley algorithm is used for trajectory tracking control. According to the lateral deviation and heading deviation of the robot relative to the path, the front wheel turning angle δ=ψ+arctan(ke) is calculated, where ψ is the heading deviation, e is the lateral deviation, and k is the gain coefficient. It is dynamically adjusted according to the robot's speed and path curvature, and the value range is usually 0.1 to 0.5. During the trajectory tracking process, the actual position and energy consumption data of the robot are collected at a frequency of 10Hz. On the one hand, it is used to update the local energy consumption map. Through the Kalman filtering method, the actual energy consumption data is fused with the predicted value to generate a more accurate energy consumption estimate.

[0045] S106. If the current local energy consumption map has not yet covered the global target position, navigation decisions are made only within the scope of the local energy consumption map. The current position of the robot is used as the root node, and a random sampling tree is generated in the neighborhood. According to the maximum a posteriori estimate of the water flow direction, water flow speed, robot posture and energy consumption, the branch with the lowest energy consumption cost is selected as the local energy consumption optimal path. At the same time, the energy consumption and water flow change trends in the branch extension direction are predicted to guide the growth direction of the tree.

[0046] Specifically, when it is detected that the current local energy consumption map has not yet covered the global target position, it switches to the navigation decision mode within the local map; based on the environmental information contained in the local map and the robot's state estimation, the optimal energy consumption path is searched in the local range; a tree structure is generated by random sampling, in which each node records the position, direction and energy consumption cost of the sampling point; the energy consumption cost of each node is calculated by combining the water flow direction, water flow speed, robot posture and energy consumption factors; among all the leaf nodes of the random sampling tree, the node with the lowest energy consumption cost is selected, and its corresponding path from the root node to the node is connected to the root node. The path is taken as the local energy consumption optimal path; after selecting the local optimal path node, the water depth, water flow velocity, and terrain undulation in the rectangular area are interpolated or extrapolated by using the Gaussian process regression method with the node as the center; the prediction result is used as prior information to guide the sampling direction of subsequent nodes, and the area with more favorable prediction map information is explored first, and the local energy consumption optimal path is continuously expanded and updated; when the distance between the current position of the robot and the map boundary is less than a certain threshold, new environmental information is collected through lidar and sonar sensors, and the factor graph optimization method is used to integrate the new information into the original map.

[0047] In one embodiment, when it is detected that the current local energy consumption map has not yet covered the global target position, that is, when the local map range is insufficient to support the path planning to reach the target position, it is necessary to switch to the navigation decision mode within the local map. According to the environmental information contained in the local map and the state estimation of the robot, the energy-optimal path is searched within the local range to guide the movement of the robot. With the current position of the robot as the root node, a tree structure is generated by random sampling within the neighborhood covered by the local map. The number of nodes in the tree is adaptively adjusted according to the size and resolution of the local map, and the sampling interval is adjusted according to the robot's movement ability and the complexity of the environment. The length of the tree is determined by the distance, usually between 0.5m and 2m. The edges between tree nodes are connected by Dubins curves to meet the kinematic constraints of the robot. Through random sampling, feasible movement paths are explored more evenly in the local map. In the process of generating random sampling trees, the following factors are considered to affect the path cost: First, the direction of the water flow. Sailing downstream is more energy-efficient than sailing upstream. Therefore, the sampling direction with an angle of less than 90° with the water flow direction is preferred; second, the water flow speed. The greater the flow speed, the greater the resistance and energy consumption. The energy consumption cost is weighted in a way that is proportional to the square of the flow speed; third, the robot's posture, Excessive deviation from the expected heading and pitch angle will increase energy consumption. Thresholds are set for heading deviation and pitch angle deviation respectively. The energy consumption cost corresponding to the posture state exceeding the threshold is multiplied by a penalty coefficient greater than 1; Fourth, the energy consumption of the robot. A polynomial fitting model or a neural network model based on speed and propeller speed is used. According to the real-time state parameters and historical energy consumption data, the energy consumption prediction model is fitted and updated online to estimate the future energy consumption of the sampling node. For each node of the random sampling tree, its energy consumption cost is calculated. A path is preset to consist of n nodes. The energy consumption cost of each node and edge is Jv and Je respectively. The total cost of the path is J=w1∑Jv+w2∑Je, where w1 and w2 are the weight coefficients of node cost and edge cost, which are adjusted according to the actual effect. The node energy consumption cost Jv is the weighted sum of various influencing factors, that is, Jv=a1Jwater+a2Jspeed+a3Jpose+a4Jenergy, where a1 to a4 are the weight coefficients of various factors, jwater, jspeed, jpose and jenergy represent the cost of underwater environment on energy consumption, the cost of robot movement speed on energy consumption, the cost of robot posture on energy consumption, and the cost of robot battery power and power generation power on energy consumption respectively;The edge cost Je mainly considers the smoothness and length of the path, and is measured by the average curvature and length of the Dubins curve where the edge is located. In this way, the comprehensive energy consumption cost of each node is quantitatively evaluated. Among all the leaf nodes of the random sampling tree, the node with the lowest energy consumption cost is selected, and the corresponding path from the root node to the node is used as the local energy consumption optimal path. This path not only considers the environmental factors within the local map range, such as water flow and terrain, but also combines the robot's own state and energy consumption prediction, and can achieve optimal energy consumption on a local scale. In order to further expand the search range of the random sampling tree and improve the purpose of the search, after selecting the local optimal path node, the map information within a certain distance in the future is predicted along the branch direction of the node. With the node as the center, a rectangular area with a length of L and a width of W is used to represent the prediction range. The size of L and W is determined according to the robot's perception ability and movement speed. In this rectangular area, the Gaussian process regression method is used to predict the known map information and node status. The water depth, water flow velocity, and terrain undulation are interpolated or extrapolated, and the prediction results are used as prior information to guide the sampling direction of subsequent nodes, give priority to exploring areas with more favorable prediction map information, and continuously expand and update the local energy consumption optimal path. As the robot continues to move, the coverage of the local energy consumption map is also expanding. When the distance between the current position of the robot and the map boundary is less than a certain threshold (twice the map resolution), the map expansion mechanism is triggered, and new environmental information is collected through lidar and sonar sensors. The factor graph optimization method is used to integrate the new information into the original map. The factor graph optimization constructs observation factors and motion factors to describe the constraint relationship between sensor observation and map, and the constraint relationship between robot motion and map, respectively. Under the maximum a posteriori probability criterion, the optimal configuration of the factor graph is solved, the map is updated and the robot's posture is corrected. The fused map not only covers the new environmental area, but also eliminates the accumulated error between the old map and the actual environment, dynamically maintaining the accuracy and real-time performance of the map. In the motion control of the robot, it is also necessary to consider how to achieve a smooth transition from local path planning to global path planning. When the coverage of the local map reaches a certain proportion (50%), the time and spatial scales of the local path planning are gradually increased, and matched and integrated with the global path. Through trajectory interpolation and smoothing methods, a transition trajectory connecting the local path and the global path is generated, and the control instructions are dynamically adjusted according to the confidence of the two plans, finally achieving seamless switching from local map navigation to global map navigation.

[0048] In the process of generating random sampling trees of local maps, the number of tree nodes is adaptively determined according to the size of the local map. The formula is: the number of nodes n=ksqrt(S), where S is the area of ​​the local map in square meters, and k is the proportional coefficient, which is between 5 and 10. For example, when the size of the local map is 50m×50m, k=8, then the number of nodes n=8sqrt(2500)=400, and the edge sampling interval is determined according to the robot's minimum turning radius r and maximum speed v_max, generally taken as d=max(r,v_maxt_s), where t_s is the sampling time interval, usually 0.5s to 1s. In the calculation of node energy consumption cost, the weight coefficient of each influencing factor is fitted by simulation and actual ship experimental data using the least squares method or gradient descent optimization algorithm. And adjust to make the calculated energy consumption cost as close as possible to the actual energy consumption value. For example, in a set of experimental data, the correlation coefficients of different factors and actual energy consumption are 0.6 for water flow direction, 0.3 for water flow speed, 0.2 for heading deviation, and 0.1 for pitch angle deviation. Then, their weight coefficients a1 to a4 are initialized to the above correlation coefficients, and then continuously updated through data fitting to finally obtain a set of optimal weight coefficients. In the prediction sampling stage of local path search, the prediction of unknown area map information adopts Gaussian process regression (GPR) algorithm. Taking water depth prediction as an example, the water depth value of known map grid is used as training sample, the coordinates (x, y) of each grid is used as feature vector, and the corresponding water depth value z is used as target value. Gaussian process regression model z=f(x, y)+ε is constructed, where f is the water depth distribution function to be estimated, and ε is Gaussian white noise. By maximizing the marginal likelihood function, the posterior distribution of f is obtained, which is used to predict the water depth value at any position. For a 30m×30m local map, if it is divided into 0.5m×0.For a 5m grid, there are 3600 grid nodes. One-third of the nodes (1200) are taken as training samples to better fit the water depth distribution function and use it to predict the water depth values ​​at other locations. The confidence interval of the prediction result is used to measure uncertainty. Based on the predicted map information, the sampling direction and weight of the subsequent path search are corrected. For example, the sampling density is reduced in areas where the water depth is predicted to be shallow, and the sampling density is increased in areas where the water depth is predicted to be deep, so as to improve the search efficiency. In the factor graph optimization method, taking the local map as an example, the state variables are the robot pose (x, y, θ) and the feature point position (l_x, l_y), and the observation variable is the lidar ranging value d and IMU readings (a_x, a_y, w_z), the observation factor is expressed as d=sqrt((x-l_x)^2+(y-l_y)^2)+σ_d, where σ_d is the ranging noise; the motion factor is expressed as x'=x+vcosθΔt+σ_x, y'=y+vsinθΔt+σ_y, θ'=θ+w_zΔt+σ_θ, where (x', y', θ') is the new robot pose, v is the linear velocity, Δt is the time interval, σ_x, σ_y, σ_θ are motion noises, and the maximum a posteriori estimate of the state variables is obtained by constructing a factor graph and applying the LM algorithm for optimization, updating the map and correcting the robot pose. .

[0049] S107, discretize the above-mentioned global or local energy consumption optimal path into a control waypoint sequence, generate a control instruction, and execute it through a motion control module. The motion control adopts an adaptive PID controller to adjust the control parameters online according to the deviation between the actual energy consumption and the expected energy consumption while tracking the desired waypoint.

[0050] Specifically, according to the kinematic characteristics and control requirements of the underwater robot, the global or local energy consumption optimal path obtained by planning is discretized to obtain a series of discrete control waypoints, each of which contains the desired position, speed, and heading information; according to the discretized control instruction sequence, PID control, sliding mode control, and adaptive control algorithms are used to calculate the output of the control thruster actuator to achieve tracking control of the underwater robot's motion; in view of the complexity and variability of the underwater environment, the idea of ​​adaptive control is introduced, and fuzzy control or neural network methods are used to adjust the controller parameters online according to the position deviation and speed deviation between the robot and the desired waypoint; an energy consumption sensor is established. The function relationship between energy consumption and speed, acceleration and heading state is determined by using the robot dynamics equation and thruster model or the data-based black box modeling method. In the motion control process, the actual energy consumption of the underwater robot is collected and calculated in real time, and compared with the predicted value of the energy consumption perception model to obtain the energy consumption deviation. The energy consumption deviation is used as feedback, and the speed and acceleration control inputs of the underwater robot are optimized by using the optimal control or adaptive dynamic programming method, so that the actual energy consumption approaches or tracks the expected energy consumption. The actual motion trajectory of the underwater robot is recorded and analyzed, and the control performance and energy consumption optimization effect are evaluated by calculating the maximum deviation and root mean square error evaluation indicators.

[0051] In one embodiment, the global or local energy consumption optimal path obtained by planning is discretized, and the continuous path curve is sampled into a series of discrete control waypoints according to the kinematic characteristics and control requirements of the robot. Each waypoint contains the expected position, speed, and heading information to form a discrete control instruction sequence as the input of the motion control module to guide the movement of the robot. The discretization sampling interval of the control instruction sequence needs to comprehensively consider the control accuracy, energy consumption optimization, and calculation load factors. After weighing various requirements, a reasonable sampling interval is determined, and the discretized control instruction sequence is sent to the motion control module. The control module uses PID control, sliding mode control, and adaptive control algorithms according to the expected state in the instruction. The output of the servo and thruster actuators is calculated to achieve tracking control of the robot's motion. The selection of the control algorithm requires comprehensive consideration of the robot's dynamic characteristics, control accuracy, and robustness requirements. If necessary, multiple algorithms can be combined to give full play to their respective advantages. For example, PID control and adaptive control can be combined to achieve high-precision and highly adaptable motion tracking control by utilizing the simplicity and effectiveness of PID control and the parameter adjustment capability of adaptive control. Considering the complexity and variability of the underwater environment, the actual motion trajectory of the robot will inevitably deviate from the expected path. At the same time, due to environmental interference and measurement errors, the actual energy consumption may also be inconsistent with the expected energy consumption. In order to overcome the influence of these uncertain factors and improve motion control and energy consumption optimization, The idea of ​​adaptive control and energy consumption feedback control is introduced to adjust the controller parameters online. The adaptive PID controller adjusts the PID parameters in real time according to the position deviation and speed deviation between the robot and the expected waypoint. Taking the position deviation as an example, when the deviation is large, the proportional coefficient is appropriately increased to speed up the tracking speed; when the deviation is small, the proportional coefficient is reduced to avoid overshoot. The adaptive adjustment law of PID parameters is set using fuzzy logic or neural network intelligent algorithm. For example, using a fuzzy PID controller, the PID parameters are adjusted online according to the deviation size and change rate through membership functions and rule tables to achieve smooth and accurate trajectory tracking. Energy consumption feedback control is based on the deviation between the actual energy consumption of the robot and the expected energy consumption. The dynamic control strategy is optimized and adjusted. Through the energy consumption perception model, the dynamic equation and thruster model of the robot are analyzed to obtain the functional relationship between energy consumption and speed, acceleration and heading state quantities; then, in the motion control process, the actual energy consumption of the robot is collected and calculated in real time, and compared with the predicted value of the energy consumption perception model to obtain the energy consumption deviation; finally, the energy consumption deviation is used as feedback, a closed-loop control strategy is set, and the speed and acceleration control inputs of the robot are optimized to make the actual energy consumption approach or track the expected energy consumption. For example, the optimal control method based on the Pontryagin maximum principle is adopted to transform the energy consumption minimization problem into a two-point boundary value problem, solve the optimal control law, and obtain a state feedback control strategy;The model-free adaptive control method is also used to adjust the controller parameters online according to the energy consumption deviation to achieve energy consumption tracking. While controlling the motion, it is also necessary to record and analyze the actual motion trajectory of the robot, calculate the maximum deviation and root mean square error evaluation indicators, and evaluate the control performance and energy consumption optimization effect by comparing with the expected path;

[0052] When discretizing and sampling the optimal energy consumption path, the Dijkstra algorithm or the A algorithm is used to discretize the continuous path curve into a series of nodes. Each node represents a control waypoint. The distance between the nodes is determined according to the robot's maximum speed and turning radius kinematic parameters. For example, if the maximum speed is 1.5m / s and the maximum turning radius is 5m, the node spacing is 1.5m / s×1s=1.5m. At the same time, it is necessary to ensure that the turning angle between adjacent nodes does not exceed the maximum turning angle allowed by the turning radius of 5m, which is obtained through geometric calculation. In practical applications, it is also necessary to consider the control cycle and calculation load factors, balance the control accuracy and real-time performance, and set the control instruction sending frequency to 5-10Hz. For the selection of motion control algorithm, a combination of adaptive PID control and sliding mode control is adopted. First, according to the robot's dynamic model, such as the UVMS model, the initial parameters of the PID controller are derived. For example, using the Ziegler-Nichols frequency response method, Kp= 2.5, Ki=0.8, Kd=0.1; then, adjust the PID parameters online, use fuzzy logic control rules, and dynamically adjust the PID parameters according to the waypoint tracking error and the error change rate, such as: IF the error is positive AND the error change rate is negative THENKp increases by 10%. At the same time, introduce the sliding mode control law to compensate the output of the PID controller to improve the convergence speed and robustness, such as: u_sliding=-k×sat(s), where k is the sliding mode gain and s is the sliding mode surface function ,sat is a saturation function. In energy consumption feedback control, the maximum principle is used to solve the optimal control problem. For example, for a simplified energy consumption model: J=∫(u^2)dt, where u is the control input, a Hamiltonian function is constructed: H=u^2+λ(Ax+Bu), λ is the co-state variable, and the optimal control law is solved: u*=-0.5B^T*λ. The optimal control law is substituted into the state equation and the co-state equation to obtain a two-point boundary value problem, which is solved using the numerical method of the shooting method to obtain the optimal control trajectory.

[0053] S108. During the robot's movement, the robot continuously senses the environment and builds energy consumption maps. When the actual energy consumption deviates seriously from the expected value, the local path replanning is triggered and an energy consumption compensation strategy is generated. If the environment changes significantly and the current path becomes invalid, the global planning needs to be re-executed.

[0054] Specifically, the multi-dimensional information of the terrain, landform, obstacles, water flow, and water quality of the underwater environment is obtained based on the multi-source heterogeneous sensors carried by the robot, and a combination of extended Kalman filter SLAM and graph optimization SLAM is adopted, wherein the extended Kalman filter SLAM is used to recursively update the robot's position and local map online, and the graph optimization SLAM is used to optimize the consistency of the global map at the back end. The energy consumption of each subsystem of the robot is collected and monitored in real time through the robot's power management system and energy consumption sensors, and the energy consumption perception model is updated in combination with the robot's motion state. On the basis of energy consumption mapping, the robot's current actual energy consumption is compared with the expected energy consumption in real time, and the energy consumption deviation is calculated. When the energy consumption deviation exceeds a preset threshold, the replanning of the local path is triggered. The local path replanning adopts an improved A* search algorithm. On the basis of the traditional Euclidean distance heuristic function, the heuristic information of the energy consumption distribution gradient is added to form a composite heuristic function. For the energy consumption compensation path obtained by replanning, the strategy of reducing the speed and reducing unnecessary posture adjustments is adopted, and the compensation time is appropriately extended according to the degree of energy consumption deviation to meet the kinematic and dynamic constraints of the robot. The changes in the external environment are continuously monitored. The weighted sum cost function is set in combination with the geometric parameters of the path and the semantic information of the environment. The path is quantitatively scored. When the path cost increment exceeds the preset threshold, the global path replanning is started.

[0055] In one embodiment, during the movement of the robot, the multi-source heterogeneous sensors carried by the robot, such as sonar, optical, and chemical sensors, continuously perceive and monitor the underwater environment, obtain multi-dimensional information about the terrain, landform, obstacles, water flow, and water quality of the environment, and integrate these perception information with the movement trajectory of the robot through the SLAM algorithm to dynamically update the underwater environment and energy consumption map. Considering the particularity of the underwater environment, the selection of the SLAM algorithm needs to weigh the factors of computational efficiency, accuracy, and robustness. The extended Kalman filter SLAM (EKF-SLAM) and graph optimized SLAM (GraphSLAM) are combined to utilize the high update frequency of EKF-SLAM and the high performance of GraphSLAM. The global consistency of LAM optimizes the underwater environment construction and realizes high-precision and high-robust underwater environment modeling. At the same time, through the robot's power management system and energy consumption sensors, the energy consumption of each subsystem of the robot is collected and monitored in real time, including the voltage, current, and power parameters of the propeller, controller, sensor, and computing platform electrical equipment. Combined with the robot's motion state, such as speed, acceleration, and heading, the energy consumption perception model is updated, and a real-time energy consumption perception model that maps the robot's motion and environmental state in real time is fitted and learned online. The energy consumption perception model is modeled using polynomial regression, neural network, and Gaussian process methods. It is selected based on modeling accuracy, computational complexity, and data volume factors. Taking Gaussian process regression (GPR) as an example, through Through the energy consumption data collected by the robot under different motion states and environmental conditions, a Gaussian process prior is constructed, and the kernel function is used to describe the correlation between different states. The hyperparameters of the kernel function are learned through the maximum likelihood estimation method, and then the Bayesian inference is used to predict the energy consumption distribution under unknown states. According to the uncertainty of the prediction, the model complexity and update frequency are dynamically adjusted to realize the adaptive learning of the energy consumption perception model. The learned energy consumption model is used to dynamically predict the energy consumption change trend of the robot in the future, and a real-time updated energy consumption distribution map is generated. On the basis of the energy consumption map, the current actual energy consumption of the robot is compared with the expected energy consumption in real time, and the energy consumption deviation is calculated. When the deviation exceeds a certain threshold, such as 20% of the expected energy consumption, the local path is triggered. The local path replanning is to take the current position as the starting point and re-search a local optimal path from the current position to a subsequent waypoint of the original path in the local energy consumption map, so that the expected energy consumption on the path segment matches the actual energy consumption, eliminate energy consumption deviation, and ensure the energy consumption optimality of the planned path. The local path replanning adopts the improved A* search algorithm. On the basis of the traditional Euclidean distance heuristic function, the heuristic information of the energy consumption distribution gradient is added to form a composite heuristic function, so that the search process tends to move in the direction of optimal energy consumption while pursuing the shortest path. Specifically, the energy consumption gradient is defined as the energy consumption difference between the current node and the neighboring node, and the negative gradient direction is taken as the direction of the fastest energy consumption decrease. The negative energy consumption gradient is multiplied by a coefficient as the energy consumption heuristic term.Added to the distance heuristic term, a comprehensive heuristic function is obtained to guide the node expansion order of A search. For the energy consumption compensation path obtained by local path replanning, an energy consumption compensation strategy is adopted, including reducing the speed, reducing unnecessary posture adjustments, or appropriately increasing the energy consumption compensation time when the energy consumption deviation is large, to ensure that the robot can maintain the optimal energy consumption state. In addition, the energy consumption compensation path is smoothed to meet the kinematic constraints and dynamic constraints of the robot. The B-spline curve and Bezier curve parameterized curve are used to fit the discrete waypoint sequence to generate a smooth and continuous energy consumption compensation trajectory. While smoothing, collision detection and motion constraint checks are also required to ensure that the smoothed path is within the robot. The robot must be able to move within the range of its motion capabilities and will not collide with obstacles in the environment. If a collision or constraint violation is detected, the control points of the smooth curve need to be locally adjusted until all constraints are met. After a smooth and safe compensation trajectory is generated, the motion control instructions are regenerated based on the trajectory to guide the robot to smoothly transition to the energy consumption compensation path until it returns to the original planned path. During the robot's movement, it is also necessary to continuously monitor changes in the external environment, especially major changes that affect the robot's movement and energy consumption, such as sudden water surges, terrain collapses, and obstacles. Once these changes are detected, it is necessary to evaluate their impact on the current path and re-evaluate the path cost. It is necessary to comprehensively consider the impact of the geometric parameters of the path (length, curvature) and the semantic information of the environment (terrain undulation, obstacle density) on energy consumption, set a weighted sum cost function, and quantify the path score. When the increment of the path cost exceeds a certain threshold (30% of the original path cost), it means that the current path is invalid and the global path planning needs to be retriggered. The selection of the threshold needs to strike a balance between maintaining the relative stability of the planned path and quickly responding to environmental changes. Adaptive adjustment is performed through simulation experiments and empirical data. The process of global path replanning is similar to the initial planning. It is also on a global scale, integrating environmental information and energy consumption information to search for a global energy-optimal path. However, unlike the initial planning, the replanning It is necessary to take the current position of the robot as the new starting point, and set the area caused by local environmental changes as a high-cost area, reduce the path selection priority of the area, so that the re-planned global path avoids the affected area as much as possible, and has higher environmental adaptability and robustness. The affected area is determined by referring to the obstacle distribution and terrain feature information in the environmental semantic map, and combined with the high-energy consumption area in the energy consumption distribution map for comprehensive analysis. Once a new global path is obtained, the above local planning and motion control process is repeated to generate and execute new motion control instructions, so that the robot can still track the global target and optimize energy consumption while responding to environmental changes, and achieve long-term, high-efficiency, and strong adaptive autonomous operation;

[0056] When performing underwater environment perception and modeling, the multi-beam sonar and stereo optical camera carried by the robot are used to collect point cloud data and image data at a frequency of 10 frames per second, and the robot's posture and environmental map are updated in real time through the EKF-SLAM algorithm. In EKF-SLAM, each frame of observation data is associated with the feature points in the map through the motion model and observation model to form a Gaussian distributed state vector and covariance matrix, which describes the probability distribution of the robot's posture and feature point positions. Through the prediction and update steps of the Kalman filter, the state estimation is continuously optimized, and the map is dynamically expanded according to the observation data. At the same time, loop detection and global optimization are introduced to regularly perform nonlinear optimization on the map to eliminate cumulative errors and maintain the global consistency of the map. For energy consumption modeling, the Gaussian process regression (GPR) model is selected. The robot's speed (m / s), heading angular velocity (rad / s), load power (W) motion and environmental parameters are used as input, and energy consumption power (W) is used as output. Through training with 500 sets of data samples collected under different working conditions, a GPR model with a square exponential kernel function as a priori is obtained. When predicting, given a set of new motion and environmental parameters, the GPR model obtains the posterior Gaussian distribution of energy consumption power. The mean represents the energy consumption prediction value, and the variance represents the prediction uncertainty. By continuously using the newly collected energy consumption data to update the model online, the accuracy and reliability of energy consumption prediction are adaptively improved. When replanning the local path, the energy consumption gradient-based method is used. The improved A* algorithm uses the current position of the robot as the center of the search space, covering a range of 100 meters around the robot, with a resolution of 1 meter. The comprehensive heuristic function takes the weighted sum of distance heuristic and energy gradient heuristic, that is, f(n)=w_d·d(n)+w_g·g(n), where d(n) represents the Euclidean distance from node n to the target point, g(n) represents the projection length of node n in the direction of the negative energy gradient, w_d and w_g are the weights of the two heuristic items, which are adjusted according to the focus of the re-planning task. For example, in the case of tight energy consumption, the weight of w_g is increased to pursue energy consumption optimization; in the case of urgent tasks, the weight of w_d is increased to pursue the shortest path. When smoothing the obtained local path, use The cubic B-spline curve uses discrete waypoints on the trajectory as control points to generate a smooth, curvature-continuous parametric curve. By solving the acceleration minimization problem of the sampling points on the trajectory, the position of the control points is optimized. While satisfying the motion constraints (maximum speed 1m / s, maximum acceleration 0.2m / s²), the curvature of the trajectory changes as smoothly as possible to reduce the robot's manipulation. Finally, when evaluating the failure risk of the global path, each node on the path is quantitatively evaluated for obstacle density, terrain complexity, and energy consumption over-limit risk factors to obtain a set of normalized risk values. The risk values ​​of each node are then weighted and summed to obtain the comprehensive risk score of the path. For example, if the comprehensive risk score of a path is 0.6, and the threshold is set to 0.5, then the failure probability of the path is high, and global replanning is required. When setting the threshold, balance the stability and adaptability of the path. Too low a threshold will cause frequent replanning, while too high a threshold will miss the best planning opportunity. Through simulation statistical analysis, a reasonable threshold interval is obtained. For the avoidance of major environmental change areas, refer to the obstacle labels (reefs, shipwrecks) and danger zone labels (strong water flow, whirlpool) in the semantic map, and check the distance between these areas and the global path. If the shortest distance is less than the safety margin (50 meters), the area is regarded as a high-risk area. During replanning, a repulsive potential field is set to keep the new path away from the area until the shortest distance is greater than the safety margin. .

[0057] So far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it is easy for those skilled in the art to understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

[0058] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and rules of the present invention shall be included in the protection scope of the present invention.

Claims

1. An underwater robot autonomous navigation method based on energy consumption perception, characterized in that: S101. Acquire underwater environment parameters, including water velocity vector field, water pressure distribution, and obstacle distribution map, based on sensor fusion data carried by the robot. Combined with robot state parameters, including speed, posture, and energy consumption data, construct a multi-dimensional spatiotemporal energy consumption perception model. S102. For the energy consumption perception model, a graph-based SLAM algorithm is used to construct a factor graph model including posture nodes and energy consumption nodes, where the posture nodes represent the position and direction of the robot in the environment, the energy consumption nodes represent the energy consumption value under the posture, and the edges between the nodes represent the posture constraints and energy consumption constraints; S103, combining the factor graph model and the energy consumption perception model to calculate the energy consumption increment between postures, forming an energy consumption increment factor, and optimizing the factor graph method together with the traditional vision and odometer factors to solve the maximum a posteriori estimation of posture and energy consumption; S104, fusing the maximum a posteriori estimate of the position and energy consumption with the robot state parameters to obtain a local area environmental energy consumption map including the spatial distribution of environmental parameters and the estimated energy consumption of each position, and updating the local energy consumption map in real time according to the expansion of the robot exploration area; S105, if the current local energy consumption map covers the global target position, perform global path planning, use an octree structure, search in the tree space with the optimal energy consumption as the goal, and calculate the global energy consumption optimal path based on the path length and energy consumption; S106. If the current local energy consumption map has not yet covered the global target position, navigation decisions are made only within the scope of the local energy consumption map. The current position of the robot is used as the root node, and a random sampling tree is generated in the neighborhood. According to the maximum a posteriori estimate of the water flow direction, water flow speed, robot posture and energy consumption, the branch with the lowest energy consumption cost is selected as the local energy consumption optimal path. At the same time, the energy consumption and water flow change trends in the branch extension direction are predicted to guide the growth direction of the tree. S107, discretizing the above-mentioned global or local energy consumption optimal path into a control waypoint sequence, generating a control instruction, and executing it through a motion control module. The motion control adopts an adaptive PID controller, and while tracking the desired waypoint, the control parameters are adjusted online according to the deviation between the actual energy consumption and the expected energy consumption; S108. During the robot's movement, the robot continuously senses the environment and builds energy consumption maps. When the actual energy consumption deviates seriously from the expected value, the local path replanning is triggered and an energy consumption compensation strategy is generated. If the environment changes significantly and the current path becomes invalid, the global planning needs to be re-executed.

2. The method for autonomous navigation of an underwater robot based on energy consumption perception according to claim 1, characterized in that: The step S101 uses Kalman filtering and Bayesian estimation multi-sensor data fusion algorithm to fuse the sensor data to obtain underwater environmental parameters.

3. The method for autonomous navigation of an underwater robot based on energy consumption perception according to claim 1, characterized in that: The step S102 adopts a graph-based SLAM algorithm to construct a factor graph model including pose nodes and energy consumption nodes, uses an energy consumption perception model to calculate the constraint relationship between adjacent energy consumption nodes, constructs an energy consumption factor, and introduces energy consumption information into the factor graph model.

4. The method for autonomous navigation of an underwater robot based on energy consumption perception according to claim 1, characterized in that: The step S103 constructs an energy consumption increment factor according to the factor graph model and the energy consumption perception model; The graph optimization library is used to minimize the sum of squared errors of all factors.

5. The method for autonomous navigation of an underwater robot based on energy consumption perception according to claim 1, characterized in that: The step S104 uses a Gaussian process regression method to establish a nonlinear mapping relationship between the state parameters and the environmental parameters based on the robot's state parameters and environmental parameters, obtains the parameter space distribution, and uses the spatial correlation and temporal continuity of energy consumption for smoothing. A sliding window strategy that dynamically adjusts the window size based on the robot's movement speed and the size of the exploration area is used. By setting thresholds for speed and area size, the size of the local map window is dynamically adjusted, and environmental factors are introduced as prior factors into the factor graph to construct a constraint relationship between environmental factors and posture and energy consumption factors.

6. The method for autonomous navigation of an underwater robot based on energy consumption perception according to claim 1, characterized in that: The step S105 uses an octree structure to spatially divide the global map according to the local energy consumption map to obtain leaf nodes, each leaf node corresponding to a local energy consumption map block; the cost function uses an A* heuristic search algorithm for path planning.

7. The method for autonomous navigation of an underwater robot based on energy consumption perception according to claim 1, characterized in that: The step S106 uses the Gaussian process regression method to interpolate or extrapolate the water depth, water flow velocity, and terrain undulation in the rectangular area. When the distance between the robot's current position and the map boundary is less than a threshold, new environmental information is collected through lidar and sonar sensors, and the factor graph optimization method is used to integrate the new information into the original map.

8. The method for autonomous navigation of an underwater robot based on energy consumption perception according to claim 1, characterized in that: In the step S107, according to the control requirements of the underwater robot, the planned global or local optimal energy consumption path is discretized to obtain discrete control waypoints, each of which contains the desired position, speed, and heading information; according to the discretized control instruction sequence, PID control, sliding mode control, and adaptive control algorithms are used, and the energy consumption deviation is used as feedback. The speed and acceleration control inputs of the underwater robot are adjusted by optimal control or adaptive dynamic programming methods; and the evaluation indicators are calculated by calculating the maximum deviation and root mean square error.

9. The method for autonomous navigation of an underwater robot based on energy consumption perception according to claim 1, characterized in that: The S108 step obtains multi-dimensional information of the terrain, topography, obstacles, water flow, and water quality of the underwater environment based on the multi-source heterogeneous sensors carried by the robot, and adopts a combination of extended Kalman filter SLAM and graph optimization SLAM, wherein the extended Kalman filter SLAM is used to recursively update the robot's posture and local map online, and the local path replanning adopts an improved A* search algorithm.

Images