Path Planning Method and System for an Autonomous Vehicle

Abstract

The present invention provides a path planning method and system for an autonomous underwater vehicle, which relates to the technical field of path planning. The method includes obtaining acoustic sensor data of an underwater environment to obtain the contour of the underwater terrain and the position information of obstacles; performing dynamic environment modeling processing; using the Euclidean distance as a heuristic function to calculate the dynamic model of the underwater environment to obtain a second optimized path that meets the kinematic and dynamic constraints of the autonomous underwater vehicle; performing smoothing processing on the second optimized path through the dynamic movement primitive technology; constructing a three-dimensional map for path planning and obstacle avoidance; predicting its action trajectory in the future for a period of time; and completing the path planning of the autonomous underwater vehicle through real-time adjustment processing by an adaptive control system. The present invention can comprehensively consider the dynamic characteristics of the underwater environment and the movement constraints of the autonomous underwater vehicle, can construct a three-dimensional map containing obstacle and terrain information in real time, and can predict the future movement trajectory of obstacles.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular, to a path planning method and system for an automatically driving vehicle. Background Art

[0002] In the fields of ocean exploration and resource development, the application of autonomous underwater vehicles (AUVs) is becoming increasingly widespread, and their path planning technology has become the key to achieving efficient and safe operations. Current path planning technologies mainly rely on static maps and simple obstacle avoidance algorithms, and these methods often cannot adapt to the dynamic changes in the underwater environment, such as the uncertainty of water flow, the movement of obstacles, and the variability of complex terrains.

[0003] In addition, existing path planning methods rarely consider the kinematic and dynamic characteristics of AUVs themselves, resulting in paths planned that are difficult to execute precisely in practical applications and may even lead to safety accidents. Although there have been some attempts to improve this situation by introducing advanced sensors and algorithms, these technologies usually require high costs, and their reliability and stability in the actual ocean environment still need to be improved. Summary of the Invention

[0004] The purpose of the present invention is to provide a path planning method and system for an automatically driving vehicle to improve the above problems. To achieve the above purpose, the technical solutions adopted by the present invention are as follows:

[0005] In a first aspect, the present application provides a path planning method for an automatically driving vehicle, including:

[0006] Obtain acoustic sensor data of the underwater environment, where the acoustic sensor data includes the frequency and intensity of the emitted acoustic wave, as well as the timestamp and amplitude information of the received reflected acoustic wave, and based on the difference between the first timestamp of the emitted acoustic wave and the second timestamp of the received acoustic wave, through acoustic wave propagation time calculation processing, obtain the contour of the underwater terrain and the position information of the obstacles;

[0007] Based on the contour of the underwater terrain and the position information of the obstacles, through dynamic environment modeling processing, obtain a dynamic model of the underwater environment, where the dynamic model includes the terrain, obstacle distribution, and water flow influence;

[0008] The Euclidean distance is used as a heuristic function to calculate the dynamic model of the underwater environment, and the first optimized path from the starting point to the ending point is obtained, where the first optimized path is the globally shortest optimal path calculated from the dynamic model of the underwater environment; in combination with the first optimized path, through the model predictive control algorithm for calculation and processing, a second optimized path that meets the kinematic and dynamic constraints of the autonomous vehicle is obtained, where the second optimized path is the path obtained after considering the kinematic and dynamic constraints, and the constraints include maximum speed, acceleration limit, and steering angle limit;

[0009] After the second optimized path is smoothed by the dynamic motion primitive technology, a smooth trajectory that meets the tracking of the autonomous vehicle is obtained; by acquiring the data of the lidar and inertial measurement unit, and applying the lidar-inertial simultaneous localization and mapping algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance is constructed, where the data of the inertial measurement unit includes acceleration, angular velocity, pitch angle, and roll angle;

[0010] The three-dimensional map is processed through sensor fusion perception to extract the current state information of the dynamic obstacles in the surrounding environment, and is processed by using the prediction model construction, and based on the current state and motion characteristics of the obstacles, the action trajectory in the future period of time is predicted;

[0011] According to the smooth trajectory tracked by the autonomous vehicle and the future action trajectory of the obstacles, through the real-time adjustment and processing of the adaptive control system, a stable path tracking control instruction that can adapt to different wind waves and water flow changes is obtained, thereby completing the path planning of the autonomous vehicle.

[0012] Preferably, the Euclidean distance is used as a heuristic function to calculate the dynamic model of the underwater environment, and the first optimized path from the starting point to the ending point is obtained, which includes:

[0013] The dynamic model of the underwater environment is input into the heuristic function for calculation. During the calculation process, the node with the minimum f(n) value is selected as the current node and moved from the open list to the closed list. For each neighbor node of the current node, if it is not in the closed list, calculate its f(n) value, and update its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function as needed. Repeat the above process until the target node is added to the closed list or the open list is empty. If the target node is found, construct the path from the starting point to the ending point by backtracking the parent node of each node, thereby obtaining the globally shortest optimal path to reach the target node.

[0014] Preferably, in combination with the first optimized path, through the model predictive control algorithm for calculation and processing, a second optimized path that meets the kinematic and dynamic constraints of the autonomous vehicle is obtained, which includes:

[0015] Establish the kinematic model of the autonomous vehicle. After processing through state - space representation, obtain the initial conditions of the state variables and control inputs;

[0016] Based on the initial conditions, in each control cycle, through the state prediction process of the model predictive control algorithm, obtain the state prediction sequence of the autonomous vehicle in a future period of time;

[0017] According to the state prediction sequence, introduce through cost - function definition to evaluate the quality of the path, and obtain the path - quality evaluation result;

[0018] Use numerical optimization techniques to evaluate and solve the path - quality evaluation result, obtain the control - input sequence of the autonomous vehicle in a future period of time, and apply the first control input to repeat the optimization according to the latest system state in each control cycle to achieve path tracking and adjustment, and obtain the second optimized path.

[0019] Preferably, perform smoothing processing on the second optimized path through the dynamic motion primitive technology to obtain a smooth trajectory that conforms to the tracking of the autonomous vehicle; by acquiring lidar and inertial measurement unit data, apply the lidar - inertial simultaneous localization and mapping algorithm for processing to construct a three - dimensional map of the underwater environment for path planning and obstacle avoidance, including:

[0020] According to the vertex sequence of the second optimized path, dynamically adjust the path points through a radial basis function neural network to generate a continuous smooth trajectory;

[0021] Acquire lidar data. According to the point - cloud data in the lidar data, through point - cloud processing and feature extraction, obtain the three - dimensional structure information of the environment;

[0022] Acquire inertial measurement unit data. According to the acceleration and angular - velocity information in the inertial measurement unit data, through data pre - processing and filtering, obtain the denoised inertial - measurement data set;

[0023] Through the tight coupling of the three - dimensional structure information and the inertial - measurement data set, use the simultaneous localization and mapping algorithm and combine graph - optimization technology to realize the three - dimensional modeling of the underwater environment, where the three - dimensional modeling includes the construction and update of the underwater map, thereby generating a three - dimensional map of the underwater environment for path planning and obstacle avoidance.

[0024] Preferably, process the three - dimensional map through sensor - fusion perception, extract the current - state information of dynamic obstacles in the surrounding environment, use the prediction - model construction for processing, and based on the current state and motion characteristics of the obstacles, predict their action trajectories in a future period of time, including:

[0025] Identify and separate the feature points of dynamic obstacles using the Random Sample Consensus (RANSAC) algorithm. By randomly selecting data points and fitting a model, calculate the residuals between the model and all observed data, and judge the size of the residuals compared with a preset threshold. If the residuals are less than the given threshold, they are judged as inliers. Aggregate all the obtained inliers to get the inlier set of dynamic obstacles, where the inlier set includes the feature point information of the dynamic obstacles.

[0026] Based on the inlier set, use a support vector machine to build a model to obtain the motion feature model of the dynamic obstacle. The motion feature model is used to predict the future state of the obstacle. The support vector machine separates different classes of data by finding an optimal hyperplane in the feature space.

[0027] Based on the motion feature model of the dynamic obstacle, use a Kalman filter to dynamically update the expected trajectory of the obstacle in the future for a period of time. The expected trajectory includes position and velocity information.

[0028] Preferably, according to the smooth trajectory tracked by the autonomous vehicle and the future action trajectory of the obstacle, through real-time adjustment and processing by the adaptive control system, a stable path tracking control instruction that can adapt to different wind waves and water flow changes is obtained, thereby completing the path planning of the autonomous vehicle, including:

[0029] Based on the smooth trajectory tracked by the autonomous vehicle, the future action trajectory of the obstacle, and the preset dynamic model of the autonomous vehicle, through the integrated processing of real-time wind wave and water flow data, the motion state of the autonomous vehicle under the influence of wind waves and water flow is obtained. The adaptive control algorithm adjusts the control parameters to adapt to the dynamic changes of the environment.

[0030] According to the motion state, use the fuzzy logic control method to adjust the dynamic control strategy, and calculate the path tracking control instruction that adapts to wind wave and water flow changes, including adjusting the speed, direction, and thruster force of the autonomous vehicle, while avoiding obstacles and adapting to the changing environmental conditions, thereby completing the path planning and tracking tasks.

[0031] In a second aspect, the present application also provides a path planning system for an autonomous vehicle, including:

[0032] An acquisition module: used to acquire the acoustic sensor data of the underwater environment. The acoustic sensor data includes the frequency and intensity of the emitted acoustic wave, as well as the timestamp and amplitude information of the received reflected acoustic wave. According to the difference between the first timestamp of the emitted acoustic wave and the second timestamp of the received acoustic wave, through the calculation and processing of the acoustic wave propagation time, the contour of the underwater terrain and the position information of the obstacles are obtained.

