Path planning method and system for automatic driving trolley

Abstract

The invention provides a path planning method and system for an automatic driving trolley, and relates to the technical field of path planning, and the method comprises the steps: obtaining the data of an acoustic sensor of an underwater environment, and obtaining the contour of the underwater terrain and the position information of an obstacle; carrying out dynamic environment modeling processing; the Euclidean distance is used as a heuristic function to calculate the dynamic model of the underwater environment, and a second optimized path conforming to kinematics and dynamics constraints of the automatic driving trolley is obtained; carrying out smoothing processing on the second optimization path through a dynamic motion primitive technology; constructing a three-dimensional map for path planning and obstacle avoidance; predicting an action track of the robot in a period of time in the future; and through real-time adjustment processing of the self-adaptive control system, path planning of the automatic driving trolley is completed. According to the method, the dynamic characteristics of the underwater environment and the motion constraint of the automatic driving trolley can be comprehensively considered, the three-dimensional map containing the obstacle and topographic information can be constructed in real time, and the future motion trail of the obstacle can be predicted.

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 automatic driving vehicle. Background Art

[0002] In the field of ocean exploration and resource development, autonomous underwater vehicles (AUVs) are increasingly used, 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, which often cannot adapt to the dynamic changes of underwater environments, such as the uncertainty of water flow, the movement of obstacles, and the variability of complex terrain.

[0003] In addition, existing path planning methods rarely consider the kinematic and dynamic characteristics of the AUV itself, which makes it difficult to accurately execute the planned path in practical applications and may even cause safety accidents. Although there are some attempts to improve this situation by introducing advanced sensors and algorithms, these technologies usually require high costs, and their reliability and stability in actual marine environments 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 automatic driving vehicle to improve the above problems. In order to achieve the above purpose, the technical solution adopted by the present invention is as follows:

[0005] In a first aspect, the present application provides a path planning method for an autonomous vehicle, comprising:

[0006] Acquire acoustic wave sensor data of the underwater environment, wherein the acoustic wave sensor data includes the frequency and intensity of the transmitted acoustic wave and the timestamp and amplitude information of the received reflected acoustic wave, and obtain the contour of the underwater terrain and the location information of the obstacle through the acoustic wave propagation time calculation and processing based on the difference between the first timestamp of the transmitted acoustic wave and the second timestamp of the received acoustic wave;

[0007] Based on the contour of the underwater terrain and the location information of the obstacles, a dynamic model of the underwater environment is obtained through dynamic environment modeling processing, wherein the dynamic model includes the terrain, the distribution of obstacles and the influence of water flow;

[0008] The dynamic model of the underwater environment is calculated using the Euclidean distance as a heuristic function to obtain a first optimization path from the starting point to the end point, wherein the first optimization path is the global shortest optimal path calculated from the dynamic model of the underwater environment; combined with the first optimization path, a second optimization path that meets the kinematic and dynamic constraints of the automatic driving vehicle is obtained through calculation and processing using a model predictive control algorithm, wherein the second optimization path is a path obtained after processing considering the kinematic and dynamic constraints, wherein the constraints include maximum speed, acceleration limit, and steering angle limit;

[0009] The second optimized path is smoothed by using dynamic motion primitive technology to obtain a smooth trajectory that meets the requirements of the automatic driving car tracking; by acquiring laser radar and inertial measurement unit data and applying the laser radar inertial synchronous positioning and map building algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance has been constructed, in which the inertial measurement unit data includes acceleration, angular velocity, pitch angle and roll angle;

[0010] The 3D map is processed through sensor fusion perception to extract the current state information of dynamic obstacles in the surrounding environment, and the prediction model is used to build and process it. Based on the current state of the obstacle and its motion characteristics, its movement trajectory in the future is predicted;

[0011] According to the smooth trajectory tracked by the automatic driving car and the future movement trajectory of the obstacle, the adaptive control system adjusts and processes in real time to obtain stable path tracking control instructions that can adapt to different wind waves and water flow changes, thereby completing the path planning of the automatic driving car.

[0012] Preferably, the method of using the Euclidean distance as a heuristic function to calculate the dynamic model of the underwater environment to obtain a first optimized path from the starting point to the end point includes:

[0013] The dynamic model of the underwater environment is input 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, its f(n) value is calculated, and its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function are updated as needed. The above process is repeated until the target node is added to the closed list or the open list is empty. If the target node is found, the path from the starting point to the end point is constructed by backtracking the parent node of each node, thereby obtaining the global shortest optimal path to the target node.

[0014] Preferably, the first optimization path is combined with the model predictive control algorithm to obtain a second optimization path that meets the kinematic and dynamic constraints of the automatic driving vehicle, which includes:

[0015] The kinematic model of the automatic driving car is established, and the initial conditions of the state variables and control inputs are obtained through state space representation processing;

[0016] Based on the initial conditions, in each control cycle, the state prediction sequence of the automatic driving car in the future period is obtained through the state prediction processing of the model predictive control algorithm;

[0017] According to the state prediction sequence, the quality of the path is evaluated by introducing the cost function definition, and the path quality evaluation result is obtained;

[0018] The numerical optimization technology is used to evaluate and solve the path quality assessment results to obtain the control input sequence of the automatic driving car in the future. The first control input is applied to repeatedly optimize according to the latest system status in each control cycle to achieve path tracking and adjustment, and obtain the second optimized path.

[0019] Preferably, the second optimized path is smoothed by the dynamic motion primitive technology to obtain a smooth trajectory that meets the tracking requirements of the automatic driving vehicle; by acquiring laser radar and inertial measurement unit data, the laser radar inertial synchronous positioning and map construction algorithm is applied to process, and a three-dimensional map of the underwater environment for path planning and obstacle avoidance is constructed, which includes:

[0020] 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;

[0021] Obtain laser radar data, and obtain the three-dimensional structure information of the environment through point cloud processing and feature extraction based on the point cloud data in the laser radar data;

[0022] Acquire the inertial measurement unit data, and obtain the denoised inertial measurement data set through data preprocessing and filtering based on the acceleration and angular velocity information in the inertial measurement unit data;

[0023] Through the tight coupling of three-dimensional structural information and inertial measurement data sets, the use of synchronous positioning and map construction algorithms, combined with graph optimization technology, three-dimensional modeling of the underwater environment is achieved. The three-dimensional modeling includes the construction and updating of underwater maps, thereby generating a three-dimensional map of the underwater environment for path planning and obstacle avoidance.

[0024] Preferably, the three-dimensional map is processed by sensor fusion perception to extract the current state information of dynamic obstacles in the surrounding environment, and the prediction model is used to construct and process the obstacles, and based on the current state of the obstacles and their motion characteristics, the movement trajectory of the obstacles in the future is predicted, including:

[0025] The random sample consensus algorithm is used to identify and separate the characteristic points of dynamic obstacles. The data points are randomly selected and fitted to the model. The residuals of the model and all the observed data are calculated. The residuals are compared with the preset threshold. If the residual is less than the given threshold, it is judged as an inlier. All the obtained inliers are clustered to obtain the inlier set of dynamic obstacles, where the inlier set includes the characteristic point information of the dynamic obstacles.

