Distributed driving steering intelligent vehicle hierarchical trajectory planning method and system
Through improved hybrid A* algorithm and numerical optimization methods, distributed drive steering vehicles generate continuous, smooth, comfortable, safe and efficient trajectories in complex unstructured scenarios, solving the problem that traditional methods cannot fully utilize vehicle flexibility. Improved tracking performance.
Patent Information
- Application Number
- CN202410970524.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-19
- Publication Date
- 2024-11-08
AI Technical Summary
In complex unstructured scenarios with narrow congestion and dense obstacles, the traditional distributed driving steering vehicle trajectory planning method fails to fully utilize its advantages of high flexibility and passability, making it difficult to generate continuous, smooth, comfortable, safe and efficient Trajectory.
The improved hybrid A* algorithm is used to search for the initial trajectory segmented by the mode switching point, and the numerical optimization method is combined to optimize the trajectory segments of the four-wheel steering mode, design cost functions and constraints to ensure smoothness and comfort of the trajectory. safety, safety and efficiency.
It realizes the generation of continuous, smooth, comfortable, safe and efficient trajectories in complex unstructured scenarios, improves the safety and efficiency of trajectory tracking, and has faster calculation speed and higher optimization success rate.
Smart Images

Figure CN118915744A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of intelligent vehicles, and in particular relates to a distributed drive steering intelligent vehicle hierarchical trajectory planning method and system. Background Art
[0002] At present, most intelligent vehicles are retrofitted based on the traditional drive steering configuration. The mobility and controllability of the vehicle are limited by the traditional configuration, and it is difficult to adapt to complex unstructured scenes with narrow congestion and dense obstacles. The distributed drive steering configuration will become the ideal carrier of future intelligent vehicles because of its high flexibility and passability, and is conducive to the realization of integrated, lightweight and modular design.
[0003] Trajectory planning for vehicles with traditional drive steering configuration can be divided into search-based methods, sampling-based methods, curve interpolation-based methods, numerical optimization-based methods, and learning-based methods according to the planning method. However, the current distributed drive steering vehicles basically follow the planning method of the traditional configuration, without considering the characteristics of their own four wheels being able to independently control driving and steering, and have not brought into play their advantages of high flexibility and passability in complex unstructured scenes with narrow congestion and dense obstacles. At the same time, on the basis of ensuring the continuity, smoothness and safety of the trajectory of the distributed drive steering vehicle, the comfort and efficiency of the trajectory should also be appropriately considered.
[0004] In summary, in complex unstructured scenarios with narrow congestion and dense obstacles, how to enable distributed drive steering intelligent vehicles to generate continuous, smooth, comfortable, safe and efficient trajectories is an urgent problem to be solved. Summary of the invention
[0005] The present invention discloses a hierarchical trajectory planning method and system for a distributed drive steering intelligent vehicle. The upper layer uses the advantages of fast search speed and good applicability of an improved hybrid A* algorithm to search and construct an initial trajectory segmented according to mode switching points. The lower layer uses a numerical optimization method to optimize the initial trajectory segment of a four-wheel steering mode to improve the quality of the trajectory, thereby effectively solving at least one technical problem involved in the background technology.
[0006] To achieve the above object, the technical solution of the present invention is:
[0007] A distributed drive steering intelligent vehicle hierarchical trajectory planning method comprises the following steps:
[0008] Step S1, analyzing the kinematic model of the distributed drive steering vehicle to obtain unstructured scene related parameters;
[0009] Step S2, redesigning the cost function and node expansion method of the hybrid A* algorithm, searching for a rough path segmented by mode switching points, and constructing a corresponding initial trajectory as an initial solution for trajectory optimization;
[0010] Step S3, optimize the initial trajectory using a numerical optimization method based on the driving corridor, design the objective function and constraints that consider trajectory smoothness, comfort, safety, and efficiency, construct the trajectory optimization optimal control problem, and then solve the trajectory optimization optimal control problem to obtain the optimal trajectory.
[0011] As a preferred improvement of the present invention, in step S1, the kinematic model of the distributed drive steering vehicle includes a four-wheel steering mode, a wedge steering mode and an in-situ steering mode, wherein:
[0012] In the four-wheel steering mode, the front and rear wheels of the vehicle have the same turning angles and opposite directions, and the left and right wheels are in the same state at any time. The vehicle posture has both translation and rotation.
[0013] In the wedge steering mode, the front and rear wheel angles of the vehicle are the same in magnitude and direction, and the left and right wheels are in the same state at any time, and the vehicle posture only translates without rotation;
[0014] In the on-the-spot steering mode, the vehicle rotates on the spot around the center point, and the vehicle posture only rotates without translation.
[0015] As a preferred improvement of the present invention, in step S1, the unstructured scene related parameters include the geometric parameters and kinematic parameters of the vehicle, the starting point position and posture End point pose and unstructured scene obstacle maps; the vehicle’s geometric parameters include the front overhang length L f 、Rear overhang length L r 、Wheelbase L m , total length L, total width B, the vehicle's kinematic parameters include the minimum speed v min , maximum speed v max , minimum acceleration a min , maximum acceleration a max , maximum steering angle δ max , maximum steering angular velocity ω max .
[0016] As a preferred improvement of the present invention, the node expansion mode in step S2 includes four-wheel steering mode node expansion, wedge steering mode node expansion and in-place steering mode node expansion. In each iterative search of the algorithm, the parent node Three different modes of subnodes can be searched in discrete space Among them, the subscript i represents the parent node information, and the subscript i+1 represents the child node information.
[0017] As a preferred improvement of the present invention, when the expansion step length is l1 and the yaw angle change is When , the position of the straight-line child node in the four-wheel steering mode is:
[0018]
[0019] Where, d i+1 =1 means the vehicle is in forward driving state, d i+1 =-1 means the vehicle is in reverse driving state;
[0020] The steering subnode pose of the four-wheel steering mode is:
[0021]
[0022] Where R is the turning radius;
[0023] When the expansion step is l2 and the yaw angle change is When , the subnode position in wedge steering mode is:
[0024]
[0025] In the formula, is the discretized wheel angle;
[0026] When the extension step is l3=0 and the yaw angle change is When , the position of the child node in the in-place turn mode is:
[0027] x i+1 =x i
[0028] y i+1 =y i
[0029]
[0030] In the formula, is the yaw angle.
[0031] As a preferred improvement of the present invention, in step S2, the cost function includes a cumulative cost function, a heuristic cost function, a mode switching cost function and a safety cost function; wherein the cumulative cost function is:
[0032]
[0033] In the formula, g1(n i+1 ) represents the cumulative cost function when the vehicle is in the four-wheel steering mode and expands the child node, g2(n i+1) represents the cumulative cost function when the vehicle is in the wedge-shaped steering mode and expands the child node, g3(n i+1 ) represents the cumulative cost function when the vehicle is in the on-site steering mode and expands the child node; m i+1 =1 means the vehicle is in four-wheel steering mode, m i+1 =2 means the vehicle is in wedge steering mode, m i+1 =3 means the vehicle is in the on-site steering mode;
[0034] The heuristic cost function is:
[0035] h(n i+1 )=max{h nonholonomics (n i+1 ), h holonomics (n i+1 )}
[0036] In the formula, h nonholonomics (n i+1 ) represents the length of the estimated path that complies with the vehicle kinematic constraints but ignores the collision factor, preventing the exploration path from reaching the target node from the wrong direction; h holonomics (n i+1 ) represents the length of the estimated path that takes into account obstacle avoidance factors but does not consider kinematic feasibility, preventing the exploration path from entering a broken path or a concave obstacle;
[0037] The mode switching cost function is:
[0038]
[0039] In the formula, m r (n i+1 ) is the restoration cost function; m c (n i+1 ) is the switching cost function;
[0040] Restoration cost function m r (n i+1 ) is expressed as:
[0041]
[0042] In the formula, v r1 、v r2 、v r3 They represent the cost of returning the four-wheel steering mode, wedge steering mode, and on-the-spot steering mode to their initial states respectively;
[0043] Switching cost function m c (n i+1 ) is expressed as:
[0044]
[0045] In the formula, v c1 represents the cost of switching between the four-wheel steering mode and the wedge steering mode; v c2 represents the cost of switching between the four-wheel steering mode and the on-site steering mode; v c3 Indicates the cost of switching between wedge steering mode and on-the-spot steering mode.
[0046] The security cost function is:
[0047]
[0048] In the formula, s j It represents the safety cost caused by a single obstacle, and its expression is:
[0049]
[0050] In the formula, K rep Represents the proportional coefficient of obstacle repulsion; r j represents the Euclidean distance from the current node to the geometric center of the jth obstacle; ρ0 represents the obstacle influence distance.
[0051] As a preferred improvement of the present invention, in step 4, the constraints include vehicle kinematic constraints, two-point boundary value constraints and driving corridor constraints, and the trajectory optimization optimal control problem is expressed as:
[0052]
[0053]
[0054] In the formula, represents the objective function to be minimized in the optimal control problem; the first and second terms in the constraint conditions are two-point boundary value constraints, [x start ,u start ] and [x final ,u final ] represent the optimization starting time t=t s and the optimization termination time t = t f The corresponding state quantity and control quantity; the third to fifth items are vehicle kinematic constraints, represents the nonlinear kinematic equation of the vehicle, [x min , x max ] and [u min ,u max ] represents the allowed value range of the state quantity and the control quantity; the last item is the driving corridor constraint, Represents the mapping of vehicle states to motion footprints.
[0055] The present invention also provides a distributed drive steering intelligent vehicle hierarchical trajectory planning system, comprising:
[0056] The environment perception module is used to obtain the vehicle's surrounding environment information and its own status information, and use relevant algorithms to process the obstacle information and vehicle status information, and input them into the trajectory planning module;
[0057] The trajectory planning module is used to obtain the vehicle motion trajectory information and the corresponding vehicle motion mode information by adopting the hierarchical trajectory planning method according to the obstacle information and vehicle status information input by the environment perception module and the vehicle starting position and ending position determined by the driving task, and input them into the motion control module;
[0058] The motion control module is used to select the motion controller of the corresponding mode to control the vehicle to achieve trajectory tracking based on the vehicle motion trajectory information and the motion modes of different trajectory segments input by the trajectory planning module.
[0059] As a preferred improvement of the present invention, the obstacle information includes the type of obstacle and the positions of the vertices of the envelope polygon, and the vehicle state information includes the position, yaw angle, speed, acceleration and angular velocity of the vehicle.
[0060] The beneficial effects of the present invention are as follows:
[0061] The present invention designs a complete set of hierarchical trajectory planning algorithm theories for complex unstructured scenarios. Compared with the complete traditional hybrid A* algorithm, the path curve is more in line with the movement characteristics of distributed drive steering intelligent vehicles, and the safety and efficiency of trajectory tracking are improved; compared with the direct use of numerical optimization algorithms, it has faster calculation speed and higher optimization success rate, and finally realizes that distributed drive steering intelligent vehicles can generate continuous, smooth, comfortable, safe and efficient trajectories in complex unstructured scenarios with narrow congestion and dense obstacles. BRIEF DESCRIPTION OF THE DRAWINGS
[0062] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative work, among which:
[0063] Figure 1 A flow chart of a distributed drive steering intelligent vehicle hierarchical trajectory planning method according to the present invention;
[0064] Figure 2 It is a schematic diagram of the kinematic model of the vehicle in different modes, where: Figure 2 (a) is a schematic diagram of the kinematic model of the four-wheel steering mode. Figure 2 (b) is a schematic diagram of the kinematic model of the wedge-shaped steering mode. Figure 2 (c) is a schematic diagram of the kinematic model of the on-the-spot turning mode;
[0065] Figure 3 Schematic diagram of node expansion in different modes, where: Figure 3 (a) is a schematic diagram of the node expansion of the four-wheel steering mode. Figure 3 (b) is a schematic diagram of the node expansion of the wedge steering mode. Figure 3 (c) is a schematic diagram of the node expansion of the on-site turning mode;
[0066] Figure 4 A schematic diagram of the search process for improving the hybrid A* algorithm;
[0067] Figure 5 Schematic diagram of the kinematic model of the four-wheel steering mode covered by double discs;
[0068] Figure 6 This is a schematic diagram of the expansion of the obstacle map in an unstructured scene, where: Figure 6 (a) is a schematic diagram before puffing. Figure 6 (b) is a schematic diagram after expansion;
[0069] Figure 7 Generate a schematic diagram for the two-stage corridor, where Figure 7 (a) is a schematic diagram of the first stage. Figure 7 (b) is a schematic diagram of the second stage;
[0070] Figure 8 Resulting graph of trajectories for unstructured scenarios generated by the hierarchical trajectory planner;
[0071] Fig. 9 This is a framework diagram of a distributed drive steering intelligent vehicle hierarchical trajectory planning system of the present invention. DETAILED DESCRIPTION
[0072] The technical solutions in the embodiments of the present invention will be described clearly and completely below in combination with the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0073] It should be noted that all directional indications in the embodiments of the present invention (such as up, down, left, right, front, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.
[0074] In addition, in the present invention, descriptions such as "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.
[0075] In the present invention, unless otherwise clearly specified and limited, the terms "connection", "fixation", etc. should be understood in a broad sense. For example, "fixation" can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements, unless otherwise clearly defined. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.
[0076] In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the fact that ordinary technicians in the field can implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by the present invention.
[0077] The present invention proposes a distributed driving steering intelligent vehicle hierarchical trajectory planning method, such as Figure 1 As shown, the following steps are included:
[0078] Step S1, analyzing the kinematic model of the distributed drive steering vehicle to obtain unstructured scene related parameters;
[0079] The kinematic model analysis of the vehicle is the basis for trajectory planning, which can ensure that the planned trajectory can be tracked safely, efficiently and reasonably by the vehicle. In complex unstructured scenes with narrow congestion and dense obstacles, the vehicle's movement speed is low, so the vehicle is set to have three movement modes: four-wheel steering, wedge steering, and in-situ steering, such as Figure 2 As shown, the center point of the vehicle's wheelbase is (x, y), and the vehicle's yaw angle is The vehicle posture composed of the two is
[0080] The four-wheel steering mode is the basic motion mode of distributed drive steering vehicles. Under normal circumstances, the vehicle moves in this mode. Assuming that the front and rear wheels of the vehicle have the same turning angles and opposite directions in this mode, and the left and right wheels are in the same state at any time, the kinematic model of the four-wheel steering mode is established as follows: Figure 2 (a) In this mode, the vehicle posture has both translation and rotation, and the turning radius R is:
[0081]
[0082] Where δ1 is the wheel angle.
[0083] The corresponding change in yaw angle for:
[0084]
[0085] Where l1 is the vehicle movement distance.
[0086] The wedge steering mode is a special motion mode of distributed drive steering vehicles. It is assumed that the magnitude and direction of the front and rear wheel steering angles of the vehicle are the same, and the left and right wheels are in the same state at any time. Therefore, the kinematic model of the wedge steering mode is established as follows: Figure 2 (b) In this mode, the vehicle posture only translates but does not rotate. Direction straight driving, and yaw angle Remain unchanged.
[0087] The on-the-spot steering mode is a special motion mode of distributed drive steering vehicles. Assuming that the vehicle rotates on the spot around the center point in this mode, the kinematic model of the on-the-spot steering mode is established as follows: Figure 2 (c) In this mode, the vehicle posture only rotates without translation, the center point (x, y) remains unchanged, and the yaw angle changes have occurred.
[0088] After analyzing the kinematic model of the distributed drive steering vehicle, the unstructured scene related parameters are obtained, including the vehicle's geometric parameters and kinematic parameters, the starting point position and posture End point pose And unstructured scene obstacle map. Among them, the vehicle's geometric parameters include the front overhang length L f 、Rear overhang length L r 、Wheelbase L m , total length L, total width B; the vehicle's kinematic parameters include the minimum speed v min , maximum speed v max , minimum acceleration a min , maximum acceleration a max , maximum steering angle δ max , maximum steering angular velocity ω max .
[0089] Step S2, redesigning the cost function and node expansion method of the hybrid A* algorithm, searching for a rough path segmented by mode switching points, and constructing a corresponding initial trajectory as an initial solution for trajectory optimization;
[0090] (I) Node expansion method design
[0091] The node expansion method of traditional hybrid A* is based on the classic bicycle model to generate a continuous motion path that satisfies the vehicle's kinematic constraints and can avoid obstacles. In order to adapt to the motion characteristics of different modes of distributed drive steering vehicles, three node expansion methods are designed: four-wheel steering, wedge steering, and in-place steering. In each iteration of the algorithm, the parent node Three different modes of subnodes can be searched in discrete space And it is necessary to record the extended node movement mode, movement direction and other information into the hybrid A* node information. Specifically, the following provisions are made in advance: subscript i represents the parent node information, subscript i+1 represents the child node information; m i+1 =1 means the vehicle is in four-wheel steering mode, m i+1 =2 means the vehicle is in wedge steering mode, m i+1 =3 means the vehicle is in the on-site steering mode; d i+1 =1 means the vehicle is in forward driving state, d i+1 =-1 indicates that the vehicle is in reverse driving state.
[0092] 1. Four-wheel steering mode node expansion
[0093] like Figure 3 (a) shows the node expansion method of the four-wheel steering mode, which generates the straight-line subnode and steering subnode of the four-wheel steering mode that meet the kinematic constraints, and discretizes the wheel angle δ1 to obtain:
[0094]
[0095] In the formula, δ max Indicates the maximum turning angle of the wheel in the non-stationary steering mode; N1 indicates the number of nodes expanded each time in the four-wheel steering mode.
[0096] When the expansion step is l1 and the yaw angle change is When , the position and posture of the sub-node of the four-wheel steering mode are obtained according to the kinematic model analysis in step S1.
[0097] Among them, the posture of the straight child node is:
[0098]
[0099] The posture of the steering child node is:
[0100]
[0101] 2. Wedge-shaped steering mode node expansion
[0102] like Figure 3(b) shows the expansion method of the wedge steering mode node, which generates a wedge steering mode subnode that satisfies the kinematic constraints. The wheel angle δ2 is discretized to obtain:
[0103]
[0104] Where N2 represents the number of nodes expanded each time in the wedge steering mode.
[0105] When the expansion step is l2 and the yaw angle change is When , the subnode position of the wedge steering mode is obtained according to the kinematic model analysis in step S1:
[0106]
[0107] 3. On-site steering mode node expansion
[0108] like Figure 3 (c) shows the expansion method of the on-the-spot steering mode node, which generates a sub-node of the on-the-spot steering mode that satisfies the kinematic constraints. Its wheel angle δ3 is tangent to the steering circle, and the yaw angle change is discretized to obtain:
[0109]
[0110] When the extension step is l3=0 and the yaw angle change is When , the position and posture of the child node in the in-place turning mode are obtained according to the kinematic model analysis in step S1:
[0111] x i+1 =x i
[0112] y i+1 =y i
[0113]
[0114] (II) Node cost function design
[0115] The cost function of the traditional hybrid A* includes cumulative cost and heuristic cost, which are mainly used to guide the path to search for the target node. Compared with the traditional hybrid A* algorithm, the present invention introduces mode switching cost and safety cost, which can not only reduce the energy and time loss caused by mode switching, but also ensure a certain safety distance between the path and obstacles. The designed total cost function f(n) is:
[0116] f(n)=g(n)+h(n)+m(n)+s(n)
[0117] Where g(n) is the cumulative cost function, which represents the cumulative cost from the starting node to the current node; h(n) is the heuristic cost function, which represents the heuristic cost from the current node to the target node; m(n) is the mode switching cost function, which represents the cost of the vehicle switching from the current mode to the next motion mode; s(n) is the safety cost function, which represents the sum of the costs from the current node to the center of all obstacles within the affected range.
[0118] 1. Cumulative cost function g(n)
[0119] In principle, the cumulative cost function g(n) mainly records the total cumulative cost from the starting node to the current node. However, in order to make the searched path as short as possible and avoid excessive reversing, changes in travel direction, excessive turns, and changes in turning direction, a series of penalties are introduced in the hybrid A* algorithm to enrich the meaning of the cumulative cost function.
[0120] The calculation method of the cumulative cost function of the improved hybrid A* algorithm proposed in the present invention has different calculation formulas for three different motion modes:
[0121]
[0122] In the formula, g1(n i+1 ) represents the cumulative cost function when the vehicle is in the four-wheel steering mode expansion subnode; g2(n i+1 ) represents the cumulative cost function when the vehicle is in the wedge-shaped steering mode and expands the subnode; g3(n i+1 ) represents the cumulative cost function when the vehicle is in the on-the-spot steering mode and expands the child node.
[0123] Specifically, when the four-wheel steering mode expands the child node, the cumulative cost function g1(n i+1 )for:
[0124]
[0125] Where, α represents the priority coefficient of the four-wheel steering mode; They respectively represent the reversing penalty coefficient, the travel direction change penalty coefficient, the wheel angle size penalty coefficient, and the wheel angle direction change penalty coefficient in the four-wheel steering mode.
[0126] When the wedge-shaped steering mode expands the child node, the cumulative cost function g2(n i+1 )for:
[0127]
[0128] Where, β represents the priority coefficient of the wedge steering mode; They respectively represent the reversing penalty coefficient, the travel direction change penalty coefficient, the wheel angle size penalty coefficient, and the wheel angle direction change penalty coefficient in the wedge steering mode.
[0129] When the in-place turning mode expands the child node, the cumulative cost function g3(n i+1 )for:
[0130]
[0131] In the formula, γ represents the priority coefficient of the on-the-spot steering mode, Indicates the penalty coefficient for the rotation angle in the on-the-spot turning mode.
[0132] 2. Heuristic cost function h(n)
[0133] The heuristic cost function h(n) has an important impact on the search efficiency of the entire hybrid A* algorithm. Theoretically, h(n) reflects the length of the shortest path from the current node posture to the target node posture that satisfies the kinematic constraints and obstacle avoidance constraints. However, since the length of the theoretical path cannot be known in advance, the hybrid A* algorithm can only estimate the length of the theoretical path.
[0134] The improved hybrid A* algorithm proposed in the present invention follows the heuristic cost function calculation method of the traditional hybrid A*, adopts the parallel calculation of the non-holonomic constraint heuristic function without considering obstacles and the integrity constraint heuristic function considering obstacles, and takes the larger of the two as the heuristic cost value of the hybrid A* algorithm. Its expression is:
[0135] h(n i+1 )=max{h nonholonomics (n i+1 ), h holonomics (n i+1 )}
[0136] For the first term, the non-holonomic constraint heuristic function h does not consider obstacles. nonholonomics (n i+1 ), which represents the length of the estimated path that meets the vehicle kinematic constraints but ignores collision factors, which can prevent the exploration path from reaching the target node from the wrong direction. The calculation method is: Use the Reeds-Shepp curve to connect the extended child node poses With the target node pose And interpolate to calculate the curve length.
[0137] For the second item, consider the integrity constraint heuristic function h of the obstacle holonomics (n i+1), which represents the length of the estimated path that takes into account obstacle avoidance factors but does not consider kinematic feasibility, which can prevent the exploration path from entering a broken path or a concave obstacle. The calculation method is: Use the A* algorithm or Dijkstra algorithm to search for the node from the expanded child node (x i+1 ,y i+1 ) to the target node (x target ,y target ) path and calculate the length of the path.
[0138] The hybrid A* algorithm takes the larger value of the two methods of calculating the heuristic cost function h(n) as the current heuristic cost, which has the following characteristics: when the distance between the current node and the target node is far, h holonomics (n i+1 ) is more likely to occur. The difficulty of generating this path mainly lies in avoiding obstacles within a larger range. When the distance between the current node and the target node is close, h nonholonomics (n i+1 ) is more likely to occur, and the difficulty in generating this path mainly lies in ensuring the kinematic feasibility.
[0139] 3. Mode switching cost function m(n)
[0140] The mode switching cost m(n) is mainly used to record the cost of changing the motion mode of the distributed drive steering vehicle. When the vehicle switches the motion mode, it first switches to the initial state, that is, the wheel angles are all zero, and then switches to the next motion mode. Therefore, the mode switching cost function includes two parts: recovery cost and switching cost.
[0141] For the recovery cost m r (n i+1 ), and its calculation formula is:
[0142]
[0143] In the formula, v r1 、v r2 、v r3 They represent the cost of restoring the four-wheel steering mode, wedge steering mode, and on-the-spot steering mode to their initial states respectively.
[0144] For the switching cost m c (n i+1 ), and its calculation formula is:
[0145]
[0146] In the formula, v c1 represents the cost of switching between the four-wheel steering mode and the wedge steering mode; v c2 represents the cost of switching between the four-wheel steering mode and the on-site steering mode; vc3 Indicates the cost of switching between wedge steering mode and on-the-spot steering mode.
[0147] Therefore, the total mode switching cost m(n i+1 )for:
[0148]
[0149] 4. Security cost function s(n)
[0150] The traditional hybrid A* algorithm focuses on searching for a collision-free path that satisfies the vehicle's kinematic constraints, but the path may be too close to obstacles in some areas, or the searched path may choose to go through narrow passages instead of more spacious areas. If the subsequent tracking control is not accurate enough, the risk of collision between the vehicle and the obstacle will increase. In order to solve this problem of the traditional A* algorithm, the safety cost function s(n) is introduced into the total cost function. The main idea is to use the obstacle repulsion potential field in the artificial potential field as a safety cost function that needs to be considered in the heuristic function. The introduction of this safety cost will make the search of the path node keep a certain distance from the obstacle, thereby reducing the risk of collision between the vehicle and the obstacle.
[0151] The safety cost function calculation method of the improved hybrid A* algorithm proposed in the present invention is: if the Euclidean distance between the current node and the geometric center of the obstacle is greater than the obstacle impact distance, the impact of the obstacle on the current node is ignored; if the Euclidean distance between the current node and the geometric center of the obstacle is less than or equal to the obstacle impact distance, the safety cost of the obstacle to the current node needs to be considered. The specific safety cost calculation formula for a single obstacle is:
[0152]
[0153] Where ρ0 represents the obstacle impact distance; r j K represents the Euclidean distance from the current node to the geometric center of the jth obstacle; rep Represents the proportional coefficient of the obstacle repulsion force; s j Represents the safety cost of the j-th obstacle to the current node.
[0154] Finally, the total safety cost is obtained by summing up the safety costs of all obstacles to the current node, which is expressed as:
[0155]
[0156] Where M represents the number of obstacles.
[0157] (III) Improved Hybrid A* Algorithm Search
[0158] Combined with the node expansion design and cost function design in the improved hybrid A* algorithm in the above steps, follow the steps below Figure 4 The process shown is to perform path search:
[0159] 1) Input the starting node start_node and the target node final_node in the unstructured scene;
[0160] 2) Create and initialize an open list open_list and a closed list close_list. The open_list is used to store the nodes to be searched, and the close_list is used to store the nodes that have been searched.
[0161] 3) Set the current node now_node to start_node and add it to open_list;
[0162] 4) Expand now_node according to the node expansion methods of three different motion modes to obtain several adjacent child nodes i_node, and perform obstacle collision detection on i_node in turn, eliminate all colliding infeasible child nodes, and then perform the following operations on the remaining child nodes:
[0163] A. Determine whether i_node is in close_list. If so, skip this node and determine the next one. If not, continue with the following steps;
[0164] B. If i_node is not in the open_list, add it to the open_list, and calculate the cumulative cost g(n), heuristic cost h(n), mode switching cost m(n), safety cost s(n), and total cost f(n) corresponding to i_node, and set the parent node to now_node;
[0165] C. If i_node is in the open_list, determine the total cost f(n) of i_node and the total cost f′(n) of the original node. If it is less than that, replace the information of the original node corresponding to the open_list with i_node, otherwise keep the information of the original node unchanged.
[0166] D. Use a sorting algorithm to sort the nodes in open_list in ascending order of total cost f(n).
[0167] 5) Determine whether open_list is empty:
[0168] A. If open_list is not empty, delete now_node from open_list and add it to close_list. Then select the node with the smallest total cost f(n) from open_list as now_node, and use the Reeds-Shepp curve to connect now_node and final_node. If there is a collision between the lines, return to step 4) and re-execute; if there is no collision between the lines, it means that the path search is successful and the path from start_node to final_node is found;
[0169] B. If open_list is empty, it means that the path search failed and the path from start_node to final_node could not be found.
[0170] (IV) Segmented initial trajectory construction
[0171] The segmented path searched by the improved hybrid A* algorithm in (III) above is used to estimate the total planning time using the Pontryagin principle with the maximum acceleration as the optimal control quantity, and the A* algorithm is used to search for the initial speed curve in the ST diagram of speed planning. Then, the original hybrid A* discrete path points are resampled at equal time steps to obtain new discrete path points.
[0172] Taking a path segment of a four-wheel steering mode as an example, if the number of resampling points is N and the time of the starting point is t s The end point is at time t f , the time step is t step =(t f -t s ) / (N-1), the sampling time is t k = k·t step and Uniformly distributed in the time domain [t s , t f ], so the rough path formed after resampling of this path segment is Then the rough path ζ0 is constructed into the initial trajectory segment corresponding to the four-wheel steering mode according to the following formula: And give it to the lower layer for trajectory optimization.
[0173]
[0174]
[0175]
[0176]
[0177]
[0178] Step S3, optimize the initial trajectory using a numerical optimization method based on the driving corridor, design the objective function and constraints that consider trajectory smoothness, comfort, safety, and efficiency, construct the trajectory optimization optimal control problem, and then solve the trajectory optimization optimal control problem to obtain the optimal trajectory.
[0179] 1. Trajectory Optimization Problem Construction
[0180] The trajectory optimization problem of the distributed drive steering vehicle mainly adopts the numerical optimization method for the initial trajectory segment of the four-wheel steering mode, including three constraints: vehicle kinematic constraints, two-point boundary constraints, and driving corridor constraints, as well as an objective function that considers trajectory smoothness, comfort, and efficiency. And the state quantity of the vehicle is specified as (d x represents the dimension of the state quantity), and the control quantity is (d u represents the dimension of the control quantity), the total planning time is T = t f -t s , the workspace is The obstacle space is Free space is In general, the trajectory optimization problem in complex unstructured scenarios given the vehicle’s starting state and target state can be described as a standard optimal control problem in the following form:
[0181]
[0182] In the formula, represents the objective function to be minimized in the optimal control problem; the first and second terms in the constraint conditions are two-point boundary value constraints, [x start ,u start ] and [x final ,u final ] represent the optimization starting time t=t s and the optimization termination time t = t f The corresponding state quantity and control quantity; the third, fourth and fifth items in the constraint conditions are vehicle kinematic constraints. represents the nonlinear kinematic equation of the vehicle, [x min , x max ] and [u min ,u max ] represents the allowed value range of the state quantity and the control quantity; the last item in the constraint condition is the obstacle avoidance constraint, i.e., the driving corridor constraint. Represents the mapping of vehicle states to motion footprints.
[0183] 1. Vehicle kinematic constraints
[0184] Considering the actual motion capability of the vehicle, and the trajectory must be well tracked, the kinematic constraints must be met. Since the trajectory optimization is for the initial trajectory segment of the four-wheel steering mode, the kinematic model analysis of the four-wheel steering mode should be satisfied. Therefore, the nonlinear kinematic equation of the vehicle in this mode is Defined as:
[0185]
[0186] In the formula, x(t) and y(t) represent the horizontal and vertical coordinates of the center point of the vehicle wheelbase; represents the yaw angle of the vehicle, that is, the angle between the positive direction of the longitudinal axis of the vehicle body and the positive direction of the X axis; v(t) and a(t) represent the speed and acceleration of the vehicle respectively; δ(t) and ω(t) represent the turning angle and angular velocity of the wheel respectively. And define the state quantity Control quantity u(t)=[a(t),ω(t)].
[0187] At the same time, the state and control of the vehicle are limited by the mechanical characteristics and movement capabilities of the vehicle, and there is an allowable range of values, mainly:
[0188]
[0189] The formula describes the upper and lower bounds of the velocity v(t), acceleration a(t), steering angle δ(t), and steering angular velocity ω(t).
[0190] 2. Two-point boundary value constraints
[0191] Considering the connection between the optimized trajectory segment and other trajectory segments, it is necessary to ensure that the posture of the optimization starting point and the end point are consistent. At the same time, there is a vehicle motion mode switch between trajectory segments, so the vehicle needs to return to the initial state (speed, acceleration, steering angle, and steering angular velocity are all 0), so the two-point boundary value constraint is defined as:
[0192]
[0193] In the formula, Indicates the optimization starting point t = t s The position at the moment; Indicates the optimization termination point t = t f The position of the moment.
[0194] 3. Driving corridor constraints
[0195] Considering that the vehicle needs to avoid collisions with obstacles throughout the planning cycle, it must satisfy obstacle avoidance constraints to ensure the safety of the trajectory. However, obstacle constraints are usually large in scale and non-differentiable, making it difficult for the optimal control problem solver to solve. Therefore, in order to simplify obstacle constraints, reduce the difficulty of solving, and speed up the solving rate, the obstacle constraints are Using the driving corridor method:
[0196] First, the rectangular body is evenly covered with two disks of the same radius, e.g. Figure 5 As shown, and from the symmetry, we know that the centers of the two disks are the four-division points on the longitudinal axis of the vehicle body. Therefore, through geometric analysis, the radius R of the disk can be obtained. disc for:
[0197]
[0198] And the center of the front disk is P f (x f ,y f ) coordinates and the center of the rear disk P r (x r ,y r ) are:
[0199]
[0200] Then, shrink the two disks toward their respective centers and move the boundaries of the polygonal obstacles in the environment to the radius R of the disk. disc Expand outward, such as Figure 6 As shown, the obstacle constraint can be transformed from the vehicle rectangle not colliding with the obstacle to the center mass point P f , P r Not on the expanded obstacle area.
[0201] Finally, the initial trajectory χ0 is transformed into the initial trajectory of each center point through the above formula and Then construct their own driving corridors on the initial trajectory. Figure 7 As shown in the figure, the two-stage corridor construction method is: the planning time domain [t s , t f ] uniformly sample N moments, for the tth k At each moment, a local rectangle is constructed with the center point as the center. In the first stage, the large step length Δl1 is incrementally expanded clockwise around the center until the expansion areas in all four directions touch the expansion obstacles. In the second stage, the small step length Δl2 is expanded in the same way on the basis of the first stage, and finally a complete driving rectangular area is formed. This method does not sacrifice too much free space and can save expansion time.
[0202]
[0203] 4. Cost Function
[0204] In order to reflect the trajectory comfort and efficiency, the designed objective function is defined as follows:
[0205]
[0206] Where w1, w2, w3>0 represents the weight coefficient of each item; the first item represents the total planning time, which reflects the efficiency of the trajectory; the second item represents the cumulative square value of acceleration, which reflects the comfort of the trajectory; the third item represents the product of the cumulative speed and the square of the steering angular velocity, which reflects the comfort of the trajectory.
[0207] (II) Trajectory Optimization Solution
[0208] After constructing the optimal control problem of trajectory optimization, it is necessary to solve the problem to obtain the optimal solution. The present invention discretizes the continuous variables in the optimal control problem, and then solves the nonlinear programming problem formed after discretization to directly obtain the numerical optimal solution of the original problem.
[0209] First, according to the discretization method of the initial solution in step S2 (iv), the uniformly distributed in the time domain [t s , t f ] at each sampling moment The corresponding constraints and objective functions are discretized, and the results are:
[0210] 1) The nonlinear kinematic equation constraints in the discretized kinematic constraints are as follows:
[0211]
[0212] 2) The range of state and control variables in the discretized kinematic constraints is as follows:
[0213]
[0214] 3) The starting point constraint in the discretized two-point boundary value constraint is as follows:
[0215]
[0216] 4) The end point constraints in the discretized two-point boundary value constraints are as follows:
[0217]
[0218] 5) The center position constraints of the front and rear disks in the discretized driving corridor constraints are as follows:
[0219]
[0220] 6) The local rectangular constraints in the discretized driving corridor constraints are as follows:
[0221]
[0222] 7) The discretized objective function is as follows:
[0223]
[0224] Then, the above discretized constraints and objective functions are summarized to form a complete nonlinear programming problem to be solved:
[0225] m inObjective function
[0226]
[0227] Finally, the open source solver IPOPT based on the interior point method, which is widely used in the field of autonomous driving, is used to solve the nonlinear programming problem formed by the above construction to obtain the numerical optimal solution of the trajectory, and then construct it into the optimal trajectory. Output to the downstream tracker for trajectory tracking. In this embodiment, the optimal trajectory of the unstructured scene finally planned is as follows Figure 8 As shown in the figure, the circle indicates that the vehicle is in the on-the-spot steering mode, the curve indicates that the vehicle is in the four-wheel steering mode, the dotted curve is the path searched by the upper-layer improved hybrid A* algorithm, and the solid curve is the trajectory after numerical optimization of the lower layer.
[0228] The present invention also provides a distributed drive steering intelligent vehicle hierarchical trajectory planning system, such as Fig. 9 As shown, it includes an environment perception module 1, a trajectory planning module 2 and a motion control module 3. Among them:
[0229] Environmental perception module 1, including the functions of obtaining and processing laser radar information, camera information and positioning information, is mainly used to obtain the vehicle's surrounding environment information and its own status information, and adopts relevant algorithms to process the obstacle information and vehicle status information, and inputs them into trajectory planning module 2;
[0230] Specifically, the obstacle information includes the type of obstacle and the position of the vertex of the envelope polygon, etc., and the vehicle state information includes the position, yaw angle, speed, acceleration and angular velocity of the vehicle, etc.
[0231] The trajectory planning module 2 has the functions of input information preprocessing, upper-level trajectory planning, lower-level trajectory planning, etc. It is used to obtain the vehicle motion trajectory information and the corresponding vehicle motion mode information according to the obstacle information and vehicle status information input by the environment perception module 1 and the vehicle starting posture and ending posture determined by the driving task, using the hierarchical trajectory planning method proposed in the present invention, and input them into the motion control module 3.
[0232] The motion control module 3 has the functions of four-wheel steering mode, wedge steering mode, and in-place steering mode motion control. It is used to select the motion controller of the corresponding mode to control the vehicle steering wheel angle, throttle, and brake to achieve trajectory tracking based on the vehicle motion trajectory information and the motion modes of different trajectory segments input by the trajectory planning module 2.
[0233] The beneficial effects of the present invention are as follows:
[0234] The present invention designs a complete set of hierarchical trajectory planning algorithm theories for complex unstructured scenarios. Compared with the complete traditional hybrid A* algorithm, the path curve is more in line with the movement characteristics of distributed drive steering intelligent vehicles, and the safety and efficiency of trajectory tracking are improved; compared with the direct use of numerical optimization algorithms, it has faster calculation speed and higher optimization success rate, and finally realizes that distributed drive steering intelligent vehicles can generate continuous, smooth, comfortable, safe and efficient trajectories in complex unstructured scenarios with narrow congestion and dense obstacles.
[0235] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation modes, which are merely illustrative rather than restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the present invention and the claims, all of which are within the protection of the present invention.
Claims
1. A distributed drive steering intelligent vehicle hierarchical trajectory planning method, characterized in that: The following steps are involved: Step S1, analyzing the kinematic model of the distributed drive steering vehicle to obtain unstructured scene related parameters; Step S2, redesigning the cost function and node expansion method of the hybrid A* algorithm, searching for a rough path segmented by mode switching points, and constructing a corresponding initial trajectory as an initial solution for trajectory optimization; Step S3, optimize the initial trajectory using a numerical optimization method based on the driving corridor, design the objective function and constraints that consider trajectory smoothness, comfort, safety, and efficiency, construct the trajectory optimization optimal control problem, and then solve the trajectory optimization optimal control problem to obtain the optimal trajectory.
2. A distributed drive steering intelligent vehicle hierarchical trajectory planning method according to claim 1, characterized in that: In step S1, the kinematic model of the distributed drive steering vehicle includes a four-wheel steering mode, a wedge steering mode and an in-situ steering mode, wherein: In the four-wheel steering mode, the front and rear wheels of the vehicle have the same turning angles and opposite directions, and the left and right wheels are in the same state at any time. The vehicle posture has both translation and rotation. In the wedge steering mode, the front and rear wheel angles of the vehicle are the same in magnitude and direction, and the left and right wheels are in the same state at any time, and the vehicle posture only translates without rotation; In the on-the-spot steering mode, the vehicle rotates on the spot around the center point, and the vehicle posture only rotates without translation.
3. The distributed drive steering intelligent vehicle hierarchical trajectory planning method according to claim 1 is characterized in that: In step S1, the unstructured scene related parameters include the vehicle's geometric parameters and kinematic parameters, the starting point position and End point pose and unstructured scene obstacle maps; the vehicle’s geometric parameters include the front overhang length L f 、Rear overhang length L r 、Wheelbase L m , total length L, total width B, the vehicle's kinematic parameters include the minimum speed v min , maximum speed v max , minimum acceleration a min , maximum acceleration a max , maximum steering angle δ max , maximum steering angular velocity ω max .
4. A distributed drive steering intelligent vehicle hierarchical trajectory planning method according to claim 3, characterized in that: The node expansion methods in step S2 include four-wheel steering mode node expansion, wedge steering mode node expansion and in-place steering mode node expansion. In each iteration search of the algorithm, the parent node Three different modes of subnodes can be searched in discrete space Among them, the subscript i represents the parent node information, and the subscript i+1 represents the child node information.
5. A distributed drive steering intelligent vehicle hierarchical trajectory planning method according to claim 4, characterized in that: When the expansion step is l1 and the yaw angle change is When , the position of the straight-moving child node in the four-wheel steering mode is: Where, d i+1 =1 means the vehicle is in forward driving state, d i+1 =-1 means the vehicle is in reverse driving state; The steering subnode pose of the four-wheel steering mode is: Where R is the turning radius; When the expansion step is l2 and the yaw angle change is When , the wedge steering mode child node position is: In the formula, is the discretized wheel angle; When the extension step is l3=0 and the yaw angle change is When , the position of the child node in the in-place turn mode is: x i+1 =x i and i+1 =and i In the formula, is the yaw angle.
6. A distributed drive steering intelligent vehicle hierarchical trajectory planning method according to claim 5, characterized in that: In step S2, the cost function includes a cumulative cost function, a heuristic cost function, a mode switching cost function and a safety cost function, wherein the cumulative cost function is: In the formula, g1(n i+1 ) represents the cumulative cost function when the vehicle is in the four-wheel steering mode and expands the child node, g2(n i+1 ) represents the cumulative cost function when the vehicle is in the wedge-shaped steering mode and expands the child node, g3(n i+1 ) represents the cumulative cost function when the vehicle is in the on-site steering mode and expands the child node; m i+1 =1 means the vehicle is in four-wheel steering mode, m i+1 =2 means the vehicle is in wedge steering mode, m i+1 =3 means the vehicle is in the on-site steering mode; The heuristic cost function is: h(n i+1 )=max{h nonholonomics (n i+1 ),h holonomics (n i+1 )} In the formula, h nonholonomics (n i+1 ) represents the length of the estimated path that complies with the vehicle kinematic constraints but ignores the collision factor, preventing the exploration path from reaching the target node from the wrong direction; h holonomics (n i+1 ) represents the length of the estimated path that takes into account obstacle avoidance factors but does not consider kinematic feasibility, preventing the exploration path from entering a broken path or a concave obstacle; The mode switching cost function is: In the formula, m r (n i+1 ) is the restoration cost function; m c (n i+1 ) is the switching cost function; Restoration cost function m r (n i+1 ) is expressed as: In the formula, v r1 、v r2 、v r3 They represent the cost of returning the four-wheel steering mode, wedge steering mode, and on-the-spot steering mode to their initial states respectively; Switching cost function m c (n i+1 ) is expressed as: In the formula, v c1 represents the cost of switching between the four-wheel steering mode and the wedge steering mode; v c2 represents the cost of switching between the four-wheel steering mode and the on-site steering mode; v c3 Indicates the cost of switching between the wedge steering mode and the on-the-spot steering mode; The security cost function is: In the formula, s j It represents the safety cost caused by a single obstacle, and its expression is: In the formula, K rep Represents the proportional coefficient of obstacle repulsion; r j represents the Euclidean distance from the current node to the geometric center of the jth obstacle; ρ0 represents the obstacle influence distance.
7. A distributed drive steering intelligent vehicle hierarchical trajectory planning method according to claim 6, characterized in that: In step 4, the constraints include vehicle kinematic constraints, two-point boundary value constraints and driving corridor constraints. The trajectory optimization optimal control problem is expressed as: In the formula, represents the objective function to be minimized in the optimal control problem; the first and second terms in the constraint conditions are two-point boundary value constraints, [x start ,u start ] and [x final ,u final ] represent the optimization starting time t=t s and the optimization termination time t = t f The corresponding state quantity and control quantity; the third to fifth items are vehicle kinematic constraints, represents the nonlinear kinematic equation of the vehicle, [x min ,x max ] and [u min ,u max ] represents the allowed value range of the state quantity and the control quantity; the last item is the driving corridor constraint, g collision (·): Represents the mapping of vehicle states to motion footprints.
8. A distributed drive steering intelligent vehicle hierarchical trajectory planning system implementing the method described in any one of claims 1 to 7, characterized in that: include: The environment perception module is used to obtain the vehicle's surrounding environment information and its own status information, and use relevant algorithms to process the obstacle information and vehicle status information, and input them into the trajectory planning module; The trajectory planning module is used to obtain the vehicle motion trajectory information and the corresponding vehicle motion mode information by adopting the hierarchical trajectory planning method according to the obstacle information and vehicle status information input by the environment perception module and the vehicle starting position and ending position determined by the driving task, and input them into the motion control module; The motion control module is used to select the motion controller of the corresponding mode to control the vehicle to achieve trajectory tracking based on the vehicle motion trajectory information and the motion modes of different trajectory segments input by the trajectory planning module.
9. A distributed drive steering intelligent vehicle hierarchical trajectory planning system according to claim 8, characterized in that: The obstacle information includes the obstacle type and the vertex positions of the envelope polygon, and the vehicle state information includes the vehicle's position, yaw angle, speed, acceleration, and angular velocity.
Citation Information
Cited By
Track planning method and system for narrow unstructured scene containing dynamic obstacle
CN120063308A
Trajectory planning method and system for narrow unstructured scene with dynamic obstacles
CN120063308B
Acute and critical patient transfer stretcher based on magnetic suspension technology
CN121059381A
Steering control method and system for distributed four-wheel-drive four-steering electric automobile
CN121224656A
Steering decoupling vehicle intelligent driving data set construction method based on expert algorithm
CN121325729A
