Uneven terrain adaptive vehicle path planning method and system based on multi-objective optimization

Abstract

The application discloses a kind of uneven terrain self-adaptive vehicle path planning method and system based on multi-objective optimization, the method includes obtaining the point cloud data of vehicle scheduled work area and preprocessing, constructs three-dimensional point cloud map;Based on the construction of terrain attribute model of preprocessed point cloud, the dynamics of vehicle is calculated, and uneven terrain model is generated;Two-dimensional rough trajectory is generated using Eco-KinoA* path search algorithm;Two-dimensional trajectory is obtained by optimizing through back-end multi-objective optimization algorithm;Finally, three-dimensional vehicle trajectory is generated in combination with terrain model.In the present application, the problems of insufficient terrain characterization, lack of vehicle and terrain dynamic interaction modeling, insufficient multi-objective collaborative optimization capability, and poor real-time performance and adaptability when planning vehicle paths on uneven terrain are solved, significantly improving the reliability, energy efficiency and safety of path planning.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving and robot navigation, and particularly relates to a self-adaptive vehicle path planning method and system based on multi-objective optimization for uneven terrain. BACKGROUND

[0002] With the accelerated penetration of automatic driving technology in complex scenarios such as field exploration, agricultural operation, disaster rescue and planet exploration, the efficient, safe and autonomous navigation capability of ground unmanned vehicles (UGV) in unstructured and three-dimensional uneven terrain has become a core technical bottleneck for its large-scale application.

[0003] At present, the traditional path planning method mainly faces the flat or structured road environment, and only relies on two-dimensional map information, which is difficult to effectively cope with the comprehensive influence of key terrain factors such as slope, lateral inclination and ground roughness in undulating terrain. Specifically, the existing technology has significant limitations in the aspect of terrain fine expression: it cannot accurately depict the geometric characteristics (such as height and normal vector) and physical properties (such as roughness) of three-dimensional terrain, and it is also difficult to support real-time and efficient terrain query and dynamic update, resulting in insufficient ability to capture terrain details (such as local steep slope and soft foundation road section) for path planning, which cannot provide reliable basis for trafficability evaluation, directly affecting the safety and reliability of navigation.

[0004] On the other hand, the driving performance of the vehicle in the uneven terrain is strongly coupled with the terrain characteristics, but the existing methods generally do not establish an effective vehicle-terrain dynamics correlation model, which directly affects the stability and energy efficiency of path execution. For example, slope directly affects the distribution of traction and braking force, lateral inclination determines the lateral stability boundary, and ground roughness significantly changes the driving energy consumption and body vibration level - if the path planning ignores these dynamic coupling relationships, the generated path may exceed the actual traffic capacity of the vehicle (such as skidding caused by steep slope and rapid acceleration, and rollover caused by lateral inclination exceeding limit), or cause energy waste due to unreasonable use of terrain potential (such as downhill sliding and frequent rapid acceleration on uphill). Especially in the electric vehicle or field long-time operation scenario, energy efficiency has become a core indicator, but the existing technology mostly takes the shortest path or the shortest time as a single optimization target, lacks a multi-objective collaborative trade-off mechanism for energy efficiency, safety performance, smoothness, and attitude stability, and is difficult to achieve reasonable planning speed in complex scenarios such as uphill and downhill to reduce kinetic energy loss and energy waste, ultimately resulting in low path execution efficiency, high energy consumption or instability risk. Therefore, there is an urgent need for a new path planning method that integrates terrain fine perception, vehicle-terrain dynamic modeling and multi-objective energy efficiency optimization to break through the technical bottleneck of UGV autonomous navigation in uneven terrain. SUMMARY

[0005] The embodiment of the present application provides a kind of uneven terrain adaptive vehicle path planning method and system based on multi-objective optimization, to solve the technical problems such as insufficient terrain accurate characterization, lack of vehicle and terrain dynamic interaction modeling, insufficient multi-objective collaborative optimization capability and poor real-time and adaptability when the prior art is carried out under uneven terrain conditions Vehicle path planning, to improve the reliability, energy efficiency performance and safety of path planning.

[0006] A kind of uneven terrain adaptive vehicle path planning method based on multi-objective optimization, comprising:

[0007] Obtain the point cloud data of the predetermined work area of vehicle, preprocess point cloud data, and establish the three-dimensional point cloud map of predetermined work area;

[0008] Based on the terrain data infrastructure constructed based on preprocessed point cloud data, a terrain attribute model is further constructed, the terrain attribute at any continuous point SE (2) state sequence is queried and the gradient is analyzed using the terrain attribute model, the vehicle dynamics is calculated in combination with the gradient, and the uneven terrain model is constructed;

[0009] Based on the uneven terrain model, a preliminary two-dimensional rough trajectory is generated using Eco-KinoA* path search algorithm;

[0010] A multi-objective optimization function covering trajectory smoothness cost, curvature cost, energy cost and vehicle attitude stability cost is established using a segmented polynomial to represent the trajectory, taking the preliminary two-dimensional rough trajectory as the starting point, and an optimized two-dimensional trajectory is obtained in combination with vehicle dynamics constraints, nonholonomic constraints and terrain constraints;

[0011] According to the optimized two-dimensional trajectory and the uneven terrain model, a final three-dimensional vehicle trajectory is generated.

[0012] The present application also provides a kind of uneven terrain adaptive vehicle path planning system based on multi-objective optimization, comprising:

[0013] Data acquisition and point cloud map construction module is used for obtaining the point cloud data of the predetermined work area of vehicle, preprocessing point cloud data, and establishing the three-dimensional point cloud map of predetermined work area;

[0014] Terrain attribute modeling and dynamics calculation module is used for constructing terrain data infrastructure based on preprocessed point cloud data, further constructing terrain attribute model, querying terrain attribute at any continuous point SE (2) state sequence and analyzing gradient using terrain attribute model, calculating vehicle dynamics in combination with gradient, and constructing uneven terrain model;

[0015] a coarse trajectory generation module configured to generate a preliminary two-dimensional coarse trajectory based on the uneven terrain model by using an Eco-Kino A* path search algorithm;

[0016] a multi-objective trajectory optimization and constraint fusion module configured to use a backend trajectory optimization algorithm to represent the trajectory by using a piecewise polynomial, to take the preliminary two-dimensional coarse trajectory as a starting point, to establish a multi-objective optimization function covering a trajectory smoothness cost, a curvature cost, an energy cost and a vehicle attitude stability cost, and to combine vehicle dynamics constraints, nonholonomic constraints and terrain constraints to obtain an optimized two-dimensional trajectory;

[0017] a three-dimensional trajectory generation and output module configured to generate a final three-dimensional vehicle trajectory based on the optimized two-dimensional trajectory and the uneven terrain model.

[0018] In the present application, the energy consumption-aware heuristic function (such as the Eco-Kino A* algorithm) is introduced in the front-end path search, which means that the impact of different path choices on energy consumption will be considered during the search process, so as to preferentially select those paths with lower energy consumption. At the same time, the energy-saving cost term in the back-end multi-objective optimization, the slope-aware speed planning strategy and the energy-saving cost term are integrated, which helps the vehicle to perform energy-saving motion on the variable terrain (uneven terrain), and the vehicle can intelligently adapt to the terrain undulation, effectively utilize the gravitational potential energy, and avoid energy waste caused by sudden acceleration and braking. For example, the power output is reduced by sliding on a downhill section, and the speed curve is optimized on an uphill section to reduce peak energy consumption, thereby significantly reducing the overall energy consumption in complex terrain, prolonging the endurance time of electric vehicles or field operation equipment, and thus significantly improving the energy utilization efficiency and operation endurance capability.