[0026] Based on the internal point set, support vector machine is used 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 separates different categories of data by finding an optimal hyperplane in the feature space.

[0027] Based on the motion feature model of dynamic obstacles, the Kalman filter is used to dynamically update the expected trajectory of the obstacle in the future, where the expected trajectory includes position and velocity information.

[0028] Preferably, the smooth trajectory tracked by the automatic driving vehicle and the future movement trajectory of the obstacle are adjusted and processed in real time by the adaptive control system to 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 automatic driving vehicle, which includes:

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

[0030] The dynamic control strategy is adjusted according to the motion state using fuzzy logic control method to calculate the path tracking control instructions that adapt to changes in wind, waves and water currents. This includes adjusting the speed, direction and thruster force of the automatic driving vehicle while avoiding obstacles and adapting to 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 automatic driving vehicle, comprising:

[0032] Acquisition module: used to acquire acoustic sensor data of the underwater environment, wherein the acoustic sensor data includes the frequency and intensity of the transmitted acoustic wave and the timestamp and amplitude information of the received reflected acoustic wave, and obtains the contour of the underwater terrain and the location information of obstacles through the calculation and processing of the acoustic wave propagation time according to the difference between the first timestamp of the transmitted acoustic wave and the second timestamp of the received acoustic wave;

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

[0034] Calculation module: used to calculate the dynamic model of the underwater environment using the Euclidean distance as a heuristic function to obtain a first optimization path from the starting point to the end point, wherein the first optimization path is the global shortest optimal path calculated from the dynamic model of the underwater environment; combined with the first optimization path, a second optimization path that meets the kinematic and dynamic constraints of the automatic driving vehicle is obtained through calculation and processing by the model predictive control algorithm, wherein the second optimization path is a path obtained after processing considering the kinematic and dynamic constraints, wherein the constraints include maximum speed, acceleration limit and steering angle limit;

[0035] The second processing module is used to smooth the second optimized path through the dynamic motion primitive technology to obtain a smooth trajectory that meets the tracking requirements of the automatic driving car; by acquiring the laser radar and inertial measurement unit data, the laser radar inertial synchronous positioning and map construction algorithm is applied for processing, and a three-dimensional map of the underwater environment for path planning and obstacle avoidance has been constructed, wherein the inertial measurement unit data includes acceleration, angular velocity, pitch angle and roll angle;

[0036] Prediction module: used to process the 3D map through sensor fusion perception, extract the current state information of dynamic obstacles in the surrounding environment, build and process using the prediction model, and predict its movement trajectory in the future based on the current state of the obstacle and its motion characteristics;

[0037] Planning module: It is used to obtain stable path tracking control instructions that can adapt to different wind waves and water flow changes based on the smooth trajectory tracked by the automatic driving car and the future movement trajectory of the obstacle through real-time adjustment and processing by the adaptive control system, thereby completing the path planning of the automatic driving car.

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

[0039] Memory for storing computer programs;

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

[0041] In a fourth aspect, the present application further provides a readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the above-mentioned path planning method based on the automatic driving 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 acoustic sensor data, the method can construct a three-dimensional map containing obstacle and terrain information in real time and predict the future motion trajectory of the obstacle.

[0044] The present invention obtains underwater terrain contours and obstacle location information through acoustic wave sensor data, and combined with dynamic environment modeling, the present invention can provide an accurate underwater environment dynamic model for the automatic driving vehicle, so as to plan a safer and more reliable path, and use Euclidean distance as a heuristic function to calculate the global shortest optimal path, and consider the kinematic and dynamic constraints of the automatic driving vehicle through a model predictive control algorithm to obtain an optimized path, thereby improving the efficiency of path planning.

[0045] The present invention smoothes the optimized path through dynamic motion primitive technology, and can obtain a smooth trajectory that meets the tracking requirements of an automatic driving vehicle, thereby improving the tracking performance of the path. Combined with lidar and inertial measurement unit data, a three-dimensional map of the underwater environment for path planning and obstacle avoidance can be constructed, thereby enhancing the perception capability of a complex underwater environment.

[0046] The present invention builds a sensor fusion perception and prediction model, which can accurately extract and predict the future movement trajectory of dynamic obstacles, improve the ability to cope with dynamic obstacles, and adopt 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 automatic driving vehicle.

[0047] The present invention improves the autonomous decision-making and execution capabilities of the automatic vehicle in complex underwater environments by integrating multiple sensor data and advanced algorithms, enhances the intelligence level of the system, optimizes path planning and improves execution efficiency, reduces energy consumption and operating costs, and improves the economic benefits of the automatic vehicle. In addition, by sensing environmental changes in real time and quickly adjusting the path, the system's responsiveness to emergencies is improved, thereby enhancing the robustness of the system.

[0048] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or be understood by implementing the embodiments of the present invention. The purpose and other advantages of the present invention can be realized and obtained by the structures particularly pointed out in the written description, claims, and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0050] Figure 1 It is a schematic flow chart of a path planning method for an automatic driving vehicle according to an embodiment of the present invention;

[0051] Figure 2 It is a schematic diagram of the structure of the path planning system of the automatic driving vehicle described in an embodiment of the present invention;

[0052] Figure 3 Schematic diagram of the structure of the path planning device of the automatic driving vehicle described in the embodiment 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 automatic driving vehicle; 801, processor; 802, memory; 803, multimedia component; 804, I / O interface; 805, communication component. DETAILED DESCRIPTION

[0054] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. The components of the embodiments of the present invention generally 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 invention, but merely represents selected embodiments of the present invention. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0055] It should be noted that similar reference numerals and letters represent similar items in the following drawings, so once an item is defined in one drawing, it does not need to be further defined and explained in the subsequent drawings. At the same time, in the description of the present invention, the terms "first", "second", etc. are only used to distinguish the description 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] See also Figure 1 , the figure shows that the method includes step S100, step S200, step S300, step S400, step S500 and step S600.

[0059] S100, obtaining acoustic wave sensor data of an underwater environment, wherein the acoustic wave sensor data includes the frequency and intensity of the transmitted acoustic wave and the timestamp and amplitude information of the received reflected acoustic wave, and obtaining the contour of the underwater terrain and the location information of obstacles through calculation and processing of the acoustic wave propagation time based on the difference between a first timestamp of the transmitted acoustic wave and a second timestamp of the received acoustic wave.

[0060] It can be understood that in this step, by measuring the time difference between the emission and reception of the sound wave, the total travel time of the sound wave from the sensor to the obstacle and back to the sensor can be calculated. Since the propagation speed of the sound 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 speed at which sound waves propagate in the medium, the time difference is the time interval between the emission and reception of the sound wave, and it is divided by 2 because the sound wave needs to travel back and forth between the sensor and the obstacle, thereby obtaining the straight-line distance from the sensor to the nearest obstacle. These data provide key spatial information for path planning of the underwater autonomous vehicle.