[0033] The first processing module: It is used to obtain the dynamic model of the underwater environment through dynamic environment modeling processing based on the contour of the underwater terrain and the position information of obstacles. The dynamic model includes terrain, obstacle distribution, and water flow influence;

[0034] The calculation module: It is used to calculate the dynamic model of the underwater environment by using the Euclidean distance as a heuristic function to obtain the first optimized path from the starting point to the ending point. The first optimized path is the globally shortest and optimal path calculated from the dynamic model of the underwater environment; Combining the first optimized path, through the calculation and processing of the model predictive control algorithm, a second optimized path that conforms to the kinematics and dynamics constraints of the autonomous vehicle is obtained. The second optimized path is the path obtained after processing considering the kinematics and dynamics constraints, where the constraints include maximum speed, acceleration limit, and steering angle limit;

[0035] The second processing module: It is used to smooth the second optimized path through the dynamic motion primitive technology to obtain a smooth trajectory suitable for the autonomous vehicle to track; By acquiring lidar and inertial measurement unit data, applying the lidar-inertial simultaneous localization and mapping algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance is constructed. The inertial measurement unit data includes acceleration, angular velocity, pitch angle, and roll angle;

[0036] The prediction module: It is used to process the three-dimensional map through sensor fusion perception, extract the current state information of dynamic obstacles in the surrounding environment, construct and process using the prediction model, and predict their action trajectories in the future for a period of time based on the current state and motion characteristics of the obstacles;

[0037] The planning module: It is used to perform real-time adjustment processing through the adaptive control system according to the smooth trajectory tracked by the autonomous vehicle and the future action trajectories of the obstacles, and obtain stable path tracking control instructions that can adapt to different wind waves and water flow changes, thereby completing the path planning of the autonomous vehicle.

[0038] In a third aspect, the present application also provides a path planning device for an autonomous vehicle, including:

[0039] A memory, used to store computer programs;

[0040] A processor, used to implement the steps of the path planning method for the autonomous vehicle when executing the computer program.

[0041] In a fourth aspect, the present application also provides a readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned path planning method based on the autonomous vehicle are implemented.

[0042] The beneficial effects of the present invention are:

[0043] The present invention can comprehensively consider the dynamic characteristics of the underwater environment and the motion constraints of the AUV. By fusing the data of acoustic wave sensors, this method can construct a three-dimensional map containing obstacle and terrain information in real time and predict the future motion trajectory of the obstacles.

[0044] The present invention obtains the underwater terrain contour and obstacle position information through the data of acoustic wave sensors. Combining with dynamic environment modeling, the present invention can provide an accurate dynamic model of the underwater environment for the autonomous vehicle, so as to plan a safer and more reliable path. By using the Euclidean distance as a heuristic function, the global shortest optimal path can be calculated, and the kinematic and dynamic constraints of the autonomous vehicle are considered through the model predictive control algorithm to obtain an optimized path, improving the efficiency of path planning.

[0045] The present invention smooths the optimized path through the dynamic movement primitive technology, and can obtain a smooth trajectory that conforms to the tracking of the autonomous vehicle, improving the tracking performance of the path. Combining the data of lidar and inertial measurement unit, a three-dimensional map of the underwater environment for path planning and obstacle avoidance can be constructed, enhancing the perception ability of the complex underwater environment.

[0046] Through sensor fusion perception and prediction model construction, the present invention can accurately extract and predict the future action trajectory of dynamic obstacles, improving the ability to respond to dynamic obstacles. By using an adaptive control system to adjust the path in real time, it can adapt to different wind waves and water flow changes, ensuring the stability of path planning and the safety of the autonomous vehicle.

[0047] By integrating the data of multiple sensors and advanced algorithms, the present invention improves the autonomous decision-making and execution ability of the autonomous vehicle in the complex underwater environment, enhances the intelligent level of the system. By optimizing path planning and improving execution efficiency, it reduces energy consumption and operation costs, improving the economic benefits of the autonomous vehicle. And by real-time sensing environmental changes and quickly adjusting the path, it improves the system's response ability to emergencies and enhances the robustness of the system.

[0048] Other features and advantages of the present invention will be described in the subsequent specification, and partly become obvious from the specification, or are understood by implementing the embodiments of the present invention. The objectives and other advantages of the present invention can be realized and obtained by the structures specifically pointed out in the written specification, claims, and drawings. Brief Description of the Drawings

[0049] To more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required for use in the embodiments. It should be understood that the following drawings only show some embodiments of the present invention, and therefore should not be regarded as limiting the scope. For those of ordinary skill in the art, without creative efforts, other related drawings can also be obtained based on these drawings.

[0050] Figure 1 It is a schematic flow chart of the path planning method for the automatic driving vehicle described in the embodiments of the present invention;

[0051] Figure 2 It is a schematic structural diagram of the path planning system for the automatic driving vehicle described in the embodiments of the present invention;

[0052] Figure 3 It is a schematic structural diagram of the path planning device for the automatic driving vehicle described in the embodiments of the present invention.

[0053] In the figure: 701, acquisition module; 702, first processing module; 703, calculation module; 704, second processing module; 705, prediction module; 706, planning module; 800, path planning device for the automatic driving vehicle; 801, processor; 802, memory; 803, multimedia component; 804, I / O interface; 805, communication component. Specific embodiments

[0054] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Usually, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed present invention, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0055] It should be noted that: similar reference numerals and letters indicate similar items in the following drawings. Therefore, once an item is defined in one drawing, it does not need to be further defined and explained in subsequent drawings. At the same time, in the description of the present invention, terms such as "first" and "second" are only used for distinguishing descriptions and cannot be understood as indicating or implying relative importance.

[0056] Embodiment 1:

[0057] This embodiment provides a path planning method for an autonomous vehicle.

[0058] Refer to Figure 1 , which shows that this method includes steps S100, S200, S300, S400, S500, and S600.

[0059] S100. Obtain the acoustic sensor data of the underwater environment, where the acoustic sensor data includes the frequency and intensity of the emitted acoustic wave, as well as the timestamp and amplitude information of the received reflected acoustic wave, and based on the difference between the first timestamp of the emitted acoustic wave and the second timestamp of the received acoustic wave, through acoustic wave propagation time calculation and processing, obtain the contour of the underwater terrain and the position information of the obstacles.

[0060] It can be understood that in this step, by measuring the time difference between the emission and reception of the acoustic wave, the total travel time of the acoustic wave from the sensor to the obstacle and back to the sensor can be calculated. Since the propagation speed of the acoustic wave in the medium (such as in water) is known, the following formula can be used to calculate the distance:

[0061]

[0062] The speed of sound in the above formula is the propagation speed of the acoustic wave in the medium, the time difference is the time interval between the emission and reception of the acoustic wave, and dividing by 2 is because the acoustic wave needs to travel back and forth between the sensor and the obstacle, so as to obtain the straight-line distance from the sensor to the nearest obstacle. These data provide key spatial information for the path planning of the underwater autonomous vehicle.

[0063] In addition, it should be noted that the time difference between the emission and reception of the acoustic wave by the acoustic sensor is calculated by recording the emission time of the acoustic wave and the time when the reflected acoustic wave is received, that is, this time difference reflects the total time of the acoustic wave from the emission point to the obstacle and then back to the reception point. Among them, the propagation speed of the acoustic wave in water is known, so the distance can be calculated through the time difference and the speed of sound. Specifically, the acoustic wave propagation time measurement and processing include the following steps: emitting the acoustic wave: the acoustic sensor emits the acoustic wave, and these acoustic waves will propagate in the water and encounter obstacles; receiving the reflected acoustic wave: when the acoustic wave encounters the obstacle, they will be reflected back and received by the acoustic sensor; time difference calculation: by recording the timestamps of the emission and reception of the acoustic wave, calculate the total time of the round trip of the acoustic wave; distance calculation: using the known propagation speed of the acoustic wave in water, calculate the distance from the acoustic wave to the obstacle according to the time difference, and the formula is:

[0064]

[0065] Where v is the speed of sound, L is the propagation distance, and Δt is the time difference.

[0066] In this step, the terrain information includes features such as the undulations, gulleys, and slopes of the seabed, while the obstacle information includes the size, shape, and material of the obstacles. These pieces of information directly affect the safe navigation and mission execution of the vehicle. In the underwater environment, the relationship between the terrain and obstacles is close, and the changes in the terrain may affect the distribution and detection of obstacles. For example, on a steep slope, obstacles may be more difficult to detect due to the influence of the terrain; while on a flat seabed plain, obstacles are easier to detect. Therefore, when performing path planning, it is necessary to comprehensively consider the terrain and obstacle information to ensure that the vehicle can complete the mission safely and efficiently.

[0067] S200. Based on the contour of the underwater terrain and the position information of the obstacles, through dynamic environment modeling processing, a dynamic model of the underwater environment is obtained. The dynamic model includes the terrain, obstacle distribution, and the influence of water flow.

[0068] It can be understood that in this step, the terrain refers to the shape and features of the underwater ground, including undulations, gulleys, slopes, etc.; the obstacles include fixed or moving underwater objects, such as sunken ships, rocks, marine organisms, etc.; the influence of water flow refers to the speed, direction, and changes of the water flow. These factors will affect the movement of the self-driving vehicle and the position of the obstacles. Among them, the changes in the water flow will affect the propagation path and speed of sound waves, and thus affect the measurement of distance. In dynamic environment modeling, it is necessary to consider the influence of water flow on the data of the acoustic wave sensor to ensure the accuracy of path planning, which involves the prediction and simulation of water flow patterns, as well as the adjustment of the acoustic wave propagation path.