[0019] In the present application, by cooperatively optimizing the smoothness and curvature of the trajectory, the high-order derivative continuity of the trajectory in the time dimension is ensured, the mechanical impact and vibration during vehicle execution are reduced, and the ride comfort or load stability is improved. By suppressing unnecessary bending, the path is more intuitive, the difficulty of vehicle tracking control is reduced, and the predictability and fluency of the overall navigation behavior are enhanced. This helps to improve the autonomous navigation performance of the vehicle in complex terrain, especially in scenarios that require precise control and predictability. The optimized path reduces sharp turns and unnecessary steering, making the vehicle travel more smoothly, while also reducing the burden on the vehicle control system, thereby comprehensively optimizing the path quality and enhancing the ride smoothness and predictability.

[0020] In the present application, by fine three-dimensional terrain modeling, especially by fully considering the pose estimation of the wheel ground contact, the geometric features (such as height, normal vector) and physical properties (such as roughness) of the terrain can be more accurately captured, thereby providing more abundant terrain information for path planning. The influence of the terrain on the vehicle dynamics is comprehensively included in the constraint and cost function, ensuring the high terrain adaptability of the planned path. At the same time, by considering the influence of slope on traction and braking force, the influence of roll on lateral stability, and the influence of ground roughness on driving energy consumption and vehicle body vibration. By combining soft penalty and hard constraint on vehicle attitude, it effectively avoids the possibility of excessive roll or pitch caused by sudden changes in terrain, significantly improves the safety, attitude stability and adaptability of the vehicle when driving in uneven and potentially dangerous terrain, and expands the effective operation range of the vehicle. This helps to reduce the risk of vehicle instability, skidding or getting stuck in complex terrain, and improves the applicability and reliability of the vehicle in field exploration, agricultural operation, disaster rescue and other scenarios. BRIEF DESCRIPTION OF DRAWINGS

[0021] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings needed to be used in the description of the embodiments of the present application. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0022] Figure 1 is a flowchart of an uneven terrain self-adaptive vehicle path planning method based on multi-objective optimization in an embodiment of the present application;

[0023] Figure 2 is a structural schematic diagram of an uneven terrain self-adaptive vehicle path planning system based on multi-objective optimization in an embodiment of the present application;

[0024] Figure 3 is a process of uneven terrain model construction and information extraction in an embodiment of the present application;

[0025] Figure 4 is a schematic diagram of a front-end energy-saving path coarse search algorithm Eco-KinoA* in an embodiment of the present application;

[0026] Figure 5 is a schematic diagram of estimating vehicle attitude on uneven terrain in an embodiment of the present application;

[0027] Figure 6 is a schematic diagram of a coarse trajectory output by the Eco-KinoA* algorithm in an embodiment of the present application;

[0028] Figure 7 is a schematic diagram of the final trajectory output after trajectory optimization in an embodiment of the present application;

[0029] Figure 8 is a schematic diagram of a controller in an embodiment of the present application. DETAILED DESCRIPTION

[0030] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some of the embodiments of the present application, but not all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of the present application.

[0031] In an embodiment, as shown in Figure 1 a multi-objective optimization-based uneven terrain adaptive vehicle path planning method is provided, including the following steps S1-S5:

[0032] S1, obtaining point cloud data of a predetermined work area of a vehicle, preprocessing the point cloud data, and establishing a three-dimensional point cloud map of the predetermined work area.

[0033] In an embodiment, the step S1 includes the following steps S101-S104:

[0034] S101, scanning the surrounding environment of the predetermined work area by the vehicle-mounted laser radar sensor to obtain the original point cloud data of the predetermined work area. Understandably, the vehicle can scan the surrounding environment at a frequency of 10 Hz by the mounted laser radar sensor (such as a 16-line laser radar with a model of Velodyne VLP-16) to obtain the original point cloud data of the predetermined work area.

[0035] S102, synchronously collecting the attitude information of the vehicle itself, correcting the original point cloud data by time stamp synchronization and the attitude information of the vehicle itself, and converting the corrected point cloud data to a unified world coordinate system. Understandably, the original point cloud data is first corrected by time stamp synchronization and the attitude information of the vehicle itself (from the inertial measurement unit IMU and GPS / RTK) for motion distortion correction, and the corrected point cloud data is converted to a unified world coordinate system.

[0036] S103, preprocessing the corrected point cloud data in the unified world coordinate system: voxel downsampling filtering to reduce data redundancy, and removing noise points by statistical outlier removal filtering. Understandably, voxel downsampling filtering (Voxel Grid Filter, for example, leaf slot size of 0.1 meters) is used to reduce data redundancy, and statistical outlier removal filtering (Statistical Outlier Removal) is used to remove noise points.

[0037] S104, the pre-processed point cloud data is subjected to continuous frame point cloud registration and fusion to generate a three-dimensional point cloud map covering the predetermined work area. Understandably, if a large-scale map needs to be constructed, the SLAM (Simultaneous Localization and Mapping) technology based on ICP (Iterative Closest Point) or NDT (Normal Distributions Transform) can be used for point cloud registration and map construction; finally, a three-dimensional point cloud map covering the predetermined work area of the vehicle is obtained, which serves as the basis for subsequent terrain analysis.

[0038] S2, a terrain data base structure is constructed based on the pre-processed point cloud data, and a terrain attribute model is constructed, the terrain attribute model is used to query the terrain attribute at any continuous point SE(2) state sequence and analyze the gradient, the gradient is combined with the vehicle dynamics to construct an uneven terrain model. The step aims to establish an uneven terrain model that can efficiently query the terrain attribute and its gradient. The algorithm flow is as shown in Figure 3

[0039] In an embodiment, the step S2 includes the following steps S201-S203:

[0040] S201, constructing a terrain data base structure: the pre-processed point cloud data is subjected to rasterization processing, and an array map_buffer is used as the core data structure, the array takes a discretized space unit as the basic unit, and each element of the array corresponds to a discrete SE(2) state unit (x, y, θ) in the space unit; wherein the SE(2) state unit is obtained through the discretization of the horizontal and vertical coordinates x and y and the yaw angle θ, each SE(2) state unit stores an RXS2 structure, and the RXS2 structure includes the following core parameters:

[0041] z, representing the estimated terrain height of the vehicle reference point under the SE(2) state unit;

[0042] zb, a two-dimensional vector used to represent the inclination direction and slope characteristics of the terrain surface, storing the first two components [n terrain ,n x ,n y ,n z ] T of the normalized local equivalent shape normal vector n x ,n y ,n T ; the third component n z is calculated by the formula

[0043] ​​Sigma, representing a terrain roughness index;

[0044] Flag, representing a passable sign.

[0045] S202, constructing a terrain attribute model based on a terrain data infrastructure.

[0046] In an embodiment, for each SE(2) unit [x q ,y q ,θ q ] T , the step S202 includes steps S2021-S2026:

[0047] S2021, obtaining a geometric model containing the wheelbase L and track W of the vehicle, and calculating the projection position set p SE2 of the four wheels of the vehicle on the horizontal plane based on the current SE(2) state S w , p w contains the left front wheel position p fl , the right front wheel position p fr , the left rear wheel position p rl , and the right rear wheel position p rr .

[0048] S2022, for the projection position p w of the four wheels, respectively centering each wheel position to draw a three-dimensional spherical region with a radius r, wherein the radius r can be the wheel radius r wheel ; extracting all four wheel point cloud data in the region from the three-dimensional point cloud map to form all wheel local point cloud

[0049] S2023, using RANSAC or least squares method or statistical analysis, including, for example, calculating point cloud centroid and principal direction, to perform plane fitting on the local point cloud at each wheel to obtain the estimated contact point height z w and the local equivalent shape normal vector n w at each wheel; the estimated contact point height z w is taken as the new vehicle body reference point height z ref , and the local equivalent shape normal vector n w is taken as the new Z-axis direction n terrain of the vehicle body.

[0050] S2024, obtaining the new vehicle's own attitude information at the next time, iterating according to steps S2021-S2023 until the vehicle attitude and height converge to obtain the stable vehicle body reference height z and the vehicle body Z-axis direction n terrain , and storing as zb.

[0051] S2025、through the wheel local point cloud The terrain roughness Sigma, denoted as σ, is calculated with respect to the root mean square error (RMSE) of the fitted plane or the standard deviation of the point cloud height;

[0052] S2026、determine the vehicle body Z-axis direction vector zb and the vertical direction vector e z =(0,0,1), through the formula c=zb·e z The cosine value c between the two vectors is calculated, which is the vehicle body inclination angle cosine value;

[0053] If c min or the terrain roughness σ>σ max , it is determined that the vehicle reference point under the SE(2) state unit is not suitable for vehicle passing, and the passing flag is set to not pass, i.e. Flag=1;

[0054] If c≥c min or the terrain roughness σ≤σ max , it is determined that the vehicle reference point under the SE(2) state unit is suitable for vehicle passing, and the passing flag is set to pass; i.e. Flag=0;

[0055] Wherein, c min represents the preset minimum value of the vehicle body inclination angle cosine value, and σ max represents the preset maximum value of the terrain roughness.

[0056] S203、based on the terrain attribute model, query the terrain attribute at any continuous point SE(2) state sequence and analyze the gradient, and calculate the vehicle dynamics.

[0057] In an embodiment, when it is necessary to query the terrain information at any continuous SE(2) state S SE2 =[x q ,y q ,θ q ] T , the step S203 includes the following steps S2031-S2035:

[0058] S2031、map the arbitrary continuous point SE(2) state sequence S SE2 =[x q ,y q ,θ q ] T to the index of the closest 8 discrete units in the map_buffer to complete the terrain information mapping.

[0059] S2032. Perform trilinear interpolation on the RXS2 data (z, zb.x, zb.y, σ) stored in the RXS2 structure corresponding to these 8 discrete units to obtain the current SE(2) state, denoted as S. SE2 The continuous terrain attribute values ​​at point n, where zb.x and zb.y are the local equivalent shape normal vectors n, respectively. terrain =[n x ,n y ,n z ] T The first two components n x ,n y .

[0060] S2033, Gradient Calculation: Calculate the gradient with respect to the query state S using the trilinear interpolation formula. SE2 =[x q ,y q ,θ q ] T The partial derivatives are used to obtain the interpolated terrain attributes (such as z, zb.x, zb.y, σ) relative to x. q ,y q ,θ q The analytical gradient.

[0061] S2034. Define key angles for uneven terrain, including the longitudinal slope angle φ. x (t), Lateral slope angle φ y (t) and attitude tilt angle ξ(t), when the terrain is horizontal, φ x =φ y =ξ=0.

[0062] In one embodiment, the derivation of the formulas for the motion variables of a vehicle on uneven terrain in step S2034 is as follows:

[0063]

[0064] δ=arctan(L w ·κ),

[0065] in

[0066]

[0067] Among them, zb.x, zb.y and vehicle yaw angle θ are obtained by interpolation in step S2032. q Calculate the key terrain angles related to the above vehicle dynamics, assuming:

[0068] b x ,b y ,b z ∈R 3

[0069] are the front, lateral and vertical unit vectors of the vehicle body coordinate system, respectively.

[0070]

[0071] is the vertical upward unit vector of the world coordinate system, and the horizontal front and lateral unit vectors of the vehicle at the yaw angle θ(t) are denoted as:

[0072]

[0073] where the longitudinal slope angle φ x (t) represents the angle between the instantaneous forward direction of the vehicle (determined by θ(t) and the terrain normal n terrain (t) together) and the horizontal plane;

[0074] the lateral slope angle φ y (t) represents the angle between the lateral direction of the vehicle and the horizontal plane;

[0075] the attitude tilt angle ξ(t) is the angle between the vehicle body b z axis and the direction of gravity e3;

[0076] When the terrain is horizontal b z = e3, we have:

[0077] φ x = φ y = ξ = 0;

[0078] The above formula directly gives the sine and cosine using the vector inner product, which is numerically stable and facilitates subsequent derivation. Then, according to the chain rule, the gradients of other physical quantities can be calculated.

[0079] S2035, determine the attitude and slope of the vehicle relative to the terrain, and derive the relationship between the vehicle dynamics and its trajectory parameters and the key angles of the uneven terrain.

[0080] In an embodiment, in step S2035, in order to accurately calculate the dynamics of the vehicle on the uneven terrain, the attitude and slope of the vehicle relative to the terrain need to be determined. These parameters are determined by the two-dimensional position trajectory p(t) = [x(t), y(t)] T of the vehicle parameterized by step S401, the yaw angle trajectory θ(t), and the local terrain normal vector n terrain (t) obtained from the terrain model, where the following key terrain influence angles and related quantities can be derived as follows:

[0081] the actual longitudinal velocity v x (t), v x (t) = v(t) / cosφ x (t), where |a

[0082] actual longitudinal acceleration a x (t), a x (t) = a t (t) / cosφ x (t) + gsinφ x (t), where actual tangential acceleration a

[0083] actual lateral acceleration a y (t), a y (t) = a n (t) / cosφ y (t) + gsinφ y (t), where actual normal acceleration a

[0084] actual yaw rate ω z (t), ω z (t) = ω(t) / cosξ(t), where actual projected yaw rate ω

[0085] actual path curvature K(t), K(t) = ω z (t) / v x (t).

[0086] equivalent front wheel steering angle δ, based on the bicycle model δ = arctan(L w · K), where the vehicle wheelbase L w .

[0087] The above vehicle dynamics quantities and their relationships with the trajectory parameters (p(t), θ(t)) and the terrain parameters (φ x , φ y , ξ) form the basis for the subsequent gradient computation; through the chain rule, the gradients of the cost function and constraints with respect to these actual dynamics quantities can be back-propagated to the gradients with respect to the optimization variables (i.e., the polynomial coefficients of the trajectory and the time allocation).