[0063] In addition, it should be noted that the time difference between the transmission and reception of the sound wave by the acoustic wave sensor is calculated by recording the time when the sound wave is transmitted and the time when the reflected sound wave is received, that is, this time difference reflects the total time for the sound wave to travel from the transmission point to the obstacle and then reflect back to the receiving point. Among them, the propagation speed of sound waves in water is known, so the distance can be calculated by the time difference and the speed of sound. Specifically, the measurement and processing of the sound wave propagation time includes the following steps: Transmitting sound waves: The acoustic wave sensor transmits sound waves, which will propagate in the water and encounter obstacles; Receiving reflected sound waves: When the sound waves encounter obstacles, they will be reflected back and received by the acoustic wave sensor; Time difference calculation: By recording the timestamps of the sound wave transmission and reception, the total time for the sound wave to go back and forth is calculated; Distance calculation: Using the known propagation speed of sound waves in water, the distance from the sound wave to the obstacle is calculated based on the time difference. The formula is:

[0064]

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

[0066] In this step, terrain information includes the ups and downs, gullies, slopes and other features of the seabed, while obstacle information includes the size, shape, material and other aspects of the obstacles. This information directly affects the safe navigation and mission execution of the vehicle. In an underwater environment, terrain and obstacles are closely related, and changes in 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; on a flat seabed plain, obstacles are easier to detect. Therefore, when planning a path, it is necessary to comprehensively consider the information of terrain and obstacles to ensure that the vehicle can complete the task safely and efficiently.

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

[0068] It can be understood that in this step, terrain refers to the shape and characteristics of the underwater ground, including ups and downs, gullies, slopes, etc.; obstacles include fixed or moving underwater objects, such as shipwrecks, rocks, marine life, etc.; water flow impact refers to the speed, direction and changes of water flow, which will affect the movement of the automatic driving car and the position of obstacles. Among them, the change of water flow will affect the propagation path and speed of sound waves, and then affect the measurement of distance. In dynamic environment modeling, it is necessary to consider the impact of water flow on the sound wave sensor data to ensure the accuracy of path planning, which involves the prediction and simulation of water flow patterns and the adjustment of the sound wave propagation path.

[0069] It should be noted that in the path planning of the autonomous vehicle, dynamic environment modeling is a comprehensive process, which includes multiple sub-models to describe the dynamic characteristics of the underwater environment. The key sub-models include the processing of acoustic sensor data, obstacle and terrain modeling, water flow impact model, dynamic Bayesian network model and path planning based on this information. 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 autonomous vehicle and obstacles in the water flow impact 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 velocity, θ is the water direction, and Δt is the time difference;

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

[0073] Each sub-model provides information about a specific aspect of the environment, which is integrated together to form a comprehensive dynamic environment model to guide the path planning of the autonomous vehicle. The "look" 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 to guide the autonomous vehicle to navigate safely and efficiently in complex underwater environments.

[0074] S300. Use Euclidean distance as a heuristic function to calculate the dynamic model of the underwater environment to obtain a first optimized path from the starting point to the end point, wherein the first optimized path is the global shortest optimal path calculated from the dynamic model of the underwater environment; combine the first optimized path, calculate and process through the model predictive control algorithm, and obtain a second optimized path that meets the kinematic and dynamic constraints of the automatic driving vehicle, wherein the second optimized path is a processed path that takes into account the relevant kinematic and dynamic constraints of the maximum speed, acceleration and steering angle of the automatic driving vehicle.

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

[0076] S301, input the dynamic model of the underwater environment as an input item into the heuristic function for calculation, select the node with the minimum f(n) value as the current node during the calculation process, 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, the path from the starting point to the end point is constructed by backtracking the parent node of each node, so as to obtain the global shortest optimal path to the target node. The calculation formula is as follows:

[0077]

[0078] 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, h(n) is the cost estimated by the heuristic function;

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

[0080] Where 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 an estimate of the total cost 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 prioritize those paths that appear to have the greatest potential to reach the target node. The A* algorithm will continue to expand (i.e., explore) the nodes with the smallest f(n) value until it reaches the target node or cannot move forward. In the path planning of underwater autonomous vehicles, the f(n) value is also applicable, where: g(n) may include the length or cost of the path that has actually been traversed, and h(n) may be based on a dynamic model of the underwater environment, such as the Euclidean distance or other appropriate heuristic functions that take into account factors such as water flow and obstacle distribution.

[0082] It is understandable that after obtaining the global optimal path in this step, only a theoretical shortest path is provided, which is based on the currently known environmental information, including the location of obstacles and terrain features. However, this path does not take into account the actual kinematic and dynamic constraints of the autonomous vehicle (AUV), such as the maximum steering angle, speed limit, acceleration limit, etc. Therefore, the model predictive control algorithm is used to optimize this global 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 autonomous vehicle. 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 in theory, but also feasible and safe in actual operation. In short, the global optimal path provided by the A* algorithm is the basis for the optimization processing 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 vehicle in actual operation, thereby obtaining the final optimized path.

[0083] At the same time, it should be noted that during the calculation process of the first optimization path, this path may not take into account the actual kinematic and dynamic characteristics of the automatic driving car, such as the maximum speed, acceleration, steering restrictions, etc. It is a path calculated from the dynamic model of the underwater environment using the Euclidean distance as a heuristic function, and this path is theoretically the shortest because it minimizes the straight-line distance from the starting point to the end point. The second optimization path is the path obtained by further processing through the model predictive control algorithm, in which the model predictive control algorithm takes into account the actual kinematic and dynamic constraints of the automatic driving car, and takes into account the kinematic and dynamic restrictions such as the maximum speed, acceleration, steering angle, etc. of the automatic driving car, and can also adapt to environmental changes in real time to ensure the actual feasibility and safety of the path. Therefore, the second optimization path is the path actually executed by the automatic driving car.

[0084] Understandably, due to the dynamic nature of the underwater environment, the global 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 status 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 can be understood that step S300 also includes S302, S303, S304 and S305, wherein:

[0086] S302: Establish a kinematic model of the automatic driving car, and obtain the initial conditions of the state variables and control inputs through state space representation processing. The calculation formula is as follows:

[0087]

[0088] In the formula, is the rate of change of the state variable over time, f is the state transfer function, x is the state variable, and u is the control input;

[0089] S303, based on the initial conditions, in each control cycle, the state prediction processing of the model predictive control algorithm is performed to obtain the state prediction sequence of the automatic driving car in the future period of time, and the calculation formula is as follows:

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

[0091] In the formula, x t+k is the state of the autonomous vehicle at the kth time step in the future (where k ranges from 1 to N) from the current time t, k is the step number index in the prediction time domain, indicating the kth time step in the future from the current time t, f is the state transfer function, x t is the state of the automatic driving car at the current time t, N is the total length of the prediction time domain, u t is the control input applied to the autonomous vehicle at the current time t;

[0092] It should be noted that in this step scenario, x t+k This state vector includes information such as the position and heading of the autonomous vehicle. t is a vector, which usually contains the position coordinates and heading angle (such as θt) of the automatic driving car; the control input for the automatic driving car may include speed (such as vt) and steering angular velocity (such as ωt). f describes how the state of the automatic driving car 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, based on the state prediction sequence, the quality of the path is evaluated by introducing the cost function definition to obtain a path quality evaluation result, wherein the cost function includes tracking error, change of control input and constraint conditions, and its calculation formula is as follows:

[0094]

[0095] Where w1 and w2 are weight coefficients, J is the total cost or performance index of the path, is to sum the costs of 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 time domain, tracking_error(x t+k , xtarget ) is the difference between the actual state xt+k of the automatic driving car and the expected target state xtarget, control_effort(u t+k ) is the cost of the control input ut+k required to achieve the desired state, x t+k is the state of the autonomous vehicle at the kth time step in the future (where k ranges from 1 to N) from the current time t, x target is the expected target state at time step t+k, 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 automatic car 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 automatic car at each time step with the expected target position and heading angle. The smaller the tracking error, the more accurately the automatic car can follow the predetermined path. The change in control input refers to the degree of change in the control input (such as speed and steering angle) required to guide the automatic car 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 and ensure the smooth operation and comfort of the automatic car. In addition, the constraints refer to the physical or operational restrictions that the automatic car needs to comply with during the movement. These may include maximum speed, maximum acceleration, maximum steering angle, etc. Therefore, in the model predictive control algorithm, the constraints ensure that the generated path is not only optimal in theory, but also feasible in actual operation and will not exceed the operational limit of the automatic car.

[0097] Therefore, the model predictive control algorithm comprehensively considers the tracking error, the change of the control input, and the constraints by defining a cost function to generate an optimized path that not only meets the kinematic and dynamic constraints of the autonomous vehicle, but also closely tracks 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 with the minimum tracking error and control input change while satisfying all constraints.

[0098] S305. Use numerical optimization technology to evaluate and solve the path quality evaluation results to obtain a control input sequence for the automatic driving car in the future, and apply the first control input to repeatedly optimize according to the latest system state in each control cycle to achieve path tracking and adjustment to obtain a second optimized path.

[0099] It should be noted that according to the path quality evaluation results, the model predictive control algorithm will generate 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 tracking errors, changes in control inputs, and constraints. The control input sequence of the automatic driving car in the future is determined by minimizing J. This step ensures that the path is not only optimal in theory, 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 automatic driving car in the future. 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 environmental changes. Quick response; after applying the first control input, the automatic driving car starts to drive along the path. During the driving process, the model predictive control algorithm will dynamically adjust the path according to the real-time status information and environmental feedback to keep on the predetermined path or adapt to environmental changes, that is, according to the real-time status information and environmental feedback, adjust its motion trajectory to keep on the predetermined path or adapt to environmental changes, and obtain the second optimized path. That is, the second optimized path is the path processed by the model predictive control algorithm, which not only considers the shortest path from the starting point to the end point, but also considers the actual movement ability of the automatic driving car, such as maximum speed, acceleration limit and steering ability, to ensure the actual feasibility and safety of the path.

[0100] In this step, the whole process includes model building, that is, the model predictive control algorithm builds the kinematic model of the automatic driving car, which usually includes the vehicle's state variables (such as position, speed and heading angle) and control inputs (such as front wheel angle and speed), where the state and input can be represented by the state space model; then the prediction model in each control cycle, the model predictive control algorithm uses the current state and control input to predict the state of a period of time in the future, which is usually achieved by constructing a state transfer matrix; followed by 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 tracking error, changes in control input and constraints; in the optimization process, the model predictive control algorithm also needs to consider kinematic and dynamic constraints, such as maximum speed, acceleration and steering angle limits, which can be introduced in the form of inequality constraints; finally, the above optimization problem is solved using numerical optimization methods (such as quadratic programming) to obtain a control input sequence for a period of time in the future, and the model predictive control algorithm usually only applies the first control input in the optimization result to enhance the ability to respond to environmental changes. Subsequently, in each control cycle, the model predictive control algorithm recalculates the control input based on the new state information to ensure that the autonomous vehicle can travel smoothly along the optimized path.

[0101] S400, the second optimized path is smoothed by dynamic motion primitive technology to obtain a smooth trajectory that meets the tracking requirements of the automatic driving vehicle; by acquiring lidar and inertial measurement unit data, the lidar inertial synchronous positioning and map construction algorithm is applied for processing, and a three-dimensional map of the underwater environment for path planning and obstacle avoidance has been constructed, wherein the inertial measurement unit data includes acceleration, angular velocity, pitch angle and roll angle.

[0102] It can be understood that the step S400 includes S401, S402, S403 and S404, wherein:

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

[0104]

[0105] Where y(t) is the position or velocity of the car 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, obtaining laser radar data, and obtaining the three-dimensional structure information of the environment through point cloud processing and feature extraction according to the point cloud data in the laser radar data. The calculation formula of the point cloud processing is as follows:

[0107] P global =T·P sensor

[0108] 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, 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 is calculated as a basis for judging corner points and plane points. The calculation of curvature usually involves the distribution of points in the local neighborhood of the point cloud. The variables involved in the formula include:

[0110]

[0111] Where 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, P 当前点 is the current point in the point cloud being processed;

[0112] S403, acquiring inertial measurement unit data, and obtaining a denoised inertial measurement data set through data preprocessing and filtering according to acceleration and angular velocity information in the inertial measurement unit data;

[0113] First, the laser radar point cloud is dedistorted using the inertial measurement unit data. Since the laser radar will produce displacement and rotation as the carrier moves during the scanning process, the point cloud data will be distorted. The inertial measurement unit data can be used to estimate the movement of the laser radar during the scanning period, and the point cloud can be corrected accordingly. The purpose is to obtain the dedistorted point cloud data and provide accurate input data for subsequent feature extraction and map construction. Feature points, including corner points and plane points, are extracted from the dedistorted point cloud data. These feature points will be used in the subsequent matching and map construction process. The purpose of feature extraction is to identify significant features in the environment so as to establish a connection 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 integration based on 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 a basis for judging corner points and plane points. The calculation of curvature usually involves the distribution of points in 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: use the inertial measurement unit data to estimate the sensor motion of the lidar during scanning, including rotation and translation; 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 angular velocity integral and acceleration integral 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 positioning and map construction tasks.

[0115] In this step, the raw data of the inertial measurement unit data is pre-integrated to estimate the movement of the sensor during the lidar scan, where the pre-integration result is used for the skew correction of the point cloud and as the initial pose estimation in the lidar odometer optimization process, with the aim of reducing the error caused by the dynamic movement of the sensor and improving the positioning accuracy.

[0116] S404. Through the tight coupling of three-dimensional structural information and inertial measurement data sets, using synchronous positioning and map building algorithms, combined with graph optimization technology, three-dimensional modeling of the underwater environment is achieved, wherein the three-dimensional modeling includes the construction and updating of the underwater map, thereby generating a three-dimensional map of the underwater environment for path planning and obstacle avoidance.