[0069] It should be noted that in the path planning of the self-driving vehicle, dynamic environment modeling is a comprehensive process. It includes multiple sub-models to describe the dynamic characteristics of the underwater environment. The key sub-models include the processing of acoustic wave sensor data, the modeling of obstacles and terrain, the water flow influence model, the dynamic Bayesian network model, and path planning based on this information. Among them, the acoustic wave propagation time calculation model and the distance calculation model are mentioned above. The calculation formula for describing the influence of water flow on the position of the self-driving vehicle and obstacles in the water flow influence model is as follows:

[0070] C t+1 =C t +Δt×(υ c ×cos(θ), υ c ×sin(θ))

[0071] In the formula, C t is the water flow state at time t, C t+1 is the water flow state at t + 1, υ c is the water flow speed, θ is the water flow direction, and Δt is the time difference;

[0072] The dynamic Bayesian network model provides a maneuver-based approach for trajectory prediction by combining time series data and the Bayesian network framework. In the underwater environment of this embodiment, the positions of obstacles (such as other AUVs, sunken ships, rocks, etc.) change over time. The dynamic Bayesian network model can simulate these dynamic changes and predict the future states of the obstacles, including: in the DBN, each state variable can represent the position and velocity of an obstacle at a specific time point; parameter learning methods such as maximum likelihood estimation, Bayesian methods, or EM algorithms are used to determine the state transition probability, establish a prediction formula, and a forward inference algorithm such as Kalman filtering or particle filtering is used to predict the future state of the obstacle. Furthermore, in the path planning of the AUV, the DBN model can be used to predict the future positions of the obstacles, thereby providing an obstacle avoidance strategy for the AUV. Then, by predicting the potential positions of the obstacles, the AUV can plan its path in advance to avoid collisions with the obstacles.

[0073] Each sub-model provides information about a specific aspect of the environment, and this information is integrated to form a comprehensive dynamic environment model for guiding the path planning of the autonomous vehicle. The "appearance" of the dynamic environment model is composed of the mathematical expressions of these sub-models and the relationships between them. In practical applications, these models are usually implemented as computer programs that can receive sensor data as input, process this data, and output the results of path planning. The output can be a path, speed, direction, or other control instructions for guiding the autonomous vehicle to navigate safely and effectively in a complex underwater environment.

[0074] S300. Use the Euclidean distance as a heuristic function to calculate the dynamic model of the underwater environment, and obtain a first optimized path from the starting point to the end point, where the first optimized path is the globally shortest and optimal path calculated from the dynamic model of the underwater environment; combine the first optimized path, and through the model predictive control algorithm for calculation and processing, obtain a second optimized path that conforms to the kinematic and dynamic constraints of the autonomous vehicle, where the second optimized path is the processed path considering the kinematic and dynamic limitations such as the maximum speed, acceleration, and steering angle of the autonomous vehicle.

[0075] It can be understood that step S300 includes S301, where:

[0076] S301. Input the dynamic model of the underwater environment as an input item into the heuristic function for calculation. During the calculation, select the node with the minimum f(n) value as the current node, and move it from the open list to the closed list. For each neighbor node of the current node, if it is not in the closed list, calculate its f(n) value, and update its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function as needed. Repeat the above process until the target node is added to the closed list or the open list is empty. If the target node is found, construct the path from the starting point to the end point by backtracking the parent node of each node, so as to obtain the globally shortest and optimal path to reach the target node. The calculation formula is as follows:

[0077]

[0078] In the formula, (x n , y n ) are the coordinates of the current node, (x g , y g ) are the coordinates of the target node, and h(n) is the cost estimated by the heuristic function;

[0079] f(n) = g(n) + h(n)

[0080] In the formula, g(n) is the actual cost from the starting point to the current node, h(n) is the cost estimated by the heuristic function, and f(n) is the total cost estimate of a node n, that is, the first optimized path.

[0081] It should be noted that in this step, the f(n) value is the total cost estimate of a node n, which consists of two parts: the actual cost g(n) and the heuristic cost h(n). The f(n) value is used to evaluate the total cost from the starting node to the current node and then to the target node. In path planning, the f(n) value is used to preferentially select those paths that seem most promising to reach the target node. The A* algorithm will continuously expand (i.e., explore) the node with the minimum f(n) value until the target node is reached or no further progress can be made. In the path planning of the underwater autonomous vehicle, the f(n) value is equally applicable, where: g(n) may include the length or cost of the path that has actually been traveled, and h(n) may be based on the dynamic model of the underwater environment, such as the Euclidean distance considering factors such as water flow and obstacle distribution or other appropriate heuristic functions.

[0082] It is understandable that after obtaining the globally optimal path in this step, only a theoretically shortest path is provided. This path is based on the currently known environmental information, including the positions of obstacles and terrain features. However, this path does not take into account the actual kinematic and dynamic constraints of the autonomous underwater vehicle (AUV), such as the maximum steering angle, speed limit, acceleration limit, etc. Therefore, the model predictive control algorithm is used to optimize this globally optimal path. The model predictive control algorithm generates an optimized path that meets the actual operating conditions by predicting future states and control inputs while considering the kinematic and dynamic constraints of the AUV. That is to say, the model predictive control algorithm will adjust the path provided by the heuristic function algorithm to ensure that the path is not only optimal theoretically but also feasible and safe in actual operation. In short, the globally optimal path provided by the A* algorithm is the basis for the optimization process of the model predictive control algorithm, and the model predictive control algorithm ensures that this path meets the kinematic and dynamic constraints of the autonomous underwater vehicle in actual operation, thus obtaining the final optimized path.

[0083] Meanwhile, it should be noted that during the calculation of the first optimized path, this path may not consider the actual kinematic and dynamic characteristics of the autonomous underwater vehicle, such as the maximum speed, acceleration, steering limit, etc. It is a path calculated from the dynamic model of the underwater environment using the Euclidean distance as the heuristic function, and this path is theoretically the shortest because it minimizes the straight-line distance from the starting point to the ending point. The second optimized path is obtained by further processing through the model predictive control algorithm. The model predictive control algorithm takes into account the actual kinematic and dynamic constraints of the autonomous underwater vehicle, considering the kinematic and dynamic limitations such as the maximum speed, acceleration, and steering angle of the autonomous underwater vehicle, and can also adapt to environmental changes in real time to ensure the actual feasibility and safety of the path. Therefore, the second optimized path is the path that the autonomous underwater vehicle actually executes.

[0084] It is understandable that due to the dynamic nature of the underwater environment, the globally optimal path obtained by the improved A* algorithm is usually used as the initial reference path. The final path planning needs to be adjusted in combination with real-time data to adapt to environmental changes and the real-time state of the AUV. This may involve path smoothing techniques such as Bezier curves or spline curves, as well as dynamic obstacle avoidance strategies.

[0085] It is understandable that in this step S300, S302, S303, S304, and S305 are also included, where:

[0086] S302. Establish the kinematic model of the autonomous underwater vehicle. After state-space representation processing, obtain the initial conditions of the state variables and control inputs, and its calculation formula is as follows:

[0087]

[0088] Wherein, is the rate of change of the state variable with respect to time, f is the state transition function, x is the state variable, and u is the control input;

[0089] S303. Based on the initial conditions, in each control cycle, through the state prediction process of the model predictive control algorithm, a state prediction sequence of the autonomous vehicle in the future for a period of time is obtained, and its calculation formula is as follows:

[0090] x t+k = f(x t , u t )(k = 1, 2,..., N)

[0091] Wherein, x t+k is the state of the autonomous vehicle at the k-th time step in the future starting from the current time t (where k ranges from 1 to N), k is the step index in the prediction horizon, indicating the k-th time step in the future starting from the current time t, f is the state transition function, x t is the state of the autonomous vehicle at the current time t, N is the total length of the prediction horizon, and u t is the control input applied to the autonomous vehicle at the current time t;

[0092] It should be noted that in the scenario of this step, x t+k this state vector includes information such as the position and heading of the autonomous vehicle, and x t is a vector, usually including the position coordinates and heading angle (such as θt) of the autonomous vehicle; and the control input for the autonomous vehicle may include speed (such as vt) and steering angular velocity (such as ωt), and f describes how the state of the autonomous vehicle evolves over time according to the current state xt and the control input ut. This function can be a set of differential equations or difference equations, depending on whether the system model is continuous or discrete.

[0093] S304. According to the state prediction sequence, through the introduction of a cost function definition to evaluate the quality of the path, a path quality evaluation result is obtained, where the cost function includes tracking error, change in control input, and constraint conditions, and its calculation formula is as follows:

[0094]