[0088] S3, based on the uneven terrain model, a preliminary two-dimensional rough trajectory is generated using the Eco-Kino A* path search algorithm. This step corresponds to the front-end energy-saving path rough search unit, which uses the uneven terrain model above, an improved A* search algorithm based on kinematic constraints is used to search from the starting state s start to the target point sgoal , generate a preliminary two-dimensional rough trajectory The core of the algorithm is that the consideration of energy consumption is integrated into the design of the cost function and heuristic function, and the algorithm flow is as shown in the algorithm Figure 4 An Eco-KinoA* path search algorithm is proposed, which generates a preliminary two-dimensional rough trajectory considering the kinematic constraints of the vehicle and energy consumption on uneven terrain model.

[0089] In an embodiment, the step S3 includes steps S301-S305:

[0090] S301, initialize the Eco-KinoA* search framework based on the uneven terrain model, maintain an open list for storing nodes to be expanded and a closed list for storing expanded nodes Define the state node n as [x n ,y n ,θ n ] T ; at the current state node, use the discrete control input u k (including a set of predefined linear velocity v and front wheel steering angle δ combinations), and perform state transition based on the bicycle kinematic model to generate the successor state S next .

[0091]

[0092] Wherein, the nodes are sorted in according to their total evaluation value f(n) = g(n) + h(n).

[0093] S302, calculate the cost function g(n), the cost function g(n) represents the cumulative actual cost from the starting state s start to the current search state node n, and its expression is:

[0094] g(n) = l(n) + e(n)

[0095] Wherein, l(n) represents the path length from the starting state to the current node n, e(n) represents the energy consumption estimate generated by driving from the parent node n parent to the current node n, which is calculated based on the displacement ΔL of the vehicle from the parent node state to the current node state, the terrain-induced elevation change Δz(mainly considering the climbing consumption), and the average terrain roughness σ avg on the path segment. The specific estimate uses the following formula:

[0096] ΔE = (C rr ·m·g·ΔL + m·g·max(0, Δz) + Crough • σ avg • ΔL) / η d

[0097] where C rr is the rolling resistance coefficient, m is the vehicle mass, g is the gravitational acceleration, C rough is the terrain roughness equivalent resistance coefficient, η d is the rolling efficiency; the energy consumption estimate e(n) is denoted as ΔE.

[0098] S303, a heuristic function h(n) is calculated, which is used to estimate the future cost from the search state node n to the target point s goal , and its expression is:

[0099] h(n) = w hd · d Dubins (n, s goal ) + w he · E Dubins (n, s goal )

[0100] where d Dubins (n, s goal ) represents the path length of the Dubins curve connecting the current node state s n and the target point state s goal ; E Dubins (n, s goal ) represents the estimated driving energy consumption along the Dubins curve, which is obtained by uniformly sampling the Dubins path to obtain N s discrete points, thereby dividing the path into (N s -1) small segments. For each small segment i, its energy consumption ΔE i is estimated according to the length ΔL i of the segment, the elevation change Δz i (mainly considering the energy consumption of climbing a slope, i.e., m·g·max(0, Δz i )), and the vehicle rolling resistance, etc., and is calculated using the formula ΔE i = (C rr · m·g· ΔL i + m·g·max(0, Δz i )) / η d . The total Dubins path energy estimate E Dubins (n, s goal ) is the sum of all small segment energy consumptions ∑ΔE i .

[0101] w hdFor the corresponding weight coefficients inspired by geometric distance, w he The corresponding weighting coefficients are inspired by energy consumption.

[0102] S304, Node Expansion and Evaluation: From the Open List Select the node n with the smallest value of f(n) = g(n) + h(n). current Extend this process to each allowed control input u k The generated successor state s next .

[0103] In one embodiment, step S304 includes the following steps S3041-S3043:

[0104] S3041, Check the subsequent status s next Is it already on the closed list? In, or from n curreat to s next Whether the path segment collides with obstacles or does not meet the requirements based on the uneven terrain model. The traversability constraints (such as maximum slope, maximum roughness) are applied; if the conditions are not met, the subsequent state is skipped.

[0105] S3042, Calculate from n current to s next The actual motion cost Δg k The actual cost of movement includes path length, turning cost, and energy consumption ΔE. k .

[0106] S3043, Update or add s next Corresponding nodes to the open list In the middle, record its g value, f value and parent node.

[0107] S305, The extended node state is located at the target point s. goal When the target point is within the range of the target point or its neighborhood, a preliminary two-dimensional coarse trajectory from the initial state to the target point is reconstructed by backtracking the parent node. like Figures 6 to 7 As shown.

[0108] S4. Employ a backend trajectory optimization algorithm, using piecewise polynomials to represent the trajectory, to obtain a preliminary two-dimensional coarse trajectory. As a starting point, a multi-objective optimization function is established to cover the trajectory smoothness cost, the curvature cost, the energy cost and the vehicle pose stability cost, and combined with the vehicle dynamics constraints, the nonholonomic constraints and the terrain constraints, the optimized two-dimensional trajectory is obtained. The step S3 generates a two-dimensional rough trajectory point sequence as an initial value, and a kind of optimization method based on optimization, that is, an optimizer based on augmented Lagrange multiplier method (ALM) is used to refine the trajectory to meet multiple objectives such as path smoothness, path geometric flatness, driving energy efficiency and vehicle pose stability, and strictly comply with various dynamics and environmental constraints.

[0109] In an embodiment, the step S4 comprises the following steps S401-S404:

[0110] S401, using the preliminary two-dimensional rough trajectory point sequence as an initial value, a piecewise quintic polynomial is used to parameterize the positions p(t) = [x(t), y(t)] of the points of the two-dimensional position trajectory of the vehicle T and the yaw angle trajectory θ(t), the position trajectory is divided into M segments, and the polynomial of each segment i is An optimizer based on augmented Lagrange multiplier method (ALM) is used to refine the trajectory, and the optimization variables are the positions, velocities, accelerations of the intermediate points and the time length T of each segment i .

[0111] In an embodiment, the step S401 comprises the following steps S4011-S4012:

[0112] S4011, using a piecewise quintic polynomial to parameterize the positions p(t) = [x(t), y(t)] of the points of the two-dimensional position trajectory of the vehicle T and the yaw angle trajectory θ(t), the position trajectory is divided into M segments, and the yaw angle trajectory θ(t) is divided into Ω segments, wherein M≤Ω;

[0113] For the i-th segment (i = 1, …, M) of the position trajectory, its expression in the local time t ∈ [0, T xy,i ] is:

[0114]

[0115] For the k-th segment (k = 1, …, Ω) of the yaw angle trajectory, its expression in the local time t ∈ [0, T θ,k ] is:

[0116]

[0117] wherein γ(t) = [1, t, t 2 ,t 3 ,t 4 ,t5 ] T Representing the polynomial basis vectors of time; c x,i ,c y,i ,c θ,k ∈R 6 T represents the polynomial coefficient vectors of the x-component and y-component of the i-th position trajectory and the k-th yaw angle trajectory, respectively; xy,i T represents the duration of the i-th position trajectory; θ,k This represents the duration of the k-th segment of the yaw angle trajectory.

[0118] S4012. To facilitate optimization, all position-related coefficient vectors are organized into a large coefficient matrix. Or flatten it into vector form ∈R 12M All yaw angle-related coefficient vectors are organized into The time of all position segments constitutes a vector T. xy =[T xy,1 ,…,T xy,M ] T ∈R M The time of all yaw angle segments constitutes a vector T. θ =[T θ,1 ,…,T θ,Ω ] T ∈R Ω .