[0117] In this step, factor graph optimization technology is used to combine various data such as lidar odometer factor, inertial measurement unit factor, GPS factor (if any) and loopback factor for global optimization. The purpose is to estimate the bias of IMU and optimize the entire trajectory estimation to generate a continuous, high-precision three-dimensional map. Then, based on the optimized posture information and feature points, a three-dimensional map of the underwater environment is constructed. This map not only includes the geometric structure of the environment, but also the position and attitude information of the autonomous vehicle in the 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 synchronous positioning and map building algorithm processing is: lidar provides accurate three-dimensional structure of the environment, while inertial measurement unit data provides dynamic information of the AUV. The lidar inertial synchronous positioning and map building algorithm combines these two data sources and generates an accurate three-dimensional map of the underwater environment through data fusion technology. This map not only includes the geometric structure of the environment, but also the position and attitude information of the AUV in the environment. The process of generating a three-dimensional map usually includes the following steps: using lidar to scan the environment and obtain point cloud data of the environment; using IMU data to correct lidar data to compensate for the impact of the AUV's movement; performing data fusion through the LIO-SAM algorithm to generate a dynamically updated three-dimensional map; this map can be used for path planning and obstacle avoidance because it provides a detailed structure of the environment and the precise position of the AUV.

[0119] S500, the three-dimensional map is processed through sensor fusion perception, the current state information of dynamic obstacles in the surrounding environment is extracted, and the prediction model is used to build and process the obstacles, and based on the current state of the obstacles and their motion characteristics, the movement trajectory of the obstacles in the future is predicted.

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

[0121] It can be understood that step S500 includes S501, S502 and S503, wherein:

[0122] S501, using a random sample consensus algorithm to identify and separate feature points of dynamic obstacles, wherein data points are randomly selected and fitted to a model, residuals between the model and all observed data are calculated, and the difference between the residual and a preset threshold is determined. If the residual is less than a given threshold, it is determined as an inlier, and all obtained inliers are aggregated to obtain an inlier set of the dynamic obstacle, wherein the inlier set includes feature point information of the dynamic obstacle;

[0123] The residuals for all observations are calculated 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 prediction value is the predicted data calculated based on the fitted model.

[0126] S502: Based on the interior point set, a support vector machine is used to build a model to obtain a motion feature model of the dynamic obstacle, wherein the motion feature model is used to predict the future state of the obstacle. The support vector machine separates data of different categories by finding an optimal hyperplane in the feature space. The calculation formula is as follows:

[0127]

[0128] sty i (w·φ(x i )+b)≥1-ξ i ,ξ i ≥0

[0129] Where w is the normal vector of the hyperplane, b is the bias term, ξi and ξ are slack variables, C is the penalty parameter, yi is the category 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 st 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 in the correct classification area; if ξi > 0, the sample point violates the classification boundary, but is still allowed to ensure that the model can handle nonlinearly separable data sets.

[0131] S503: Based on the motion feature model of the dynamic obstacle, the expected trajectory of the obstacle in the future is dynamically updated using the Kalman filter, where the expected trajectory includes position and velocity information, and the calculation formula is as follows:

[0132]

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

[0134] In the formula, is the prior state estimate at time k, A is the state transfer 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.