[0095] Wherein, w1 and w2 are weight coefficients, J is the total cost or performance index of the path, is the sum of the costs for all time steps from the current time step t to the future time step t + N, N is the total length of the prediction horizon, tracking_error(x t+k , xtarget ) is the difference between the actual state \(x_{t + k}\) of the autonomous vehicle and the desired target state \(x_{target}\), and \(control\_effort(u t+k ) is the cost of the control input \(u_{t + k}\) required to reach the desired state, and \(x t+k is the state of the autonomous vehicle at the \(k\) - th future time step starting from the current time \(t\) (where \(k\) ranges from 1 to \(N\)), and \(x target is the desired target state at time step \(t + k\), and \(u t+k is the control input at time step \(t + k\);

[0096] It should be noted that the tracking error refers to the deviation between the actual path of the autonomous vehicle and the predetermined target or reference path. In the model predictive control algorithm, the tracking error is calculated by comparing the actual position and heading angle of the autonomous vehicle at each time step with the desired target position and heading angle. The smaller the tracking error, the more accurately the autonomous vehicle can follow the predetermined path. Among them, the change in the control input refers to the degree of change in the control input (such as speed and steering angle) required to guide the autonomous vehicle along the desired path during the path planning process. In the model predictive control algorithm, the smoothness of the control input is usually considered to avoid frequent or drastic changes, ensuring the smooth operation of the autonomous vehicle and the comfort of operation. Also, the constraint conditions refer to the physical or operational limitations that the autonomous vehicle needs to abide by during the movement process, which may include the maximum speed, maximum acceleration, maximum steering angle, etc. Therefore, in the model predictive control algorithm, the constraint conditions ensure that the generated path is not only theoretically optimal but also feasible in actual operation and does not exceed the operation limit of the autonomous vehicle.

[0097] Therefore, the model predictive control algorithm comprehensively considers the tracking error, the change in the control input, and the constraint conditions by defining a cost function to generate an optimized path that not only conforms to the kinematic and dynamic constraints of the autonomous vehicle but also closely follows the desired path. This process usually involves numerical optimization techniques such as quadratic programming to solve the optimal control input sequence. By minimizing the cost function \(J\), the model predictive control algorithm can find an optimized path that has the minimum tracking error and control input change while satisfying all constraints.

[0098] S305. Use numerical optimization techniques to evaluate and solve the path quality assessment results to obtain the control input sequence of the autonomous vehicle for a future period of time, and apply the first control input to repeat the optimization according to the latest system state in each control cycle to achieve path tracking and adjustment, and obtain the second optimized path.

[0099] It should be noted that according to the path quality assessment results, the model predictive control algorithm generates a control input sequence. This sequence is obtained by optimizing the cost function J based on the current state and the prediction model. The cost function J includes the tracking error, the change in the control input, and the constraint conditions. By minimizing J, the control input sequence for the autonomous driving vehicle in the next period of time is determined. This step ensures that the path is not only theoretically optimal but also feasible in actual operation. The model predictive control algorithm calculates a series of control instructions in each control cycle. These instructions are used to guide the movement of the autonomous driving vehicle in the next period of time. In the actual control process, only the first control instruction in the control sequence is applied, and then the optimization process is repeated based on the latest system state to achieve a rapid response to environmental changes. After applying the first control input, the autonomous driving vehicle starts to drive along the path. During the driving process, the model predictive control algorithm dynamically adjusts the path according to the real-time state information and environmental feedback to stay on the predetermined path or adapt to environmental changes, that is, according to the real-time state information and environmental feedback, it adjusts its motion trajectory to stay on the predetermined path or adapt to environmental changes, obtaining the second optimized path. The second optimized path is the path processed by the model predictive control algorithm. It not only considers the shortest path from the starting point to the ending point but also takes into account the actual motion capabilities of the autonomous driving vehicle, such as the maximum speed, acceleration limit, and steering ability, ensuring the actual feasibility and safety of the path.

[0100] In this step, the whole process includes model establishment. That is, the model predictive control algorithm establishes the kinematic model of the autonomous driving vehicle, which usually includes the state variables of the vehicle (such as position, speed, and heading angle) and the control inputs (such as front wheel angle and speed). The state and input can be represented by a state space model. Then, for the prediction model, in each control cycle, the model predictive control algorithm uses the current state and control input to predict the state in the next period of time, which is usually achieved by constructing a state transition matrix. Next is the construction of the optimization problem: The model predictive control algorithm evaluates the quality of the path by defining a cost function, which usually includes the tracking error, the change in the control input, and the constraint conditions. During the optimization process, the model predictive control algorithm also needs to consider the kinematic and dynamic constraints, such as the maximum speed, acceleration, and steering angle limits, which can be introduced in the form of inequality constraints. Finally, numerical optimization methods (such as quadratic programming) are used to solve the above optimization problem to obtain the control input sequence for the next period of time. The model predictive control algorithm usually only applies the first control input in the optimization result to enhance the response ability to environmental changes. Subsequently, in each control cycle, the model predictive control algorithm recalculates the control input according to the new state information to ensure that the autonomous driving vehicle can drive smoothly along the optimized path.

[0101] S400. The second optimized path is smoothed through the dynamic motion primitive technology to obtain a smooth trajectory suitable for the automatic driving vehicle to follow. By acquiring the data of the lidar and the inertial measurement unit and applying the lidar-inertial simultaneous localization and mapping algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance has been constructed, where the data of the inertial measurement unit includes acceleration, angular velocity, pitch angle, and roll angle.

[0102] It can be understood that in this step S400, it includes S401, S402, S403, and S404, where:

[0103] S401. According to the vertex sequence of the second optimized path, the path points are dynamically adjusted through a radial basis function neural network to generate a continuous smooth trajectory, and its calculation formula is as follows:

[0104]

[0105] In the formula, y(t) is the position or speed of the vehicle at time t, N is the number of basis functions used to generate the trajectory, ωi is the weight corresponding to each basis function, Ψi is the basis function, τ i is the center of the basis function, t is the time, and i is the index variable;

[0106] S402. Acquire the lidar data. According to the point cloud data in the lidar data, through point cloud processing and feature extraction, the three-dimensional structure information of the environment is obtained, and the calculation formula of the point cloud processing is as follows:

[0107] P global = T·P sensor

[0108] In the formula, P global is the point cloud data in the global coordinate system, T is the transformation matrix from the sensor coordinate system to the global coordinate system, and P sensor is the point cloud data in the sensor coordinate system;

[0109] In the feature extraction process of this step, the curvature of each point will be calculated as the basis for judging corner points and plane points. The calculation of curvature usually involves the point distribution in the local neighborhood of the point cloud, and the variables that may be involved in its formula include:

[0110]

[0111] In the formula, N is the unit vector, d is the distance from the point to the plane, P is the point in the point cloud, and P 邻域 is the point cloud data in the local area around the current point, and P 当前点 is the current point in the point cloud being processed;

[0112] S403. Obtain inertial measurement unit data. Based on the acceleration and angular velocity information in the inertial measurement unit data, after data preprocessing and filtering, obtain the denoised inertial measurement data set;

[0113] First, use the inertial measurement unit data to perform de-distortion processing on the lidar point cloud. Since the lidar will undergo displacement and rotation during the scanning process with the movement of the carrier, resulting in distortion of the point cloud data. The movement of the lidar during scanning can be estimated through the inertial measurement unit data, and the point cloud can be corrected accordingly. The purpose is to obtain the de-distorted point cloud data, providing accurate input data for subsequent feature extraction and map construction. Extract feature points, including corner points and plane points, from the de-distorted point cloud data. These feature points will be used in subsequent matching and map construction processes. The purpose of feature extraction is to identify significant features in the environment to establish connections between data scanned at different times. Among them, the relevant calculations involve the calculation of rotation matrices and translation vectors, which are usually obtained by integrating the angular velocity and acceleration data provided by the inertial measurement unit data. In addition, during the feature extraction process, the curvature of each point is calculated as the basis for judging corner points and plane points. The calculation of curvature usually involves the point distribution within the local neighborhood of the point cloud.

[0114] It should be noted that in the calculation formula for point cloud processing, the transformation matrix T can be obtained through the following steps: Estimate the sensor movement of the lidar during scanning, including rotation and translation, using the inertial measurement unit data; Integrate the angular velocity of the inertial measurement unit data to obtain rotation, and integrate the acceleration to obtain translation; Specifically, it also involves the integration of the angular velocity and acceleration of the inertial measurement unit data, which can ensure that the point cloud data can be accurately transformed from the sensor coordinate system to the global coordinate system, providing accurate input data for subsequent simultaneous localization and mapping tasks.

[0115] In this step, perform pre-integration processing on the original data of the inertial measurement unit to estimate the movement of the sensor during lidar scanning. The result of pre-integration is used for the skew correction of the point cloud and as the initial pose estimate in the lidar odometry optimization process. The purpose is to reduce the errors caused by the dynamic movement of the sensor and improve the positioning accuracy.

[0116] S404. Through the tight coupling of three-dimensional structure information and the inertial measurement data set, use the simultaneous localization and mapping algorithm and combine graph optimization technology to achieve three-dimensional modeling of the underwater environment. The three-dimensional modeling includes the construction and update of the underwater map, thereby generating a three-dimensional map of the underwater environment for path planning and obstacle avoidance.

[0117] In this step, using the factor graph optimization technique, a global optimization is performed by combining various types of data such as lidar odometry factors, inertial measurement unit factors, GPS factors (if any), and loop closure factors. The aim is to estimate the biases of the IMU and optimize the entire trajectory estimation to generate a continuous and high-precision 3D map. Then, based on the optimized pose information and feature points, a 3D map of the underwater environment is constructed. This map not only includes the geometric structure of the environment but also contains the position and pose information of the autonomous vehicle in this environment, providing detailed environmental information for path planning and obstacle avoidance.

[0118] It should be noted that the logical relationship between obtaining lidar and inertial measurement unit data and the lidar-inertial simultaneous localization and mapping algorithm processing is as follows: The lidar provides the precise 3D structure of the environment, while the inertial measurement unit data provides the dynamic information of the AUV. The lidar-inertial simultaneous localization and mapping algorithm combines these two data sources and generates a precise 3D map of the underwater environment through data fusion technology. This map not only includes the geometric structure of the environment but also contains the position and pose information of the AUV in this environment. The process of generating the 3D map generally includes the following steps: Using the lidar to scan the environment to obtain the point cloud data of the environment; Using the IMU data to correct the lidar data to compensate for the influence brought by the movement of the AUV; Performing data fusion through the LIO-SAM algorithm to generate a dynamically updated 3D map; This map can be used for path planning and obstacle avoidance because it provides the detailed structure of the environment and the precise position of the AUV.

[0119] S500. Process the 3D map through sensor fusion perception, extract the current state information of dynamic obstacles in the surrounding environment, construct and process using a prediction model, and predict their action trajectories within a certain period in the future based on the current state and motion characteristics of the obstacles.

[0120] It should be noted that the choice of the prediction model depends on various factors, including the type and quality of the available data, the complexity of the prediction task, and the requirements for model accuracy and real-time performance. In the underwater environment, due to the complexity and dynamics of the environment, the prediction model needs to be updated regularly to adapt to environmental changes.

[0121] It can be understood that in this step S500, it includes S501, S502, and S503, where:

[0122] S501. Identify and separate the feature points of dynamic obstacles using the RANSAC algorithm. By randomly selecting data points and fitting a model, calculate the residuals between the model and all observed data, and judge the size of the residuals compared with a preset threshold. If the residual is less than the given threshold, it is judged as an inlier. Aggregate all the obtained inliers to get the inlier set of the dynamic obstacle, where the inlier set includes the feature point information of the dynamic obstacle.

[0123] The calculation formula for the residuals of all observed data is as follows:

[0124] Residual = ||Observed value - Model predicted value||

[0125] In the formula, the observed value is the original data directly measured by the sensor, and the model predicted value is the predicted data calculated according to the fitted model.

[0126] S502. Based on the inlier set, use the support vector machine to build a model to obtain the motion feature model of the dynamic obstacle, where the motion feature model is used to predict the future state of the obstacle. The support vector machine finds an optimal hyperplane in the feature space to separate data of different classes, and its calculation formula is as follows:

[0127]

[0128] s.t.y i (w·φ(x i ) + b) ≥ 1 - ξ i , ξ i ≥ 0

[0129] In the formula, w is the normal vector of the hyperplane, b is the bias term, ξi, ξ are slack variables, C is the penalty parameter, yi is the class label, φ(xi) is the feature vector mapped to the high-dimensional space, l is the total number of samples, i is the i-th sample in the dataset, and s.t. is the constraint condition in the specified optimization problem.

[0130] These constraints ensure that each sample point xi is correctly classified and allow a certain error range, which is controlled by the slack variable ξi. If ξi = 0, the sample point is completely within the correct classification region; if ξi > 0, the sample point violates the classification boundary but is still allowed to ensure that the model can handle non-linearly separable datasets.

[0131] S503. Based on the motion feature model of the dynamic obstacle, use the Kalman filter to dynamically update the expected trajectory of the obstacle in the future for a period of time, where the expected trajectory includes position and velocity information, and its calculation formula is as follows:

[0132]

[0133] Pk|k-1 = APk -1|k-1 A T + Q

[0134] wherein, is the prior state estimate at time k, A is the state transition matrix, B is the control input matrix, uk is the control input, and P k|k-1 is the prior error covariance matrix, and Q is the process noise covariance matrix.

[0135] It should be noted that when using a Kalman filter or a particle filter to predict the future position of a dynamic obstacle, the step process is as follows: First, set the initial state for the Kalman filter or the particle filter, which includes the current position, velocity, and other possible state variables of the obstacle, such as acceleration. At the same time, the uncertainty of the state needs to be initialized, which is usually represented by a covariance matrix. In the Kalman filter, the prediction step uses the motion model of the obstacle to estimate its state at a future time. This motion model takes into account the velocity and acceleration of the obstacle, as well as the influence of environmental factors, such as water flow or wind speed. In the particle filter, the prediction step involves updating the state of each particle according to the motion model to generate a new set of particles, where the particles represent the possible future state distribution of the obstacle. Next is the update step. When new observation data is obtained from the sensor, the Kalman filter uses this data to correct the prediction of the obstacle state. The filter calculates a Kalman gain, which is a weight factor that balances the predicted state and the observation data. Then, the filter adjusts the predicted state according to the Kalman gain to produce a more accurate estimate. In the particle filter, the update step involves updating the weights of the particles according to the observation data. The weights of the particles reflect the consistency of each particle with the observation data. Then, the particle filter may generate a new set of particles through a resampling step to focus on representing the most likely state estimate. Finally, iterative updates are performed, including the prediction and update steps of the Kalman filter or the particle filter being iteratively carried out at each time step. As new observation data continuously arrives, the filter continuously updates its estimate of the obstacle state, thereby achieving dynamic tracking of the future position of the obstacle.

[0136] In summary, through this dynamic update process, the Kalman filter or the particle filter can provide continuous predictions of the future position of the obstacle. These predictions take into account the motion characteristics of the obstacle and the uncertain factors in the environment. The method adopted in this step is applicable to path planning in a dynamic environment. It can provide real-time estimates of the future state of the obstacle, enabling the autonomous driving vehicle to make timely obstacle avoidance decisions.

[0137] S600. Based on the smooth trajectory tracked by the autonomous vehicle and the future action trajectory of the obstacle, through real-time adjustment and processing by the adaptive control system, a stable path tracking control instruction that can adapt to different wind waves and water flow changes is obtained, thereby completing the path planning of the autonomous vehicle.

[0138] It can be understood that in this step S600, S601 and S602 are included, where:

[0139] S601. Based on the smooth trajectory tracked by the autonomous vehicle, the future action trajectory of the obstacle, and the preset dynamic model of the autonomous vehicle, through the integrated processing of real-time wind wave and water flow data, the motion state of the autonomous vehicle under the influence of wind waves and water flow is obtained, where the adaptive control algorithm adjusts the control parameters to adapt to the dynamic changes of the environment;

[0140] It should be noted that in this step, the adaptive control algorithm adjusts the control parameters to adapt to the dynamic changes of the environment. For example, an adaptive PID control algorithm can be used, where the proportional (P), integral (I), and derivative (D) parameters are automatically adjusted according to the change of the error signal to achieve real-time control of the AUV motion state. A dynamic model can be used to describe the behavior of the vehicle under these external forces. For example, a state space model including the influence terms of wind waves and water flow can be used:

[0141]

[0142] In the formula, x is the state vector of the vehicle, u is the control input, w is the influence of wind waves and water flow, and A, B, and D are system matrices that describe the motion characteristics of the vehicle and the influence of external forces. The derivative of the state vector, that is, the rate of change of the vehicle state with time.

[0143] It can be understood that the dynamic model of the autonomous vehicle is usually preset. It is a mathematical model established based on physical principles and experimental data, used to describe and predict the motion behavior of the vehicle when receiving control inputs. This model includes factors such as the mass, size, shape, thruster thrust, water resistance, and buoyancy of the vehicle, and how they affect the motion state of the vehicle. It is to predict the response of the vehicle according to the control input, such as how the vehicle will move under a given control input; and use the model to plan the path of the vehicle to ensure that the vehicle can travel along the predetermined trajectory while avoiding obstacles. In the calculation process, a PID controller or model predictive control (MPC) is used to achieve precise control of the vehicle motion.

[0144] S602. Adjust the dynamic control strategy using the fuzzy logic control method according to the motion state, and calculate the path tracking control instructions that adapt to the changes in wind waves and water flow, including adjusting the speed, direction, and thruster force of the autonomous vehicle, while avoiding obstacles and adapting to the changing environmental conditions, so as to complete the path planning and tracking tasks.

[0145] It should be noted that the fuzzy logic control method is used to handle uncertainty and nonlinear problems. By defining a series of fuzzy rules, it determines how to adjust the control output according to the input error and rate of change, and is applicable to complex systems that are difficult to describe with traditional mathematical models. The purpose of these control algorithms is to generate accurate control instructions to regulate the motion of the autonomous vehicle, ensuring that it can smoothly and stably travel along the planned path and maintain the correct heading even under the influence of external environmental factors such as wind waves and water flow. Through these algorithms, the autonomous vehicle can automatically adjust its speed, direction, and attitude to adapt to the real-time changing environmental conditions, thus completing the path planning and tracking tasks.

[0146] Embodiment 2:

[0147] As Figure 2 shown, this embodiment provides a path planning system for an autonomous vehicle. Refer to Figure 2 The system includes:

[0148] Acquisition module 701: Used to acquire the acoustic sensor data of the underwater environment. The acoustic sensor data includes the frequency and intensity of the transmitted acoustic wave, as well as the timestamp and amplitude information of the received reflected acoustic wave. Based on the difference between the first timestamp of the transmitted acoustic wave and the second timestamp of the received acoustic wave, through acoustic wave propagation time calculation and processing, the contour of the underwater terrain and the position information of the obstacles are obtained;

[0149] First processing module 702: Used to obtain the dynamic model of the underwater environment through dynamic environment modeling processing based on the contour of the underwater terrain and the position information of the obstacles. The dynamic model includes the terrain, obstacle distribution, and water flow influence;

[0150] Calculation module 703: Used to calculate the dynamic model of the underwater environment using the Euclidean distance as the heuristic function to obtain the first optimized path from the starting point to the ending point. The first optimized path is the globally shortest optimal path calculated from the dynamic model of the underwater environment; Combining the first optimized path, through model predictive control algorithm calculation and processing, a second optimized path that meets the kinematic and dynamic constraints of the autonomous vehicle is obtained. The second optimized path is the path obtained after considering the kinematic and dynamic constraints, where the constraints include maximum speed, acceleration limit, and steering angle limit;

[0151] The second processing module 704: It is used to smooth the second optimized path through the dynamic motion primitive technology to obtain a smooth trajectory suitable for the automatic driving vehicle to track; by acquiring the data of the lidar and the inertial measurement unit, and applying the lidar inertial simultaneous localization and mapping algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance is constructed, where the data of the inertial measurement unit includes acceleration, angular velocity, pitch angle, and roll angle;

[0152] The prediction module 705: It is used to process the three-dimensional map through sensor fusion perception, extract the current state information of the dynamic obstacles in the surrounding environment, construct and process it using the prediction model, and based on the current state and motion characteristics of the obstacles, predict their action trajectories in a future period of time;

[0153] The planning module 706: It is used to perform real-time adjustment processing through the adaptive control system according to the smooth trajectory tracked by the automatic driving vehicle and the future action trajectories of the obstacles, and obtain a stable path tracking control instruction that can adapt to different wind waves and water flow changes, so as to complete the path planning of the automatic driving vehicle.

[0154] Specifically, the calculation module 701 includes:

[0155] The calculation unit: It is used to input the dynamic model of the underwater environment into the heuristic function for calculation. During the calculation process, the node with the minimum f(n) value is selected as the current node and moved from the open list to the closed list. For each neighbor node of the current node, if it is not in the closed list, calculate its f(n) value, and update its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function as needed. Repeat the above process until the target node is added to the closed list or the open list is empty. If the target node is found, construct the path from the starting point to the end point by backtracking the parent node of each node, so as to obtain the globally shortest and optimal path to reach the target node. Its calculation formula is as follows:

[0156]

[0157] In the formula, (x n , y n ) is the coordinate of the current node, (x g , y g ) is the coordinate of the target node, and h(n) is the cost estimated by the heuristic function;

[0158] f(n) = g(n) + h(n)

[0159] In the formula, g(n) is the actual cost from the starting point to the current node, h(n) is the cost estimated by the heuristic function, and f(n) is the total cost estimate of a node n, that is, the first optimized path.

[0160] Specifically, the computing module 701 includes:

[0161] A model establishment unit: used to establish a kinematic model of the autonomous vehicle. After state-space representation processing, the initial conditions of state variables and control inputs are obtained, and its calculation formula is as follows:

[0162]

[0163] In the formula, is the change rate of the state variable with time, f is the state transition function, x is the state variable, and u is the control input;

[0164] A prediction processing unit: used to perform state prediction processing of the model predictive control algorithm based on the initial conditions in each control cycle to obtain the state prediction sequence of the autonomous vehicle within a future period of time, and its calculation formula is as follows:

[0165] x t+k = f(x t , u t )(k = 1, 2,..., N)

[0166] In the formula, x t+k is the state of the autonomous vehicle at the k-th time step in the future starting from the current time t (where k ranges from 1 to N), k is the step index in the prediction horizon, indicating the k-th time step in the future starting from the current time t, f is the state transition function, x t is the state of the autonomous vehicle at the current time t, N is the total length of the prediction horizon, and u t is the control input applied to the autonomous vehicle at the current time t;

[0167] An evaluation unit: used to evaluate the quality of the path by introducing a cost function based on the state prediction sequence to obtain the path quality evaluation result. The cost function includes tracking error, change of control input, and constraint conditions, and its calculation formula is as follows:

[0168]

[0169] In the formula, w1 and w2 are weight coefficients, J is the total cost or performance index of the path, is the sum of the costs for all time steps from the current time step t to the future time step t + N, N is the total length of the prediction horizon, tracking_error(x t+k , x target ) is the difference between the actual state x t+k of the autonomous vehicle and the desired target state xtarget, and control_effort(ut+k ) The control input u required to reach the desired state t+k The cost, x t+k The state of the autonomous vehicle at the k-th time step in the future starting from the current time t (where k ranges from 1 to N), x target The desired target state at time step t + k, u t+k The control input at time step t + k;

[0170] Solution optimization unit: Used to evaluate and solve the path quality assessment results using numerical optimization techniques, obtain the control input sequence of the autonomous vehicle in the future for a period of time, and apply the first control input to repeat the optimization according to the latest system state in each control cycle to achieve path tracking and adjustment, and obtain the second optimized path.

[0171] Specifically, the second processing module 704, which includes:

[0172] Adjustment unit: Used to dynamically adjust the path points through a radial basis function neural network according to the vertex sequence of the second optimized path to generate a continuous smooth trajectory, and its calculation formula is as follows:

[0173]

[0174] In the formula, y(t) is the position or speed of the vehicle at time t, N is the number of basis functions used to generate the trajectory, ωi is the weight corresponding to each basis function, Ψi is the basis function, τ i Is the center of the basis function, t is the time, and i is the index variable;

[0175] Extraction unit: Used to obtain lidar data, and through point cloud processing and feature extraction based on the point cloud data in the lidar data, obtain the three-dimensional structure information of the environment, and the calculation formula of point cloud processing is as follows:

[0176] P global = T · P sensor

[0177] In the formula, P global Is the point cloud data in the global coordinate system, T is the transformation matrix from the sensor coordinate system to the global coordinate system, P sensor Is the point cloud data in the sensor coordinate system;

[0178] Acquisition unit: Used to obtain inertial measurement unit data, and through data preprocessing and filtering based on the acceleration and angular velocity information in the inertial measurement unit data, obtain the denoised inertial measurement data set;

[0179] Combining unit: It is used to realize 3D modeling of the underwater environment through the tight coupling of 3D structure information and inertial measurement data sets, using the simultaneous localization and mapping algorithm and combining graph optimization technology. The 3D modeling includes the construction and update of the underwater map, so as to generate a 3D map of the underwater environment for path planning and obstacle avoidance.

[0180] Specifically, the prediction module 705 includes:

[0181] Judgment unit: It is used to identify and separate the feature points of dynamic obstacles by using the random sample consensus algorithm. By randomly selecting data points and fitting a model, calculating the residuals between the model and all observed data, and judging the size of the residuals and a preset threshold. If the residuals are less than the given threshold, they are judged as inliers, and all the obtained inliers are aggregated to obtain the inlier set of dynamic obstacles, where the inlier set includes the feature point information of dynamic obstacles;

[0182] Modeling unit: It is used to perform modeling using a support vector machine according to the inlier set to obtain a motion feature model of dynamic obstacles, where the motion feature model is used to predict the future state of the obstacles. The support vector machine separates different categories of data by finding an optimal hyperplane in the feature space, and its calculation formula is as follows:

[0183]

[0184] s.t.y i (w·φ(x i )+b)≥1-ξ i ,ξ i ≥0

[0185] In the formula, w is the normal vector of the hyperplane, b is the bias term, ξi, ξ are slack variables, C is the penalty parameter, yi is the class label, φ(xi) is the feature vector mapped to the high-dimensional space, l is the total number of samples, i is the i-th sample in the data set, and s.t. is the constraint condition in the specified optimization problem;

[0186] Update unit: It is used to dynamically update the expected trajectory of the obstacle in the future period of time based on the motion feature model of the dynamic obstacle, where the expected trajectory includes position and velocity information, and its calculation formula is as follows:

[0187]

[0188] P k|k-1 =AP k-1|k-1 A T +Q

[0189] In the formula, is the prior state estimate at time k, A is the state transition matrix, B is the control input matrix, uk is the control input, and P k|k-1 is the prior error covariance matrix, and Q is the process noise covariance matrix.

[0190] Specifically, the planning module 706 includes:

[0191] An integrated processing unit: configured to obtain the motion state of the autonomous vehicle under the influence of wind waves and water currents through integrated processing of real-time wind wave and water current data based on the smooth trajectory tracked by the autonomous vehicle, the future action trajectory of the obstacle, and the preset dynamic model of the autonomous vehicle. An adaptive control algorithm adjusts control parameters to adapt to the dynamic changes of the environment;

[0192] A calculation instruction unit: configured to adjust the dynamic control strategy using the fuzzy logic control method according to the motion state, and calculate the path tracking control instruction adapted to the changes of wind waves and water currents, including adjusting the speed, direction, and thruster force of the autonomous vehicle, while avoiding obstacles and adapting to the changing environmental conditions, so as to complete the path planning and tracking tasks.

[0193] It should be noted that regarding the system in the above embodiments, the specific manners in which each module performs operations have been described in detail in the embodiments related to the method, and will not be elaborated herein.

[0194] Embodiment 3:

[0195] Corresponding to the above method embodiment, in this embodiment, a path planning device for an autonomous vehicle is also provided. A path planning device for an autonomous vehicle described below can be mutually referred to with a path planning method for an autonomous vehicle described above.

[0196] Figure 3 is a block diagram of a path planning device 800 for an autonomous vehicle shown according to an exemplary embodiment. As Figure 3 shown, the path planning device 800 for the autonomous vehicle includes: a processor 801 and a memory 802. The path planning device 800 for the autonomous vehicle further includes one or more of a multimedia component 803, an I / O interface 804, and a communication component 805.

[0197] Among them, the processor 801 is used to control the overall operation of the path planning device 800 of the autonomous driving vehicle to complete all or part of the steps in the above-mentioned path planning method for the autonomous driving vehicle. The memory 802 is used to store various types of data to support the operation of the path planning device 800 of the autonomous driving vehicle. These data may include, for example, instructions for any application or method operating on the path planning device 800 of the autonomous driving vehicle, as well as application-related data, such as contact data, received and sent messages, pictures, audio, video, and so on. The memory 802 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read-Only Memory (EPROM), Programmable Read-Only Memory (PROM), Read-Only Memory (ROM), magnetic memory, flash memory, a magnetic disk, or an optical disc. The multimedia component 803 may include a screen and an audio component. Among them, the screen may be, for example, a touch screen, and the audio component is used to output and / or input audio signals. For example, the audio component may include a microphone, and the microphone is used to receive external audio signals. The received audio signal may be further stored in the memory 802 or sent through the communication component 805. The audio component further includes at least one speaker for outputting audio signals. The I / O interface 804 provides an interface between the processor 801 and other interface modules, and the above-mentioned other interface modules may be a keyboard, a mouse, or buttons, etc. These buttons may be virtual buttons or physical buttons. The communication component 805 is used for wired or wireless communication between the path planning device 800 of the autonomous driving vehicle and other devices. Wireless communication, such as Wi-Fi, Bluetooth, Near Field Communication (NFC), 2G, 3G, or 4G, or a combination of one or more of them. Therefore, the corresponding communication component 805 may include: a Wi-Fi module, a Bluetooth module, or an NFC module.

[0198] In an exemplary embodiment, the path planning device 800 of the autonomous vehicle can be implemented by one or more application specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field programmable gate arrays (FPGAs), controllers, microcontrollers, microprocessors, or other electronic components, and is used to execute the path planning method of the autonomous vehicle described above.

[0199] In another exemplary embodiment, a computer-readable storage medium including program instructions is also provided. When the program instructions are executed by a processor, the steps of the path planning method of the autonomous vehicle described above are implemented. For example, the computer-readable storage medium can be the memory 802 including the program instructions described above, and the program instructions can be executed by the processor 801 of the path planning device 800 of the autonomous vehicle to complete the path planning method of the autonomous vehicle described above.

[0200] Embodiment 4:

[0201] Corresponding to the above method embodiment, a readable storage medium is also provided in this embodiment. A readable storage medium described below can be referred to correspondingly with a path planning method of an autonomous vehicle described above.

[0202] A computer program is stored on the readable storage medium. When the computer program is executed by a processor, the steps of the path planning method of the autonomous vehicle in the above method embodiment are implemented.

[0203] The readable storage medium can specifically be various readable storage media such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disc that can store program codes.

[0204] The above are only the preferred embodiments of the present invention and are not used to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

[0205] As described above, it is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of changes or substitutions, which should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A path planning method for an automatic driving vehicle, characterized in that, Including: Obtain acoustic sensor data of the underwater environment, where the acoustic sensor data includes the frequency and intensity of the emitted acoustic wave, as well as the timestamp and amplitude information of the received reflected acoustic wave. Based on the difference between the first timestamp of the emitted acoustic wave and the second timestamp of the received acoustic wave, through acoustic wave propagation time calculation and processing, obtain the contour of the underwater terrain and the position information of obstacles; Based on the contour of the underwater terrain and the position information of obstacles, through dynamic environment modeling and processing, obtain a dynamic model of the underwater environment, where the dynamic model includes terrain, obstacle distribution, and water flow influence; Use the Euclidean distance as a heuristic function to calculate the dynamic model of the underwater environment, and obtain a first optimized path from the starting point to the ending point, where the first optimized path is the globally shortest and optimal path calculated from the dynamic model of the underwater environment; Combine the first optimized path, and through model predictive control algorithm calculation and processing, obtain a second optimized path that conforms to the kinematic and dynamic constraints of the autonomous vehicle, where the second optimized path is the path obtained after considering kinematic and dynamic constraints, and the constraints include maximum speed, acceleration limit, and steering angle limit; Through the dynamic movement primitive technology, smooth the second optimized path to obtain a smooth trajectory suitable for the autonomous vehicle to track; By obtaining lidar and inertial measurement unit data, apply the lidar inertial synchronization positioning and mapping algorithm for processing to construct a three-dimensional map of the underwater environment for path planning and obstacle avoidance, where the inertial measurement unit data includes acceleration, angular velocity, pitch angle, and roll angle; Through sensor fusion perception, process the three-dimensional map, extract the current state information of dynamic obstacles in the surrounding environment, use prediction model construction and processing, and based on the current state and movement characteristics of the obstacles, predict their action trajectories in the next period of time; According to the smooth trajectory tracked by the autonomous vehicle and the future action trajectories of the obstacles, through real-time adjustment and processing by the adaptive control system, obtain a stable path tracking control instruction that can adapt to different wind waves and water flow changes, thereby completing the path planning of the autonomous vehicle.

2. The path planning method for an automatic driving vehicle according to claim 1, wherein, The use of the Euclidean distance as a heuristic function to calculate the dynamic model of the underwater environment and obtain a first optimized path from the starting point to the ending point includes: Take the dynamic model of the underwater environment as an input item and input it into the heuristic function for calculation. During the calculation process, select the node with the minimum f(n) value as the current node, and move it from the open list to the closed list. For each neighbor node of the current node, if it is not in the closed list, calculate its f(n) value, and update its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function as needed. Repeat the above process until the target node is added to the closed list or the open list is empty. If the target node is found, construct a path from the starting point to the ending point by backtracking the parent node of each node, thereby obtaining the globally shortest and optimal path to reach the target node. Its calculation formula is as follows: Where (x n , y n ) are the coordinates of the current node, (x g , y g ) are the coordinates of the target node, and h(n) is the cost estimated by the heuristic function; f(n) = g(n) + h(n) Wherein, g(n) is the actual cost from the starting point to the current node, h(n) is the cost estimated by the heuristic function, and f(n) is the total cost estimate of a node n, that is, the first optimized path.

3. The path planning method of the automatic driving vehicle according to claim 1, wherein Combined with the first optimized path, through the calculation and processing of the model predictive control algorithm, a second optimized path that meets the kinematic and dynamic constraints of the autonomous vehicle is obtained, including: Establish the kinematic model of the autonomous vehicle, and through state-space representation processing, obtain the initial conditions of the state variables and control inputs. The calculation formula is as follows: wherein, is the rate of change of the state variable with time, f is the state transition function, x is the state variable, and u is the control input; Based on the initial conditions, in each control cycle, through the state prediction processing of the model predictive control algorithm, obtain the state prediction sequence of the autonomous vehicle in the future period of time. The calculation formula is as follows: x t+k = f(x t , u t ), k = 1, 2, ..., N where x t+k is the k-th time step in the future from the current time t, where k is the state of the autonomous vehicle from 1 to N, k is the step index in the prediction horizon, representing the k-th time step in the future from the current time t, f is the state transition function, x t is the state of the autonomous vehicle at the current time t, N is the total length of the prediction horizon, u t is the control input applied to the autonomous vehicle at the current time t; According to the state prediction sequence, through the introduction of the cost function definition to evaluate the quality of the path, obtain the path quality evaluation result. The cost function includes tracking error, the change of control input, and constraint conditions. The calculation formula is as follows: where \(w_1\) and \(w_2\) are weight coefficients, and \(J\) is the total cost or performance metric of the path. is to sum the costs over all time steps from the current time step \(t\) to the future time step \(t + N\), where \(N\) is the total length of the prediction horizon, and \(\text{tracking\_error}(x t+k ,x target ) is the difference between the actual state \(x t+k \) of the autonomous vehicle and the desired target state \(x_{\text{target}}\), \(\text{control\_effort}(u t+k )\) is the cost of the control input \(u t+k \) required to reach the desired state, \(x t+k \) is the state of the autonomous vehicle at the \(k\)-th future time step from the current time \(t\), where \(k\) ranges from 1 to \(N\), \(x target \) is the desired target state at time step \(t + k\), and \(u t+k \) is the control input at time step \(t + k\). Use numerical optimization technology to evaluate and solve the path quality evaluation result, obtain the control input sequence of the autonomous vehicle in the future period of time, and apply the first control input to repeat the optimization according to the latest system state in each control cycle to achieve path tracking and adjustment, and obtain the second optimized path.

4. The path planning method of the automatic driving vehicle according to claim 1, characterized in that The second optimized path is smoothed through the dynamic motion primitive technology to obtain a smooth trajectory that meets the tracking of the autonomous vehicle; by acquiring the data of the lidar and inertial measurement unit, and applying the lidar-inertial synchronization positioning and mapping algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance is constructed, including: According to the vertex sequence of the second optimized path, the path points are dynamically adjusted through a radial basis function neural network to generate a continuous smooth trajectory. The calculation formula is as follows: where y(t) is the position or velocity of the trolley at time t, N is the number of basis functions used to generate the trajectory, ωi is the weight corresponding to each basis function, Ψi is the basis function, τ i is the center of the basis function, t is the time, and i is the index variable; Acquire lidar data, and according to the point cloud data in the lidar data, through point cloud processing and feature extraction, obtain the three-dimensional structure information of the environment. The calculation formula for point cloud processing is as follows: P global = T·P sensor Where, P global is the point cloud data in the global coordinate system, T is the transformation matrix from the sensor coordinate system to the global coordinate system, and P sensor is the point cloud data in the sensor coordinate system; Acquire inertial measurement unit data, and according to the acceleration and angular velocity information in the inertial measurement unit data, through data preprocessing and filtering processing, obtain the denoised inertial measurement data set; Through the tight coupling of the three-dimensional structure information and the inertial measurement data set, using the synchronization positioning and mapping algorithm and combining the graph optimization technology, realize the three-dimensional modeling of the underwater environment. The three-dimensional modeling includes the construction and update of the underwater map, so as to generate a three-dimensional map of the underwater environment for path planning and obstacle avoidance.

5. The path planning method for an automatic driving vehicle according to claim 1, characterized in that, Through the processing of the three-dimensional map by sensor fusion perception, extract the current state information of the dynamic obstacles in the surrounding environment, use the prediction model construction for processing, and based on the current state and motion characteristics of the obstacles, predict their action trajectories in the future period of time, including: Identify and separate the feature points of dynamic obstacles using the RANSAC algorithm. By randomly selecting data points and fitting a model, calculate the residuals between the model and all observed data, and judge the size of the residuals compared with a preset threshold. If the residuals are less than the given threshold, they are judged as inliers. Aggregate all the obtained inliers to get the inlier set of the dynamic obstacle, where the inlier set includes the feature point information of the dynamic obstacle. Based on the inlier set, use the support vector machine to build a model to obtain the motion feature model of the dynamic obstacle. The motion feature model is used to predict the future state of the obstacle. The support vector machine finds an optimal hyperplane in the feature space to separate data of different classes. Its calculation formula is as follows: s.t.y i (w·φ(x i )+b)≥1-ξ i ,ξ i ≥0 In the formula, w is the normal vector of the hyperplane, b is the bias term, ξi, ξ are slack variables, C is the penalty parameter, yi is the class label, φ(xi) is the feature vector mapped to the high-dimensional space, l is the total number of samples, i is the i-th sample in the dataset, and s.t. is the constraint condition in the specified optimization problem. Based on the motion feature model of the dynamic obstacle, use the Kalman filter to dynamically update the expected trajectory of the obstacle in the future for a period of time. The expected trajectory includes position and velocity information. Its calculation formula is as follows: P k|k-1 = AP k-1|k-1 A T + Q where is the prior state estimate at time k, A is the state transition matrix, B is the control input matrix, uk is the control input, P k|k-1 is the prior error covariance matrix, and Q is the process noise covariance matrix.

6. The path planning method of the automatic driving vehicle according to claim 1, characterized in that According to the smooth trajectory tracked by the autonomous vehicle and the future action trajectory of the obstacle, through real-time adjustment and processing by the adaptive control system, obtain a stable path tracking control instruction that can adapt to different wind waves and water flow changes, thereby completing the path planning of the autonomous vehicle, including: Based on the smooth trajectory tracked by the autonomous vehicle, the future action trajectory of the obstacle, and the preset dynamic model of the autonomous vehicle, through the integrated processing of real-time wind wave and water flow data, obtain the motion state of the autonomous vehicle under the influence of wind waves and water flow. The adaptive control algorithm adjusts the control parameters to adapt to the dynamic changes of the environment. Adjust the dynamic control strategy using the fuzzy logic control method according to the motion state, and calculate the path tracking control instruction that adapts to wind wave and water flow changes, including adjusting the speed, direction, and thruster force of the autonomous vehicle, while avoiding obstacles and adapting to changing environmental conditions, thereby completing the path planning and tracking tasks.

7. A path planning system for an automatic driving vehicle, based on the path planning method for an automatic driving vehicle described in claim 1, characterized in that, Including: Acquisition module: used to acquire the acoustic sensor data of the underwater environment. The acoustic sensor data includes the frequency, intensity of the emitted acoustic wave, and the timestamp and amplitude information of the received reflected acoustic wave. According to the difference between the first timestamp of the emitted acoustic wave and the second timestamp of the received acoustic wave, through the calculation and processing of the acoustic wave propagation time, obtain the contour of the underwater terrain and the position information of the obstacle. First processing module: used to obtain the dynamic model of the underwater environment through dynamic environment modeling processing based on the contour of the underwater terrain and the position information of the obstacle. The dynamic model includes the terrain, obstacle distribution, and water flow influence. Calculation module: It is used to calculate the dynamic model of the underwater environment by using the Euclidean distance as a heuristic function to obtain the first optimized path from the starting point to the ending point, where the first optimized path is the globally shortest and optimal path calculated from the dynamic model of the underwater environment; in combination with the first optimized path, through the model predictive control algorithm for calculation and processing, a second optimized path that conforms to the kinematics and dynamics constraints of the autonomous vehicle is obtained, where the second optimized path is the path obtained after processing considering the kinematics and dynamics constraints, and the constraints include maximum speed, acceleration limit, and steering angle limit; Second processing module: It is used to smooth the second optimized path through the dynamic movement primitive technology to obtain a smooth trajectory suitable for the autonomous vehicle to follow; by acquiring lidar and inertial measurement unit data, and applying the lidar-inertial simultaneous localization and mapping algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance is constructed, where the inertial measurement unit data includes acceleration, angular velocity, pitch angle, and roll angle; Prediction module: It is used to process the three-dimensional map through sensor fusion perception, extract the current state information of dynamic obstacles in the surrounding environment, construct and process using the prediction model, and based on the current state and motion characteristics of the obstacles, predict their action trajectories in the future for a period of time; Planning module: It is used to obtain stable path tracking control instructions that can adapt to different wind waves and water flow changes through real-time adjustment processing by the adaptive control system according to the smooth trajectory followed by the autonomous vehicle and the future action trajectories of the obstacles, so as to complete the path planning of the autonomous vehicle.

8. The path planning system of the automatic driving vehicle according to claim 7, wherein The calculation module, which includes: Calculation unit: It is used to input the dynamic model of the underwater environment as an input item into the heuristic function for calculation. During the calculation process, the node with the smallest f(n) value is selected as the current node and moved from the open list to the closed list. For each neighbor node of the current node, if it is not in the closed list, calculate its f(n) value, and update its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function as needed. Repeat the above process until the target node is added to the closed list or the open list is empty. If the target node is found, construct the path from the starting point to the ending point by backtracking the parent node of each node, so as to obtain the globally shortest and optimal path to reach the target node, and its calculation formula is as follows: Wherein, (x n , y n ) are the coordinates of the current node, (x g , y g ) are the coordinates of the target node, and h(n) is the cost estimated by the heuristic function; f(n) = g(n) + h(n) In the formula, g(n) is the actual cost from the starting point to the current node, h(n) is the cost estimated by the heuristic function, and f(n) is the total cost estimate of a node n, that is, the first optimized path.

9. The path planning system of the automatic driving vehicle according to claim 7, characterized in that, The second processing module, which includes: Adjustment unit: It is used to dynamically adjust the path points through the radial basis function neural network according to the vertex sequence of the second optimized path to generate a continuous smooth trajectory, and its calculation formula is as follows: where y(t) is the position or velocity of the trolley at time t, N is the number of basis functions used to generate the trajectory, ωi is the weight corresponding to each basis function, Ψi is the basis function, τ i is the center of the basis function, t is the time, and i is the index variable; Extraction Unit: It is used to obtain lidar data. Based on the point cloud data in the lidar data, through point cloud processing and feature extraction, the three-dimensional structure information of the environment is obtained. The calculation formula for point cloud processing is as follows: P global = T·P sensor where P global is the point cloud data in the global coordinate system, T is the transformation matrix from the sensor coordinate system to the global coordinate system, and Ps ensor is the point cloud data in the sensor coordinate system; Acquisition Unit: It is used to obtain inertial measurement unit data. Based on the acceleration and angular velocity information in the inertial measurement unit data, through data preprocessing and filtering, a denoised inertial measurement data set is obtained; Combination Unit: It is used to realize three-dimensional modeling of the underwater environment through the tight coupling of the three-dimensional structure information and the inertial measurement data set, using the simultaneous localization and mapping algorithm and combining graph optimization technology. The three-dimensional modeling includes the construction and update of the underwater map, thereby generating a three-dimensional map of the underwater environment for path planning and obstacle avoidance.

10. The path planning system of the automatic driving vehicle according to claim 7, characterized in that, The prediction module, which includes: Judgment Unit: It is used to identify and separate the feature points of dynamic obstacles using the random sample consensus algorithm. By randomly selecting data points and fitting a model, calculating the residuals between the model and all observed data, and judging the size of the residuals compared with a preset threshold. If the residual is less than the given threshold, it is judged as an inlier. All the obtained inliers are aggregated to obtain the inlier set of the dynamic obstacle, where the inlier set includes the feature point information of the dynamic obstacle; Modeling Unit: It is used to perform modeling using a support vector machine based on the inlier set to obtain a motion feature model of the dynamic obstacle. The motion feature model is used to predict the future state of the obstacle. The support vector machine finds an optimal hyperplane in the feature space to separate different classes of data. The calculation formula is as follows: s.t.y i (w·φ(x i )+b)≥1-ξ i ,ξ i ≥0 In the formula, w is the normal vector of the hyperplane, b is the bias term, ξi, ξ are slack variables, C is the penalty parameter, yi is the class label, φ(xi) is the feature vector mapped to the high-dimensional space, l is the total number of samples, i is the i-th sample in the data set, and s.t. is the constraint condition in the specified optimization problem; Update Unit: It is used to dynamically update the expected trajectory of the obstacle in the future for a period of time based on the motion feature model of the dynamic obstacle. The expected trajectory includes position and velocity information. The calculation formula is as follows: P k|k-1＝ AP k-1|k-1 A T +Q where is the prior state estimate at time k, A is the state transition matrix, B is the control input matrix, uk is the control input, P k|k-1 is the prior error covariance matrix, and Q is the process noise covariance matrix.

Images