[0119] In another embodiment, step S4012 has a preferred implementation where the optimization variable is not directly the polynomial coefficient c. xy ,c θ Instead, it refers to the position, velocity, acceleration, and other state values ​​of the trajectory at the connection points (i.e., waypoints) of each segment, as well as the time allocation T of each segment. xy ,T θ (or its mapping, such as a logarithmic mapping, to ensure positive time); the polynomial coefficients c can be uniquely determined by these waypoint states and times. The optimization variable c... xy ,c θ Convert to segment point position q xy ∈R 2(M-1) and q θ ∈R Ω-1 This is also an effective way to reduce the degrees of freedom of optimization variables.

[0120] S402, The set of optimized variables can be represented as X = {c} xy ,c θ ,T xy ,T θ} or its equivalent form, construct the comprehensive cost function J total It is a weighted sum of multiple sub-cost terms, and the comprehensive cost function J is... totalThe expression is:

[0121] J total (X)=w jerk J jerk +w curv J curvature +w energy J energy +w att J attitude +w T J T ;

[0122] wherein J jerk represents a trajectory smoothness cost This sub-cost determines the smoothness and high-order continuity of the trajectory by penalizing the Pth-order derivative of the position, i.e., the acceleration jerk.

[0123] J curvature represents a curvature cost, This sub-cost item reduces sharp turns by penalizing the squared curvature κ(t) of the trajectory, aiming to generate a more geometrically efficient path.

[0124] J energy represents an energy cost, This sub-cost item establishes a power metric model related to gravity to encourage energy-saving motion of the vehicle on variable terrain; v x (t) is the vehicle forward speed after pitch attitude correction.

[0125] J attitude represents a vehicle attitude stability cost, This sub-cost item maintains the upright attitude of the vehicle by penalizing the inclination deviation of the vehicle body, and suppresses excessive roll or pitch, and the cosine of the angle between the body attitude normal vector and the direction of gravity is used to measure the upright degree of the attitude.

[0126] J T represents a traversal time cost, J T =T s This sub-cost item is the total time spent to complete the entire trajectory.

[0127] w jerk ,w curv ,w energy ,w att ,w T are weight coefficients corresponding to the cost items.

[0128] S403, the back-end trajectory optimization algorithm satisfies the vehicle dynamics constraints, non-holonomic constraints and terrain constraints, and optimizes the preliminary two-dimensional rough trajectory.

[0129] In an embodiment, the step S403 comprises a step S4031 of:

[0130] S4031, the vehicle dynamics constraints comprise boundary constraints and continuity constraints:

[0131] M xy c xy = b xy

[0132] M θ c θ = b θ

[0133] where c xy represents a total position coefficient matrix or vector composed of polynomial coefficients of all position trajectory segments (x i (t), y i (t)) arranged in a certain order; for example, if there are 6 coefficients for each segment and M segments in total, c xy may be a 6M x 2 matrix or a 12M x 1 vector.

[0134] c θ represents a total yaw angle coefficient composed of polynomial coefficients of all yaw angle trajectory segments θ k (t) arranged in a certain order, which is a 6Ω x 1 vector.

[0135] M xy and M θ are large sparse constraint matrices constructed according to the evaluation of polynomial basis functions and their derivatives at the endpoints (t = 0 and t = T i ) of each segment, which associate polynomial coefficients with the states of the trajectory at the boundary points and the connection points, including position, velocity, and acceleration.

[0136] b xy and b θ represent constraint value vectors, which include the given states of the starting and ending points of the trajectory, and the required state values of the continuity conditions at the connection points of each segment.

[0137] The vehicle dynamics constraints also include time positivity constraints:

[0138] T xy ≥ 0 (i.e., T xy,i ≥ 0 for all i = 1, …, M)

[0139] T θ ≥ 0 (i.e., T θ,k ≥ 0 for all k = 1, …, Ω)

[0140] where T xy represents a vector containing the durations of all M position trajectory segments.

[0141] T θ denotes a vector containing all the Ω yaw angle trajectory segment duration time, to ensure that the duration of each segment of the trajectory is non-negative; in the actual optimization, to avoid directly dealing with inequality constraints, the method of variable substitution is usually used, such as letting Then the unconstrained τ is optimized.

[0142] The non-holonomic constraint is:

[0143]

[0144] This non-holonomic constraint is used to ensure that the instantaneous speed direction of the vehicle is consistent with the body orientation;

[0145] Terrain constraint: this constraint is satisfied at each discrete sampling point t of the trajectory s , or continuously along the trajectory, both of which must be true:

[0146]

[0147] where the quantities with the actual superscript represent the actual vehicle dynamics quantities considering the terrain properties such as slope angle φ x (t s ), φ y (t s ), and attitude angle ξ(t s ) influence in step S2, including vehicle speed longitudinal acceleration lateral acceleration and curvature where δ + is a small positive value to avoid 0 in the denominator of the curvature calculation;

[0148] S404, in the optimization solution, when optimizing the preliminary two-dimensional rough trajectory, the augmented Lagrange multiplier method (ALM) is used to handle the constraint conditions, and the problem is converted into a series of sub-problems without constraints or simple boundary constraints. Each sub-problem is solved by a gradient optimization algorithm (such as L-BFGS), and the gradients of all cost terms and constraint terms are calculated by analytical derivation or numerical method, to obtain the optimized two-dimensional trajectory x(t), y(t), θ(t).

[0149] Understandably, the rear-end trajectory optimization algorithm is used to represent the trajectory with segmented polynomials, the preliminary two-dimensional rough trajectory is taken as the initial value, a multi-objective optimization function is constructed including trajectory smoothness cost, trajectory flatness cost, driving energy saving cost, and vehicle attitude stability cost, and the two-dimensional trajectory is optimized by combining vehicle dynamics constraints, non-holonomic constraints, and terrain-related constraints, Figure 7 is a schematic diagram of the final trajectory output after trajectory optimization.

[0150] In another embodiment, the construction of the comprehensive cost function J total , namely a multi-objective optimization function, in which the trajectory jerk square integral term is used to improve the trajectory smoothness; the trajectory curvature square integral term is used to improve the trajectory straightness and reduce unnecessary bending; the energy saving index term based on the vehicle driving slope and longitudinal speed is used to guide the vehicle to adopt energy-saving driving strategy; the posture stability index term based on the angle cosine between the vehicle posture and the gravity direction is used to suppress excessive vehicle body inclination.

[0151] The vehicle dynamics constraints in the rear-end trajectory optimization algorithm include maximum speed, maximum longitudinal acceleration, maximum lateral acceleration and maximum curvature, and the calculation of each of them is modified in combination with the terrain information obtained in step S2 (such as the influence of terrain slope on acceleration, the influence of terrain normal vector on vehicle angular velocity). The rear-end trajectory optimization algorithm uses the augmented Lagrange multiplier method (ALM) to handle constraints, and uses gradient optimization methods such as L-BFGS to solve the sub-problems at each step.

[0152] S5, generating a final three-dimensional vehicle trajectory according to the optimized two-dimensional trajectory and the uneven terrain model.

[0153] In an embodiment, the step S501 includes the following steps S501-S503:

[0154] S501, sampling at a dense time interval based on the optimized two-dimensional trajectory x(t), y(t), θ(t);