[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 steps are 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 the future moment. This motion model takes into account the speed 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 set of new particles, where the particles represent the possible future state distribution of the obstacle; the next step is the update step. When new information is obtained from the sensor, the prediction step is updated. When the observation data is received, 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 weighs the predicted state and the observed 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 observed data. The weights of the particles reflect the consistency of each particle with the observed data. Then, the particle filter may generate a new set of particles through a resampling step to concentrate on representing the most likely state estimate; finally, an iterative update is performed, which includes the prediction and update steps of the Kalman filter or particle filter at each time step. As new observation data continues to arrive, 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 particle filter can provide continuous predictions of the future position of obstacles. These predictions take into account the motion characteristics of the obstacles and the uncertainty factors in the environment. The method adopted in this step is suitable for path planning in a dynamic environment. It can provide real-time estimates of the future state of obstacles, so that the autonomous vehicle can make obstacle avoidance decisions in a timely manner.

[0137] S600: Based on the smooth trajectory tracked by the automatic driving car and the future movement trajectory of the obstacle, the adaptive control system adjusts and processes in real time to 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 automatic driving car.

[0138] It can be understood that step S600 includes step S601 and step S602, wherein:

[0139] S601, based on the smooth trajectory tracked by the automatic driving car, the future movement trajectory of the obstacle and the preset dynamic model of the automatic driving car, the motion state of the automatic driving car under the influence of wind, waves and water flow is obtained through integrated processing of real-time wind, wave and water flow data, wherein 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, in which the proportional (P), integral (I), and differential (D) parameters are automatically adjusted according to the change of the error signal to achieve real-time control of the AUV's 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 that includes wind, wave, and current influence terms can be used:

[0141]

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

[0143] It is understandable that the dynamic model of the self-driving car is usually pre-set. It is a mathematical model based on physical principles and experimental data, which is used to describe and predict the motion behavior of the car when it is subject to control input. This model includes factors such as the mass, size, shape, thruster thrust, water resistance, buoyancy, etc. of the car, and how they affect the motion state of the car. It predicts the response of the car based on the control input, such as how the car will move under a given control input; and uses the model to plan the path of the car to ensure that the car can travel along the predetermined trajectory while avoiding obstacles. During the calculation process, a PID controller or model predictive control (MPC) is used to achieve precise control of the car's motion.

[0144] S602, using fuzzy logic control method to adjust the dynamic control strategy according to the motion state, 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 automatic driving vehicle, avoiding obstacles and adapting to changing environmental conditions, thereby completing the path planning and tracking tasks.

[0145] It should be noted that fuzzy logic control methods are used to deal with uncertainty and nonlinear problems by defining a series of fuzzy rules to determine how to adjust the control output according to the input error and rate of change. They are suitable for complex systems that are difficult to describe with traditional mathematical models. The purpose of these control algorithms is to generate precise control instructions to regulate the movement of the autonomous vehicle, ensuring that it can smoothly and stably travel along the planned path and maintain the correct course even under the influence of external environmental factors such as wind, waves and water currents. Through these algorithms, the autonomous vehicle can automatically adjust its speed, direction and posture to adapt to real-time changing environmental conditions, thereby completing path planning and tracking tasks.

[0146] Embodiment 2:

[0147] like Figure 2 As shown, this embodiment provides a path planning system for an automatic driving vehicle, see Figure 2 The system comprises:

[0148] Acquisition module 701: used to acquire acoustic wave sensor data of the underwater environment, wherein the acoustic wave sensor data includes the frequency and intensity of the transmitted acoustic wave and the timestamp and amplitude information of the received reflected acoustic wave, and obtains the contour of the underwater terrain and the location information of the obstacle through the acoustic wave propagation time calculation processing according to the difference between the first timestamp of the transmitted acoustic wave and the second timestamp of the received acoustic wave;

[0149] The first processing module 702 is used to obtain a dynamic model of the underwater environment based on the contour of the underwater terrain and the location information of the obstacles through dynamic environment modeling processing, wherein the dynamic model includes the terrain, the distribution of obstacles and the influence of water flow;

[0150] Calculation module 703: used to calculate the dynamic model of the underwater environment using the Euclidean distance as a heuristic function to obtain a first optimized path from the starting point to the end point, wherein the first optimized path is the global shortest optimal path calculated from the dynamic model of the underwater environment; combined with the first optimized path, a second optimized path that meets the kinematic and dynamic constraints of the automatic driving vehicle is obtained through calculation and processing by a model predictive control algorithm, wherein the second optimized path is a path obtained after processing considering the kinematic and dynamic constraints, wherein the constraints include maximum speed, acceleration limit, and steering angle limit;

[0151] The second processing module 704 is used to smooth the second optimized path through the dynamic motion primitive technology to obtain a smooth trajectory that meets the tracking requirements of the automatic driving vehicle; by acquiring laser radar and inertial measurement unit data, applying the laser radar inertial synchronous positioning and map building algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance has been constructed, wherein the inertial measurement unit data includes acceleration, angular velocity, pitch angle and roll angle;

[0152] Prediction module 705: used to process the three-dimensional map through sensor fusion perception, extract the current state information of dynamic obstacles in the surrounding environment, build and process using the prediction model, and predict its movement trajectory in the future based on the current state of the obstacle and its motion characteristics;

[0153] Planning module 706: It is used to obtain stable path tracking control instructions that can adapt to different wind waves and water flow changes based on the smooth trajectory tracked by the automatic driving car and the future movement trajectory of the obstacle through real-time adjustment and processing by the adaptive control system, thereby completing the path planning of the automatic driving car.

[0154] Specifically, the calculation module 701 includes:

[0155] Calculation unit: 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 it is 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, its f(n) value is calculated, and its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function are updated as needed. The above process is repeated until the target node is added to the closed list or the open list is empty. If the target node is found, the path from the starting point to the end point is constructed by backtracking the parent node of each node, so as to obtain the global shortest optimal path to the target node. The 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, h(n) is the cost estimated by the heuristic function;

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

[0159] Where 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 calculation module 701 includes:

[0161] Model building unit: used to build the kinematic model of the automatic driving car. After state space representation processing, the initial conditions of state variables and control inputs are obtained. The calculation formula is as follows:

[0162]

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

[0164] Prediction processing unit: Based on the initial conditions, in each control cycle, the state prediction processing of the model predictive control algorithm is performed to obtain the state prediction sequence of the automatic driving car in the future. The 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 kth time step in the future (where k ranges from 1 to N) from the current time t, k is the step number index in the prediction time domain, indicating the kth time step in the future from the current time t, f is the state transfer function, x t is the state of the automatic driving car at the current time t, N is the total length of the prediction time domain, u t is the control input applied to the autonomous vehicle at the current time t;

[0167] Evaluation unit: used to evaluate the quality of the path according to the state prediction sequence and obtain the path quality evaluation result through the introduction of the cost function definition. The cost function includes the tracking error, the change of the control input and the constraint condition. The calculation formula is as follows:

[0168]

[0169] Where w1 and w2 are weight coefficients, J is the total cost or performance index of the path, is to sum the costs of 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 time domain, tracking_error(x t+k , x target ) is the actual state x of the automatic driving car t+k The difference between the desired target state xtarget, control_effort(ut+k ) is the control input u required to achieve the desired state t+k The cost, x t+k is the state of the autonomous vehicle at the kth time step in the future (where k ranges from 1 to N) from the current time t, x target is the expected target state at time step t+k, u t+k is the control input at time step t+k;

[0170] Solution optimization unit: It is used to evaluate and solve the path quality evaluation results using numerical optimization technology, obtain the control input sequence of the automatic driving car in the future, and apply the first control input to repeatedly optimize according to the latest system status in each control cycle to achieve path tracking and adjustment, and obtain the second optimized path.

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

[0172] Adjustment unit: used to dynamically adjust the path points according to the vertex sequence of the second optimization path through the radial basis function neural network to generate a continuous smooth trajectory. The calculation formula is as follows:

[0173]

[0174] Where y(t) is the position or velocity of the car 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 laser radar data. According to the point cloud data in the laser radar data, the three-dimensional structure information of the environment is obtained through point cloud processing and feature extraction. The calculation formula for point cloud processing is as follows:

[0176] P global =T·P sensor

[0177] 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, P sensor is the point cloud data in the sensor coordinate system;

[0178] Acquisition unit: used to acquire inertial measurement unit data. According to the acceleration and angular velocity information in the inertial measurement unit data, after data preprocessing and filtering, a denoised inertial measurement data set is obtained;

[0179] Combination unit: It is used to realize 3D modeling of underwater environment by tightly coupling 3D structural information and inertial measurement data sets, utilizing synchronous positioning and map building algorithms, and combining graph optimization technology. The 3D modeling includes the construction and updating of underwater maps, thereby generating a 3D map of the underwater environment for path planning and obstacle avoidance.

[0180] Specifically, the prediction module 705 includes:

[0181] Judgment unit: used to identify and separate the feature points of dynamic obstacles using a random sample consensus algorithm, wherein the residual between the model and all observed data is calculated by randomly selecting data points and fitting the model, and the residual is judged to be smaller than a preset threshold. If the residual is smaller than a given threshold, it is judged as an inlier, and all the obtained inliers are aggregated to obtain an inlier set of dynamic obstacles, wherein the inlier set includes the feature point information of the dynamic obstacles;

[0182] Modeling unit: used to model the internal point set using support vector machine 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 separates different categories of data by finding an optimal hyperplane in the feature space. The calculation formula is as follows:

[0183]

[0184] sty i (w·φ(x i )+b)≥1-ξ i ,ξ i ≥0

[0185] Where w is the normal vector of the hyperplane, b is the bias term, ξi and ξ are slack variables, C is the penalty parameter, yi is the category 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 st 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 based on the motion feature model of the dynamic obstacle using the Kalman filter. The expected trajectory includes position and velocity information. The 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 transfer 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.

[0190] Specifically, the planning module 706 includes:

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

[0192] Computing instruction unit: It is used to adjust the dynamic control strategy according to the motion state using fuzzy logic control method, and calculate the path tracking control instructions that adapt to the changes in wind, waves and water currents, including adjusting the speed, direction and thruster force of the automatic driving vehicle, while avoiding obstacles and adapting to changing environmental conditions, thereby completing path planning and tracking tasks.

[0193] It should be noted that, regarding the system in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.

[0194] Embodiment 3:

[0195] Corresponding to the above method embodiment, this embodiment also provides a path planning device for an automatic driving vehicle. The path planning device for an automatic driving vehicle described below and the path planning method for an automatic driving vehicle described above can be referred to each other.

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

[0197] The processor 801 is used to control the overall operation of the path planning device 800 of the automatic driving car to complete all or part of the steps in the path planning method of the automatic driving car. The memory 802 is used to store various types of data to support the operation of the path planning device 800 of the automatic driving car. These data may include, for example, instructions for any application or method operated on the path planning device 800 of the automatic driving car, and application-related data, such as contact data, sent and received messages, pictures, audio, video, etc. 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, disk or optical disk. The multimedia component 803 may include a screen and an audio component. 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, which is used to receive external audio signals. The received audio signal may be further stored in the memory 802 or sent via the communication component 805. The audio component also 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 can be a keyboard, a mouse or buttons, etc. These buttons can 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 automatic driving car 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, so the corresponding communication component 805 can include: a Wi-Fi module, a Bluetooth module or an NFC module.

[0198] In an exemplary embodiment, the path planning device 800 of the automatic driving vehicle can be implemented by one or more application specific integrated circuits (Application Specific Integrated Circuit, referred to as ASIC), digital signal processors (Digital Signal Processor, referred to as DSP), digital signal processing devices (Digital Signal Processing Device, referred to as DSPD), programmable logic devices (Programmable Logic Device, referred to as PLD), field programmable gate arrays (Field Programmable Gate Array, referred to as FPGA), controllers, microcontrollers, microprocessors or other electronic components to execute the above-mentioned path planning method for the automatic driving vehicle.

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

[0200] Embodiment 4:

[0201] Corresponding to the above method embodiment, a readable storage medium is also provided in this embodiment. The readable storage medium described below and the path planning method for an automatic driving vehicle described above can be referred to each other.

[0202] A computer program is stored on the readable storage medium, and when the computer program is executed by the processor, the steps of the path planning method for the automatic driving vehicle in the above method embodiment are implemented.

[0203] The readable storage medium may specifically be a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, or other readable storage medium that can store program codes.

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

[0205] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A path planning method for an automatic driving vehicle, characterized in that: include: Acquire acoustic wave sensor data of the underwater environment, wherein the acoustic wave sensor data includes the frequency and intensity of the transmitted acoustic wave and the timestamp and amplitude information of the received reflected acoustic wave, and obtain the contour of the underwater terrain and the location information of the obstacle through the acoustic wave propagation time calculation and processing based on the difference between the first timestamp of the transmitted acoustic wave and the second timestamp of the received acoustic wave; Based on the contour of the underwater terrain and the location information of the obstacles, a dynamic model of the underwater environment is obtained through dynamic environment modeling processing, wherein the dynamic model includes the terrain, the distribution of obstacles and the influence of water flow; The dynamic model of the underwater environment is calculated using the Euclidean distance as a heuristic function to obtain a first optimization path from the starting point to the end point, wherein the first optimization path is the global shortest optimal path calculated from the dynamic model of the underwater environment; combined with the first optimization path, a second optimization path that meets the kinematic and dynamic constraints of the automatic driving vehicle is obtained through calculation and processing using a model predictive control algorithm, wherein the second optimization path is a path obtained after processing considering the kinematic and dynamic constraints, wherein the constraints include maximum speed, acceleration limit, and steering angle limit; The second optimized path is smoothed by using dynamic motion primitive technology to obtain a smooth trajectory that meets the requirements of the automatic driving car tracking; by acquiring laser radar and inertial measurement unit data and applying the laser radar inertial synchronous positioning and map building algorithm for processing, a three-dimensional map of the underwater environment for path planning and obstacle avoidance has been constructed, in which the inertial measurement unit data includes acceleration, angular velocity, pitch angle and roll angle; The 3D map is processed through sensor fusion perception to extract the current state information of dynamic obstacles in the surrounding environment, and the prediction model is used to build and process it. Based on the current state of the obstacle and its motion characteristics, its movement trajectory in the future is predicted; According to the smooth trajectory tracked by the automatic driving car and the future movement trajectory of the obstacle, the adaptive control system adjusts and processes in real time to obtain stable path tracking control instructions that can adapt to different wind waves and water flow changes, thereby completing the path planning of the automatic driving car.

2. The path planning method for an autonomous vehicle according to claim 1, characterized in that: The method uses the Euclidean distance as a heuristic function to calculate the dynamic model of the underwater environment to obtain a first optimized path from the starting point to the end point, which includes: The dynamic model of the underwater environment is input 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, its f(n) value is calculated, and its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function are updated as needed. The above process is repeated until the target node is added to the closed list or the open list is empty. If the target node is found, the path from the starting point to the end point is constructed by backtracking the parent node of each node, so as to obtain the global shortest optimal path to the target node. The calculation formula is as follows: 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, h(n) is the cost estimated by the heuristic function; f(n)=g(n)+h(n) Where 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 for an autonomous vehicle according to claim 1, characterized in that: The first optimization path is combined with the model predictive control algorithm to obtain a second optimization path that meets the kinematic and dynamic constraints of the automatic driving vehicle, including: The kinematic model of the automatic driving car is established. After state space representation processing, the initial conditions of the state variables and control inputs are obtained. The calculation formula is as follows: In the formula, is the rate of change of the state variable over time, f is the state transfer function, x is the state variable, and u is the control input; Based on the initial conditions, in each control cycle, the state prediction sequence of the automatic driving car in the future is obtained through the state prediction processing of the model predictive control algorithm. The calculation formula is as follows: x t+k =f(x t ,u t ) (k=1,2,...,N) In the formula, x t+k is the state of the autonomous vehicle at the kth time step in the future (where k ranges from 1 to N) from the current time t, k is the step number index in the prediction time domain, indicating the kth time step in the future from the current time t, f is the state transfer function, x t is the state of the automatic driving car at the current time t, N is the total length of the prediction time domain, u t is the control input applied to the autonomous vehicle at the current time t; According to the state prediction sequence, the quality of the path is evaluated by introducing the cost function definition, and the path quality evaluation result is obtained. The cost function includes tracking error, change of control input and constraint conditions. Its calculation formula is as follows: Where w1 and w2 are weight coefficients, J is the total cost or performance index of the path, is to sum the costs of 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 time domain, tracking_error(x t+k , x target ) is the actual state x of the automatic driving car t+k The difference between the desired target state xtarget, control_effort(u t+k ) is the control input u required to achieve the desired state t+k The cost, x t+k is the state of the autonomous vehicle at the kth time step in the future (where k ranges from 1 to N) from the current time t, x target is the expected target state at time step t+k, u t+k is the control input at time step t+k; The numerical optimization technology is used to evaluate and solve the path quality assessment results to obtain the control input sequence of the automatic driving car in the future. The first control input is applied to repeatedly optimize according to the latest system status in each control cycle to achieve path tracking and adjustment, and obtain the second optimized path.

4. The path planning method for an autonomous vehicle according to claim 1, characterized in that: The second optimized path is smoothed by the dynamic motion primitive technology to obtain a smooth trajectory that meets the tracking requirements of the automatic driving vehicle; by acquiring laser radar and inertial measurement unit data, the laser radar inertial synchronous positioning and map construction algorithm is applied to process, and 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 the 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 car 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; Obtain the laser radar data, and obtain the three-dimensional structure information of the environment through point cloud processing and feature extraction based on the point cloud data in the laser radar data. 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, P sensor is the point cloud data in the sensor coordinate system; Acquire the inertial measurement unit data, and obtain the denoised inertial measurement data set through data preprocessing and filtering based on the acceleration and angular velocity information in the inertial measurement unit data; Through the tight coupling of three-dimensional structural information and inertial measurement data sets, the use of synchronous positioning and map construction algorithms, combined with graph optimization technology, three-dimensional modeling of the underwater environment is achieved. The three-dimensional modeling includes the construction and updating of underwater maps, thereby generating a three-dimensional map of the underwater environment for path planning and obstacle avoidance.

5. The path planning method for an autonomous vehicle according to claim 1, characterized in that: The three-dimensional map is processed through sensor fusion perception to extract the current state information of dynamic obstacles in the surrounding environment, and the prediction model is used to build and process it, and based on the current state of the obstacle and its motion characteristics, its movement trajectory in the future is predicted, including: The random sample consensus algorithm is used to identify and separate the characteristic points of dynamic obstacles. The data points are randomly selected and fitted to the model. The residuals of the model and all the observed data are calculated. The residuals are compared with the preset threshold. If the residual is less than the given threshold, it is judged as an inlier. All the obtained inliers are clustered to obtain the inlier set of dynamic obstacles, where the inlier set includes the characteristic point information of the dynamic obstacles. According to the internal point set, support vector machine is used 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 separates different categories of data by finding an optimal hyperplane in the feature space. The calculation formula is as follows: s.t.y i (w·φ(x i )+b)≥1-ξi,ξi≥0 Where w is the normal vector of the hyperplane, b is the bias term, ξi and ξ are slack variables, C is the penalty parameter, yi is the category 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 st is the constraint condition in the specified optimization problem; Based on the motion characteristic model of dynamic obstacles, the Kalman filter is used to dynamically update the expected trajectory of the obstacle in the future. The expected trajectory includes position and velocity information, and its calculation formula is as follows: P k|k-1 =AP k-1|k-1 From T +Q In the formula, is the prior state estimate at time k, A is the state transfer 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 for an autonomous vehicle according to claim 1, characterized in that: The smooth trajectory tracked by the automatic driving car and the future trajectory of the obstacle are adjusted and processed in real time by the adaptive control system to 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 automatic driving car, including: Based on the smooth trajectory tracked by the autonomous vehicle, the future trajectory of obstacles and the preset dynamic model of the autonomous vehicle, the motion state of the autonomous vehicle under the influence of wind, waves and water flow is obtained through the integrated processing of real-time wind, wave and water flow data. The adaptive control algorithm adjusts the control parameters to adapt to the dynamic changes of the environment. The dynamic control strategy is adjusted according to the motion state using fuzzy logic control method to calculate the path tracking control instructions that adapt to changes in wind, waves and water currents. This includes adjusting the speed, direction and thruster force of the automatic driving 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 according to claim 1, characterized in that: include: Acquisition module: used to acquire acoustic sensor data of the underwater environment, wherein the acoustic sensor data includes the frequency and intensity of the transmitted acoustic wave and the timestamp and amplitude information of the received reflected acoustic wave, and obtains the contour of the underwater terrain and the location information of obstacles through the calculation and processing of the acoustic wave propagation time according to the difference between the first timestamp of the transmitted acoustic wave and the second timestamp of the received acoustic wave; The first processing module is used to obtain a dynamic model of the underwater environment based on the contour of the underwater terrain and the location information of the obstacles through dynamic environment modeling processing, wherein the dynamic model includes the terrain, the distribution of obstacles and the influence of water flow; Calculation module: used to calculate the dynamic model of the underwater environment using the Euclidean distance as a heuristic function to obtain a first optimization path from the starting point to the end point, wherein the first optimization path is the global shortest optimal path calculated from the dynamic model of the underwater environment; combined with the first optimization path, a second optimization path that meets the kinematic and dynamic constraints of the automatic driving vehicle is obtained through calculation and processing by the model predictive control algorithm, wherein the second optimization path is a path obtained after processing considering the kinematic and dynamic constraints, wherein the constraints include maximum speed, acceleration limit and steering angle limit; The second processing module is used to smooth the second optimized path through the dynamic motion primitive technology to obtain a smooth trajectory that meets the tracking requirements of the automatic driving car; by acquiring the laser radar and inertial measurement unit data, the laser radar inertial synchronous positioning and map construction algorithm is applied for processing, and a three-dimensional map of the underwater environment for path planning and obstacle avoidance has been constructed, wherein the inertial measurement unit data includes acceleration, angular velocity, pitch angle and roll angle; Prediction module: used to process the 3D map through sensor fusion perception, extract the current state information of dynamic obstacles in the surrounding environment, build and process using the prediction model, and predict its movement trajectory in the future based on the current state of the obstacle and its motion characteristics; Planning module: It is used to obtain stable path tracking control instructions that can adapt to different wind waves and water flow changes based on the smooth trajectory tracked by the automatic driving car and the future movement trajectory of the obstacle through real-time adjustment and processing by the adaptive control system, thereby completing the path planning of the automatic driving car.

8. The path planning system for an autonomous vehicle according to claim 7, characterized in that: The computing module includes: Calculation unit: 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 it is 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, its f(n) value is calculated, and its parent node, the actual cost from the starting point to the current node, and the cost value estimated by the heuristic function are updated as needed. The above process is repeated until the target node is added to the closed list or the open list is empty. If the target node is found, the path from the starting point to the end point is constructed by backtracking the parent node of each node, so as to obtain the global shortest optimal path to the target node. The calculation formula is as follows: 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, h(n) is the cost estimated by the heuristic function; f(n)=g(n)+h(n) Where 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 for an autonomous vehicle according to claim 7, characterized in that: The second processing module comprises: Adjustment unit: used to dynamically adjust the path points according to the vertex sequence of the second optimization path through the 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 car 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: used to obtain laser radar data. According to the point cloud data in the laser radar data, the three-dimensional structure information of the environment is obtained through point cloud processing and feature extraction. 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, P sensor is the point cloud data in the sensor coordinate system; Acquisition unit: used to acquire inertial measurement unit data. According to the acceleration and angular velocity information in the inertial measurement unit data, after data preprocessing and filtering, a denoised inertial measurement data set is obtained; Combination unit: It is used to realize 3D modeling of underwater environment by tightly coupling 3D structural information and inertial measurement data sets, utilizing synchronous positioning and map building algorithms, and combining graph optimization technology. The 3D modeling includes the construction and updating of underwater maps, thereby generating a 3D map of the underwater environment for path planning and obstacle avoidance.

10. The path planning system for an autonomous vehicle according to claim 7, characterized in that: The prediction module includes: Judgment unit: used to identify and separate the feature points of dynamic obstacles using a random sample consensus algorithm, wherein the residual between the model and all observed data is calculated by randomly selecting data points and fitting the model, and the residual is judged to be smaller than a preset threshold. If the residual is smaller than a given threshold, it is judged as an inlier, and all the obtained inliers are aggregated to obtain an inlier set of dynamic obstacles, wherein the inlier set includes the feature point information of the dynamic obstacles; Modeling unit: used to model the internal point set using support vector machine 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 separates different categories of data by finding an optimal hyperplane in the feature space. The calculation formula is as follows: style i (w·φ(x i )+b)≥1-ξ i ,x i ≥0 Where w is the normal vector of the hyperplane, b is the bias term, ξi and ξ are slack variables, C is the penalty parameter, yi is the category 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 st 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 based on the motion feature model of the dynamic obstacle using the Kalman filter. The expected trajectory includes position and velocity information. The calculation formula is as follows: P k|k-1 =AP k-1|k-1 From T +Q In the formula, is the prior state estimate at time k, A is the state transfer 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