[0155] S502, for each sampling point (x s ,y s ,θ s ), calling the terrain attribute model interface of step S2 to input the SE(2) state unit to obtain the corresponding three-dimensional position P s =[x s ,y s ,z s ] T and the rotation matrix R s of the vehicle body at the point, which is constructed by the terrain normal vector z θ and the yaw angle θ s ;

[0156] S503, outputting the sequence (P s , R s ) as the final three-dimensional vehicle trajectory.

[0157] It should be understood that the size of the serial number of each step in the above embodiments does not mean the order of execution, and the execution order of each process should be determined according to its function and inherent logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0158] In an embodiment, a multi-objective optimization based uneven terrain adaptive vehicle path planning system is provided, which corresponds to the multi-objective optimization based uneven terrain adaptive vehicle path planning method in the above embodiment. As shown in the figure, the multi-objective optimization based uneven terrain adaptive vehicle path planning system comprises: Figure 2

[0159] The data acquisition and point cloud map construction module 100 is configured to acquire point cloud data of a predetermined work area of a vehicle, preprocess the point cloud data, and establish a three-dimensional point cloud map of the predetermined work area;

[0160] The terrain attribute modeling and dynamics calculation module 200 is configured to construct a terrain data infrastructure based on the preprocessed point cloud data, and then construct a terrain attribute model, query terrain attributes at any continuous point SE(2) state sequence and analyze gradients using the terrain attribute model, calculate vehicle dynamics in combination with the gradients, and construct an uneven terrain model;

[0161] The rough trajectory generation module 300 is configured to generate a preliminary two-dimensional rough trajectory based on the uneven terrain model using an Eco-Kino A* path search algorithm;

[0162] The multi-objective trajectory optimization and constraint fusion module 400 is configured to use a backend trajectory optimization algorithm, represent the trajectory using a segmented polynomial, take the preliminary two-dimensional rough trajectory as a starting point, establish a multi-objective optimization function covering trajectory smoothness cost, curvature cost, energy cost and vehicle attitude stability cost, and obtain an optimized two-dimensional trajectory in combination with vehicle dynamics constraints, non-holonomic constraints and terrain constraints;

[0163] The three-dimensional trajectory generation and output module 500 is configured to generate a final three-dimensional vehicle trajectory based on the optimized two-dimensional trajectory and the uneven terrain model.

[0164] ​The specific definitions of the uneven terrain adaptive vehicle path planning system based on multi-objective optimization can refer to the definitions of the uneven terrain adaptive vehicle path planning method based on multi-objective optimization in the above, which will not be repeated here. Each module in the above uneven terrain adaptive vehicle path planning system based on multi-objective optimization can be realized by software, hardware and their combinations in whole or in part. The above modules can be embedded in or independent of the processor in the computer device in hardware form, or can be stored in the memory in the computer device in software form, so as to be called and executed by the processor to perform the operations corresponding to each module.

[0165] In one embodiment, a controller is provided, including a memory, a processor, and computer readable instructions stored on the memory and executable on the processor, wherein the processor implements the method for uneven terrain adaptive vehicle path planning based on multi-objective optimization when executing the computer readable instructions. Further, as shown in the figure, the controller includes a processor, a memory, a network interface and a database connected by a system bus. Among them, the processor of the controller is used to provide computing and control capabilities. The memory includes a readable storage medium and an internal memory. The readable storage medium stores an operating system, computer readable instructions and a database. The internal memory provides an environment for the operation of the operating system and computer readable instructions in the readable storage medium. The computer readable instructions are executed by the processor to implement a method for uneven terrain adaptive vehicle path planning based on multi-objective optimization. The readable storage medium provided in the embodiment includes non-volatile readable storage medium and volatile readable storage medium. Figure 8

[0166] In an embodiment, a computer readable storage medium is provided, which stores computer readable instructions, wherein the computer readable instructions are executed by a processor to implement the method for uneven terrain adaptive vehicle path planning based on multi-objective optimization in the above embodiment, and details are not repeated here to avoid repetition. The computer readable storage medium can be non-volatile or volatile.

[0167] ​Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer readable storage medium. When the computer program is executed, it can include the processes of the above-mentioned embodiments of each method. Any reference to memory, storage, database or other medium used in each embodiment provided by the present application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration but not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0168] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the above-mentioned division of functional units and modules is exemplified, and in actual application, the above-mentioned functions can be completed by different functional units and modules according to needs, that is, the internal structure of the system is divided into different functional units or modules to complete all or part of the above-described functions.

[0169] The above-mentioned embodiments are only used to illustrate the technical solutions of the present application, but not to limit it; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that the technical solutions recorded in the foregoing embodiments can be modified, or some technical features can be replaced by equivalents; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should be included in the protection scope of the present application.

Claims

1. A multi-objective optimization based uneven terrain adaptive vehicle path planning method, characterized by, The method comprises the following steps: S1, acquiring point cloud data of a predetermined working area of a vehicle, preprocessing the point cloud data, and establishing a three-dimensional point cloud map of the predetermined working area; S2, constructing a terrain data base structure based on the preprocessed point cloud data, and then constructing a terrain attribute model, querying terrain attributes at any continuous point SE(2) state sequence and analyzing gradients using the terrain attribute model, calculating vehicle dynamics based on the gradients, and constructing an uneven terrain model; S3, based on the uneven terrain model, using an Eco-Kino A* path search algorithm to generate a preliminary two-dimensional rough trajectory; S4, using a backend trajectory optimization algorithm, using a segmented polynomial to represent the trajectory, taking the preliminary two-dimensional rough trajectory as the starting point, establishing a multi-objective optimization function covering trajectory smoothness cost, curvature cost, energy cost, and vehicle attitude stability cost, and combining vehicle dynamics constraints, non-holonomic constraints, and terrain constraints to obtain an optimized two-dimensional trajectory; S5, generating a final three-dimensional vehicle trajectory based on the optimized two-dimensional trajectory and the uneven terrain model.

2. The multi-objective optimization based adaptive vehicle path planning method for uneven terrain according to claim 1, wherein, The step S1 further comprises the following sub-steps: S101, scanning the surrounding environment of the predetermined working area by the vehicle-mounted laser radar sensor to acquire original point cloud data of the predetermined working area; S102, synchronously collecting the attitude information of the vehicle itself, correcting the original point cloud data for motion distortion through timestamp synchronization with the attitude information of the vehicle itself, and converting the corrected point cloud data to a unified world coordinate system; S103, preprocessing the corrected point cloud data in the unified world coordinate system: voxel downsampling filtering to reduce data redundancy, and removing noise points through statistical outlier removal filtering; S104, registering and fusing continuous frame point clouds based on the preprocessed point cloud data to generate a three-dimensional point cloud map covering the predetermined working area.

3. The multi-objective optimization based adaptive vehicle path planning method for uneven terrain according to claim 2, wherein, The step S2 further comprises the following sub-steps: S201, constructing a terrain data base structure: performing rasterization processing on the preprocessed point cloud data, using an array map_buffer as the core data structure, the array taking discretized spatial units as the basic unit, and each element of the array corresponding to a discrete SE(2) state unit in the spatial unit; wherein the SE(2) state unit is obtained through discretization processing of the horizontal and vertical coordinates and the yaw angle, and each SE(2) state unit stores an RXS2 structure body including the following core parameters: z, representing the estimated terrain height of the vehicle reference point under the SE(2) state unit; zb, a two-dimensional vector used to characterize the orientation and slope of the terrain surface, stores the normalized local equivalent normal vector n terrain = [n x ,n y ,n z ] T The first two components [n x ,n y ] T ; the third component n z is calculated by the formula ; S202, constructing a terrain attribute model based on the terrain data base structure; S203, querying terrain attributes at any continuous point SE(2) state sequence and analyzing gradients based on the terrain attribute model, and calculating vehicle dynamics to construct an uneven terrain model.

4. The multi-objective optimization based adaptive vehicle path planning method for uneven terrain according to claim 3, wherein, The step S202 further comprises the following sub-steps: S2021、get the geometric model containing the wheelbase L and the track W of the vehicle, based on the current SE(2) state denoted as S SE2 , calculate the projection position set p of the four wheels of the vehicle on the horizontal plane w , p w contains the left front wheel position p fl , the right front wheel position p fr , the left rear wheel position p rl , and the right rear wheel position p rr ; S2022, the projection positions p of the four wheels w , respectively, a three-dimensional spherical region is defined with a radius r centered on each wheel position, wherein the radius r can be the wheel radius r wheel ; all four wheel point cloud data in the region is extracted from the three-dimensional point cloud map to form all wheel local point cloud S2023, using RANSAC or least squares or statistical analysis, including computing point cloud centroid and principal direction, on all local point clouds at the wheels performing a plane fit to obtain an estimated contact point height z at each wheel w and a local equivalent normal vector n w ; the estimated contact point height z w as a new vehicle body reference point height z ref , the local equivalent normal vector n w as a new Z-axis direction n of the vehicle body terrain ; S2024, obtain the new vehicle's own attitude information at the next moment, iterate according to steps S2021-S2023 until the vehicle attitude and height converge, and obtain the stable vehicle body reference height z and the vehicle body Z-axis direction n terrain and store as zb; S2025, by the wheel local point cloud The terrain roughness index Sigma, denoted as σ, is calculated with respect to the root mean square error of the fitted plane or the standard deviation of the point cloud height. S2026、Determine the vehicle body Z-axis direction vector zb and the vertical direction vector e z = (0, 0, 1), through the formula c = zb·e z Calculate the included angle cosine value c between the two vectors, that is, the body tilt angle cosine value; if c < c min or the terrain roughness σ > σ max , then determine that the vehicle reference point under the SE(2) state unit is not suitable for vehicle passing, set the passing flag Flag as not passable, that is, Flag = 1; if c ≥ c min or the terrain roughness σ ≤ σ max then determine that the vehicle reference point under the SE(2) state unit is suitable for vehicle passing, set the passing sign Flag as passable; that is, Flag = 0; wherein c min represents a preset minimum value of the cosine of the body inclination angle, σ max represents a preset maximum value of the terrain roughness.

5. The multi-objective optimization based adaptive vehicle path planning method for uneven terrain according to claim 3, wherein, The step S203 further comprises the following sub-steps: S2031, mapping an arbitrary sequence of SE(2) states S = [x SE2 ,y q ,θ q ,θ q ] T to the indices of the closest 8 discrete cells in the array map_buffer to complete the terrain information mapping; S2032, trilinear interpolation is performed on the RXS2 data (z, zb.x, zb.y, σ) stored in the RXS2 structure corresponding to the 8 discrete units to obtain the current SE(2) state, denoted as S SE2 , the continuous terrain attribute value at the position, wherein zb.x and zb.y are respectively the first two components n terrain , x , y , z of the local equivalent normal vector n T . x , y ​ S2033, take the partial derivative of the trilinear interpolation formula with respect to x q ,y q ,θ q , to obtain the analytical gradient of the interpolated terrain attribute with respect to the query state S SE2 ; S2034, defining uneven terrain key angles, including longitudinal slope angle φ x (t), lateral slope angle φ y (t) and attitude tilt angle ξ(t), when terrain is level, φ x = φ y = ξ = 0; S2035, determining the attitude and slope of the vehicle relative to the terrain, and deriving the relationship between the vehicle dynamics and its trajectory parameters and the key angles of the uneven terrain.

6. The multi-objective optimization based adaptive vehicle path planning method for uneven terrain according to claim 5, wherein, The step S3 further comprises the following sub-steps: S301、Based on the uneven terrain model, initialize the Eco-Kino A* search framework, maintain an open list for storing nodes to be expanded and a closed list for storing expanded nodes Define the state node n as [x n , y n , θ n ] T ; S302、compute the cost function g(n), the cost function g(n) represents the cumulative actual cost from the starting state s start to the current search state node n, its expression is: g(n) = l(n) + e(n) wherein I(n) represents the path length from the start state to the current node n, e(n) represents the energy consumption estimate resulting from driving from the parent node n parent to the current node n, which energy consumption estimate e(n) is denoted ΔE; S303、Calculate the heuristic function h(n), which is used to estimate the future cost from the search state node n to the target point s goal The expression is: h(n) = w hd • d Dubins (n, s goal ) + w he • E Dubins (n, s goal ) where d Dubins (n, s goal ) represents the path length of the Dubins curve connecting the current node state s n and the target point state s goal ; E Dubins (n, s goal ) represents the estimated energy consumption for driving along the Dubins curve; w hd is the corresponding weight coefficient inspired by the geometric distance, w he is the corresponding weight coefficient inspired by the energy consumption; S304, selecting the node n with the smallest value of f(n) = g(n) + h(n) from the open list S304, selecting the node n with the smallest value of f(n) = g(n) + h(n) from the open list current S304, selecting the node n with the smallest value of f(n) = g(n) + h(n) from the open list k S304, selecting the node n with the smallest value of f(n) = g(n) + h(n) from the open list next S304, selecting the node n with S3041, check successor state s next whether it has been in the closed list or whether the path segment from n curreat to s next collides with an obstacle, or does not satisfy the passability constraint based on the uneven terrain model; if not, skip this successor state; S3042, Calculate from n current to s next The actual motion cost Δg k The actual cost of movement includes path length, turning cost, and energy consumption ΔE. k ; S3043, update or add s next corresponding node-to-open list in the middle, and record its g value, f value and parent node; S305, when the expanded node state is located in the target point s goal or its neighborhood, reconstruct the preliminary two-dimensional rough trajectory from the start state to the target point by backtracking the parent nodes 7. The multi-objective optimization based adaptive vehicle path planning method for uneven terrain according to claim 6, wherein, The step S4 further comprises the following sub-steps: S401, taking the preliminary two-dimensional rough trajectory point sequence as the initial value, using piecewise quintic polynomial to parameterize the position p(t) = [x(t), y(t)] of the points of the two-dimensional position trajectory of the vehicle and the yaw angle trajectory θ(t), the position trajectory is divided into M segments, and the polynomial of each segment i is T The optimizer using the augmented Lagrange multiplier method ALM is used for fine processing of the trajectory, and the optimization variables are the positions, speeds, accelerations and time lengths T of the intermediate points i ;​ S402、constructing a comprehensive cost function J total which is a weighted sum of a plurality of sub-cost terms, the comprehensive cost function J total The expression of the comprehensive cost function J is: J total (X) = w jerk J jerk + w curv J curvature + w energy J energy + w att J attitude + w T J T ; where J jerk represents the trajectory smoothness cost This sub-cost determines the smoothness and high-order continuity of the trajectory by penalizing the P-derivative of the position, i.e., the acceleration jerk. J curvature represents a curvature cost, This sub-cost term reduces sharp turns by penalizing the squared curvature K(t) of the trajectory, aiming to generate a more geometrically efficient path; J energy represents an energy cost, This sub-cost term builds a power metric model related to gravity to encourage the vehicle to perform energy-efficient motion on variable terrain; x (t) is the vehicle forward speed after the pitch attitude correction; J attitude represents a vehicle pose stability cost, This sub-cost term keeps the upright pose of the vehicle by penalizing the deviation of the roll angle of the vehicle body, and suppresses excessive roll or pitch. It is the cosine of the angle between the body pose vector and the gravity direction, which is used to measure the upright degree of the pose. J T denotes the traversal time cost, J T = T s , which is the total time spent to complete the entire trajectory; w jerk , w curv , w energy , w att , w T is a weight coefficient for the corresponding cost term; S403, making the rear-end trajectory optimization algorithm satisfy the vehicle dynamics constraint, the non-holonomic constraint and the terrain constraint, and optimizing the preliminary two-dimensional rough trajectory; S4031, the vehicle dynamics constraint comprises the boundary constraint and the continuity constraint: M xy c xy = b xy M θ c θ = b θ where c xy represents the total position coefficient matrix or vector composed of the polynomial coefficients of all position trajectory segments (x i (t),y i (t)) arranged in a certain order; there are 6 coefficients for each segment, and there are M segments, so c xy may be a 6M x 2 matrix or a 12M x 1 vector; c θ represents all yaw angle trajectory segments θ k The total yaw angle coefficients are a 6Ω x 1 vector, where the polynomial coefficients of (t) are arranged in a specific order. M xy and M θ are determined from the polynomial basis functions and their derivatives at the end points of each segment, including the end points t = 0 and t = T i ; evaluate the large sparse constraint matrices built, which relate the polynomial coefficients to the states of the trajectory at the boundary points and at the junctions, including position, velocity, and acceleration; b xy and b θ , denotes the constraint vector, which contains the given states at the start and end of the trajectory, and the state values required by the continuity conditions at the connections of the segments; The vehicle dynamics constraint further comprises the time positive constraint: T xy ≥0T θ ≥0 where T xy denotes a vector containing all M position trajectory segment durations; T θ denotes a vector containing all the Ω yaw angle trajectory segment durations, to ensure that the duration of each segment of the trajectory is non-negative; in practical optimization, to avoid directly dealing with inequality constraints, a variable substitution method is usually adopted, i.e. i = e τi , and then re-optimizing τ without constraints. The non-holonomic constraint is: The non-holonomic constraint is used to ensure that the instantaneous speed direction of the vehicle is consistent with the body orientation; The terrain constraint is that this constraint is satisfied at each discrete sample point t of the trajectory s or continuously along the trajectory, both need to be true: where the quantities with actual superscript represent actual vehicle dynamics quantities taking into account the terrain properties in step S2, including the slope angle φ x (t s ) and φ y (t s ); the attitude angle ξ(t s ) affects the actual vehicle dynamics quantities, including the vehicle speed longitudinal acceleration lateral acceleration and curvature where δ + is a small positive value to avoid 0 in the denominator of the curvature calculation; S404, when optimizing the preliminary two-dimensional rough trajectory, the augmented Lagrange multiplier method ALM is used to process the constraint condition, the problem is converted into a series of sub-problems without constraint or simple boundary constraint, each sub-problem is solved through a gradient optimization algorithm, the gradients of all cost items and constraint items are calculated through analytical derivation or numerical method, and the optimized two-dimensional trajectory x(t), y(t), θ(t) is obtained.

8. The multi-objective optimization based adaptive vehicle path planning method for uneven terrain according to claim 7, wherein, The step S401 further comprises the following sub-steps: S4011, a piecewise quintic polynomial is employed to parameterize the positions p(t) = [x(t), y(t)] of points of a two-dimensional position trajectory of the vehicle T and a yaw angle trajectory θ(t), the position trajectory being divided into M segments and the yaw angle trajectory θ(t) being divided into Ω segments, where M < Ω; For the i-th segment of the position trajectory, its expression in local time t ∈ [0, T xy，i ] is: Wherein, i = 1, …, M; For the kth segment of the yaw angle trajectory, its expression in the local time t ∈ [0, T θ,k ] is: where k = 1,..., Ω; γ(t) = [1, t, t 2 ,t 3 , t 4 , t 5 ] T is a polynomial basis vector of time; c x，i , c y，i , c θ，k ∈ R 6 are polynomial coefficient vectors of the i-th segment of position trajectory x component, y component, and the k-th segment of yaw angle trajectory, respectively; T xy，i is the duration of the i-th segment of position trajectory; T θ，k is the duration of the k-th segment of yaw angle trajectory; S4012, organize all position related coefficient vectors into a large coefficient matrix c xy = or flatten it into a vector form ∈R 12M , all yaw angle related coefficient vectors are organized into time composition vector T for all position segments xy = xy，1 , xy,M , T ∈R M time composition vector T for all yaw angle segments θ = θ,1 , θ,Ω , T ∈R Ω .

9. The multi-objective optimization based adaptive vehicle path planning method for uneven terrain according to claim 7, wherein, The step S5 further comprises the following sub-steps: S501, based on the optimized two-dimensional trajectory x(t), y(t), θ(t), sampling is carried out at a dense time interval; S502、for each sampling point (x s ,y s ,θ s ), call the terrain attribute model interface of step S2, input the SE(2) state unit, and obtain the corresponding three-dimensional position P s =[x s ,y s ,z s ]Tand the rotation matrix R s of the vehicle body at the point, which is constructed by the terrain normal vector z θ and the yaw angle θ s ; S503, output the sequence (P s , R s ) as the final three-dimensional vehicle trajectory.

10. A multi-objective optimization based uneven terrain adaptive vehicle path planning system characterized by, Comprise: The data acquisition and point cloud map construction module is used for acquiring point cloud data of a predetermined working area of the vehicle, preprocessing the point cloud data, and establishing a three-dimensional point cloud map of the predetermined working area; The terrain attribute modeling and dynamics quantity calculation module is used for constructing a terrain data basic structure based on the preprocessed point cloud data, then constructing a terrain attribute model, querying terrain attributes at any continuous point SE(2) state sequence and analyzing gradients by using the terrain attribute model, combining the gradients to calculate vehicle dynamics quantities, and constructing an uneven terrain model; The rough trajectory generation module is used for generating a preliminary two-dimensional rough trajectory by using an Eco-Kino A* path search algorithm based on the uneven terrain model; The multi-objective trajectory optimization and constraint fusion module is used for using a rear-end trajectory optimization algorithm, using a segmented polynomial to represent a trajectory, taking the preliminary two-dimensional rough trajectory as a starting point, establishing a multi-objective optimization function covering trajectory smoothness cost, curvature cost, energy cost and vehicle attitude stability cost, and combining the vehicle dynamics constraint, the non-holonomic constraint and the terrain constraint to obtain the optimized two-dimensional trajectory; The three-dimensional trajectory generation and output module is used for generating a final three-dimensional vehicle trajectory according to the optimized two-dimensional trajectory and the uneven terrain model.

Images