Obstacle Avoidance and Turning Trajectory Planning Method for Driverless Trailers in Narrow Channels
Through the trajectory planning method based on the optimal control theory, the problem of difficulty in turning of unmanned tow vehicles in narrow channels is solved, and a safe and smooth optimal trajectory is generated, which improves traffic efficiency and safety.
Patent Information
- Application Number
- CN202211587954.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-12-12
- Publication Date
- 2025-08-05
- Estimated Expiration
- 2042-12-12
AI Technical Summary
The prior art is difficult to plan the safe and smooth turning trajectory of unmanned tow vehicles in narrow passages, especially in complex environments such as ports. The existing methods are difficult to meet complex constraints and the derived path is not smooth.
Using a trajectory planning method based on optimal control theory, the optimal trajectory that meets all constraints is generated by constructing vehicle kinematics, obstacle avoidance, control amount and angle constraints, combined with the improved Minkowski sum method and CILQR algorithm.
The traffic efficiency and driving safety of unmanned towed vehicles in narrow passages are improved, the generated trajectory is the optimal path, and the solution speed is fast.
Smart Images
Figure CN115840451B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of unmanned driving, and in particular to a trajectory planning method for an unmanned driving trailer to avoid obstacles and turn in narrow passages such as ports. Background Art
[0002] With the deepening of economic globalization, e-commerce platforms and import and export trade have flourished, driving the development of the transportation industry. Trailer trucks, due to their low transportation costs, large transport capacity, and convenient loading and unloading, have become the preferred choice for long-distance road transport in the transportation industry and are widely used in major factories and ports. However, the densely packed container ports and narrow passageways pose difficulties for trailer trucks, which can stretch over 20 meters. This is especially true when facing right-angle turns, where collision-free maneuvering is required within a confined space. This places even higher demands on the motion planning of unmanned trailer trucks.
[0003] In the prior art, path planning methods based on sampling and searching are commonly used to plan the movement paths of autonomous vehicles. The tractor and trailer of a towed vehicle are connected by an articulated joint, resulting in high flexibility in vehicle movement and complex constraints in trajectory planning. Path planning methods based on sampling and searching struggle to guarantee a feasible path while meeting these complex constraints. Furthermore, the paths derived by these two methods are not smooth, making it difficult for the subsequent tracking module to track the path. Even using mathematical methods to smooth the path, it is difficult to guarantee that the resulting path is optimal. Summary of the Invention
[0004] To address the difficulty of unmanned trailer vehicles turning in narrow passages such as ports, this paper provides a narrow passage obstacle avoidance turning trajectory planning method for unmanned trailer vehicles based on optimal control theory. This method directly generates vehicle trajectories using optimal control theory, and can plan an optimal trajectory that satisfies all constraints within a given confined space. This improves the efficiency and safety of trailer vehicles in narrow passages.
[0005] To this end, the present invention adopts the following technical solutions:
[0006] A method for planning a turning trajectory for an unmanned trailer with obstacle avoidance in a narrow passage includes the following steps:
[0007] S1, in the period t∈[t0,t0+Δt], the state of the unmanned trailer is obtained using the onboard sensor:
[0008] [x r ,y r ,θ1,θ2,v1,δ f ]
[0009] Among them, (x r ,yr ) is the position coordinate of the midpoint of the rear axle of the tractor in the Cartesian coordinate system, θ1 is the yaw angle of the tractor, θ2 is the yaw angle of the trailer, v1 is the speed of the midpoint of the rear axle of the tractor, δ f is the front wheel turning angle of the tractor;
[0010] Obtain point cloud data around the vehicle and a series of position coordinates (Xi, Yi) of the outer contours of static obstacles around the unmanned trailer, as well as a series of coordinates (Xi′, Yi′, T) of the outer contours of the current and predicted positions of dynamic obstacles;
[0011] Forming an instantaneous position mapping relationship between the unmanned trailer and the surrounding obstacles based on the state quantity of the unmanned trailer and the position point set of the static and dynamic obstacles around it;
[0012] S2, construct vehicle kinematic constraints:
[0013] The front axle, rear axle and trailer axle of the tractor are simplified into three points in the geodetic coordinate system, denoted as O f (x f ,y f ), O r (x r ,y r ), O l (x l ,y l ), assuming that the hinge point of the tractor and the trailer is located directly above the midpoint of the tractor's rear axle, the tractor's wheelbase is l1, the distance from the hinge point to the midpoint of the trailer's rear axle is l2, ignoring lateral sliding, and the axial velocity of the midpoints of all axles of the tractor and the trailer is zero, satisfying the following formula:
[0014]
[0015] According to the geometric position relationship, the midpoint O of the rear axle of the tractor is established. r and the midpoint O of the drag box axis f The relationship between the rear axle midpoint O of the tractor r and the midpoint O of the front axle of the tractor l The relationship between the two is used to calculate the horizontal and vertical speeds of the midpoint of the tractor's rear axle in the geodetic coordinate system, the speed of the midpoint of the tractor's front axle, and the speed of the midpoint of the trailer axle, and then the kinematic equation of the vehicle with the hinge point at the midpoint of the tractor's rear axle is obtained;
[0016] S3, build obstacle avoidance constraints:
[0017] Assume that the distance from the front of the tractor to the front end of the trailer is S, the width of the tractor is W, and the length and width of the trailer are L and W, respectively, and use a circle and an ellipse to cover the tractor and the trailer. Based on the geometric dimensions of the tractor and the definition of the largest inscribed rectangle of a circle, obtain the equation of the circle with the center at the origin of the Cartesian coordinate system and the equation of the ellipse with the center at the origin of the Cartesian coordinate system;
[0018] The mapped outer contours of a circle and an ellipse covering the vehicle body are obtained from the mapped outer contours of the vehicle in step S1. Based on the mapped outer contours of the circle and the ellipse and the mapped outer contours of each static and dynamic obstacle, an improved Minkowski sum method is used to generate an obstacle avoidance area for the unmanned trailer. The improved Minkowski sum method is used to define the entire area enclosed by the unmanned trailer mapping when the center of the unmanned trailer mapping circles around the outer contour of the obstacle mapping generated in step S1 as the collision area to be avoided by the center of the circle and the center of the ellipse during planning.
[0019] Connect the center of the circle / ellipse to the tangent point of the arc and the straight line in the obstacle avoidance area, and determine whether the sum of the angles of all the connecting lines is equal to 360°. If it is equal to 360°, the center of the circle is inside the obstacle avoidance area and the collision constraint condition is not satisfied. If the sum of the angles is not equal to 360°, the collision constraint condition is satisfied. The collision constraint condition can be abbreviated as:
[0020] L barrier (X(t))≤0,
[0021] Among them, X(t) is the vehicle state;
[0022] S4, constructing control quantity constraints to constrain the control quantity and its increment to ensure vehicle driving stability;
[0023] S5, construct two-point boundary constraints: identify the state and control variables at the initial time and the state and control variables at the given target time, which together constitute the two-point boundary constraints;
[0024] S6, construct the angle constraint between the tractor and the trailer;
[0025] S7, constructing the optimal control problem: by the cost function Together with the vehicle kinematic constraints, obstacle avoidance constraints, control quantity constraints, two-point boundary constraints, and the angle constraints between the tractor and the trailer, an optimal control problem for solving the optimal trajectory is constructed;
[0026] S8, by the search-based path planning algorithm A * Generate an initial path connecting the vehicle starting point and the target point, smooth the initial path with a spline curve, and calculate the slope k of the initial path in the Cartesian coordinate system. Obtain the reference front wheel turning angle at the path point; assume that the reference speed direction on the reference line is along the tangent direction of the reference line, which is a constant value ν f ; The initial path has the angle and speed reference control amount δ r with ν r , adding the control path will give us time information, thus obtaining the initial trajectory;
[0027] S9, build vehicle jerk constraints:
[0028] jerk min ≤jerk≤jerk max
[0029] Convert it into part of the cost function and construct the interior point penalty function; solve the nonlinear programming problem under complex constraints through the CILQR algorithm;
[0030] S10, using dynamic programming to solve LQR: the vehicle state transition equation is expanded along the planned trajectory at the reference point by Taylor first order, completing the linearization of the state transition equation. The ILQR problem is transformed into solving a series of LQR problems along the planned trajectory to obtain an optimized trajectory.
[0031] S11: Perform iterative LQR solution on the optimized trajectory again until the cost function converges to 10 -4 , stop the iteration and get the optimal trajectory.
[0032] Among them, step S2 tractor rear axle midpoint O r and the midpoint O of the drag box axis f The relationship between them is:
[0033]
[0034] Tractor rear axle midpoint O r and the midpoint O of the front axle of the tractor l The relationship between them is:
[0035]
[0036] The lateral and longitudinal speeds of the midpoint of the rear axle of the tractor in the geodetic coordinate system are:
[0037]
[0038] The speed of the midpoint of the tractor's front axle is:
[0039]
[0040] The speed of the midpoint of the trailer axle is:
[0041]
[0042] The kinematic equation of a vehicle with the hinge point located at the midpoint of the rear axle of the tractor is:
[0043]
[0044] The circle equation described in step S3 is:
[0045]
[0046] The equation of the ellipse is:
[0047]
[0048] The control quantity constraint in step S4 satisfies the following formula:
[0049]
[0050] Where a is the acceleration of the tractor, jerk is the derivative of the tractor's acceleration, v1 is the speed of the tractor's rear axle midpoint, and δ f is the front wheel turning angle of the tractor, and ω is the angular velocity of the front wheel turning angle of the tractor.
[0051] In step S5, the state quantity and control quantity at the initial time t=0 are identified as: X(0)=X initial ,U(0)=U initial ; Given the target time t = T, the state and control variables are: X(T) = X aim , U(T)=U aim .
[0052] In step S6, the angle constraint condition between the tractor and the trailer is: 0°≤|θ1-θ2|<90°.
[0053] In step S9, the jerk constraint cost is:
[0054] jerk min ≤jerk≤jerk max ,
[0055] The interior point penalty function is as follows:
[0056]
[0057]
[0058] Where q1 and q2 are coefficients;
[0059] By adjusting the values of q1 and q2 in the formula, a curve with properties that better meet the requirements of the cost function is obtained; if the current acceleration is within the constraint range, the penalty function will obtain a very small value; if it is outside the range, an exponential penalty value will be obtained; using the penalty function to represent the constraint function and convert it into part of the cost function, the CILQR problem is converted into an ILQR problem;
[0060] The solution process of step S10 is as follows:
[0061] The initial trajectory is divided into a series of continuous time periods. The state transfer equation is Taylor expanded in each time period. The state transfer equation and cost function after Taylor expansion are:
[0062]
[0063]
[0064] Introduce Q function, described in X T Take action U in state T The cost:
[0065]
[0066] The V function is introduced to describe the cost of the state itself. Here, the state cost is defined as equal to the Q cost that can make the subsequent state cost optimal:
[0067]
[0068] Rewrite the cost matrix as follows:
[0069]
[0070] (1)Backward process:
[0071] Taking the derivative of the Q function, we can set it to zero:
[0072]
[0073] It can be solved as follows:
[0074]
[0075] make:
[0076] The control quantity can be obtained as:
[0077] u T =K T x T +k T
[0078] According to the above derivation results, we have:
[0079]
[0080] have to:
[0081]
[0082] Where:
[0083]
[0084] Go back to the time T-1, because X T-1 It is not a terminal node state, so the extended Q function is defined as:
[0085]
[0086] By the formula:
[0087]
[0088] Rewriting the V function yields:
[0089]
[0090] The Q function at time T-1 is:
[0091]
[0092] Where:
[0093]
[0094] Derivative the Q function again and set its derivative equal to zero:
[0095]
[0096] The control quantity at time T-1 can be obtained:
[0097] u T-1 =K T-1 x T-1 +k T-1
[0098] By backtracking multiple times, we can get the initial state:
[0099] u1=K1x1+k1
[0100] (2) Forward process:
[0101] In the backward process, a series of control variables are obtained by calculating from back to front. These control variables are then introduced into the vehicle state transition equation from front to back to obtain an optimized trajectory:
[0102]
[0103] Compared with the prior art, the present invention has the following beneficial effects:
[0104] (1) This proposal addresses the difficulty of motion planning for unmanned trailers traveling through narrow passages such as ports, and proposes a trajectory planning method based on optimal control theory. This method describes the trajectory planning problem as solving an optimal control problem that satisfies constraints. By solving the optimal control problem, the optimal trajectory of the vehicle is obtained, thereby improving the efficiency and safety of unmanned trailers in narrow passages. This is of great significance for promoting the development of unmanned driving technology and improving the efficiency of port cargo transportation.
[0105] (2) The CILQR algorithm is used to solve nonlinear programming problems under complex constraints. The constraint function is represented by a penalty function, which is converted into a part of the cost function. The LQR problem is solved using dynamic programming, and the optimal trajectory is obtained through continuous iteration. Compared with other methods for solving optimal control problems, this method has clever constraint handling, and the iterative optimization solution can obtain the optimal trajectory with high solution speed.
[0106] (3) When describing obstacle avoidance constraints, the outer volumes of the tractor and cargo box are described using circles and ellipses, respectively. The concept of a collision polygon is introduced, defined as a modified Minkowski sum of the circle covering the vehicle body and the obstacle at a given moment. The collision polygon is generated by taking the vehicle's position into account. Compared with other methods for generating obstacle regions, a smaller collision polygon results in a larger traversable area, making it easier to obtain the optimal trajectory. BRIEF DESCRIPTION OF THE DRAWINGS
[0107] Figure 1 This is a schematic diagram of an unmanned trailer avoiding obstacles and turning in a narrow passage;
[0108] Figure 2 A simplified schematic diagram of the vehicle kinematic model for an unmanned trailer;
[0109] Figure 3 This is a schematic diagram of the circle and ellipse coverage of the unmanned trailer body;
[0110] Figure 4 Schematic diagram of the obstacle avoidance area for the tow box generated using the improved Minkowski sum method;
[0111] Figure 5 Solve the flowchart for ILQR. DETAILED DESCRIPTION
[0112] This paper uses the CILQR (Constraint Iteration LQR) algorithm to solve optimal control problems. CILQR, an extension of the ILQR algorithm, was developed to address optimization problems with complex constraints that the LQR algorithm cannot handle. It uses a penalty function to represent the constraint function to solve constrained LQR problems, ultimately employing the same framework as ILQR to obtain the optimal solution. The inequality constraints in the problem are transformed into a portion of the cost function, constructing an interior point penalty function:
[0113]
[0114] By adjusting the values of q1 and q2, we can obtain a curve that better meets the requirements of our cost function. LQR requires that the system state transition equation is linear, and ILQR extends it to nonlinear models. The overall idea of ILQR solution is: first determine an initial trajectory, and then use LQR to optimize this trajectory. The initial trajectory uses the search-based path planning algorithm A * Obtain, smooth the initial path with a spline curve, and in the Cartesian coordinate system, calculate the reference front wheel angle at each path point from the slope k of the initial path. Set the reference speed on this reference line as a constant value, and the direction along the tangent of the reference line. With the reference control variable, the initial path forms the initial reference trajectory.
[0115] When optimizing the trajectory using LQR, dynamic programming is employed. First, a backward process is performed, generating a series of δu values. Then, a forward process is used to sequentially substitute the control variable changes obtained in the backward process into the state transition equation, updating the state variables and obtaining an optimized trajectory. The backward and forward processes are repeated iteratively until the cost function converges, resulting in an optimal trajectory that satisfies the constraints.
[0116] The method of the present invention is described in detail below with reference to the accompanying drawings.
[0117] The unmanned trailer described in the present invention includes a tractor and a trailer box.
[0118] See also Figure 1 The narrow channel unmanned trailer obstacle avoidance turning trajectory planning method of the present invention comprises the following steps:
[0119] S1, in the period t∈[t0,t0+Δt], the state of the unmanned trailer is obtained using vehicle-mounted sensors such as GPS and IMU:
[0120] [x r,y r ,θ1,θ2,v1,δ f ]
[0121] Among them, (x r ,y r ) is the position coordinate of the midpoint of the rear axle of the tractor in the Cartesian coordinate system, θ1 is the yaw angle of the tractor, θ2 is the yaw angle of the trailer, v1 is the speed of the midpoint of the rear axle of the tractor, δ f is the front wheel turning angle of the tractor.
[0122] LiDAR scans the vehicle's surrounding point cloud data. The perception and prediction module then generates a series of position coordinates (Xi, Yi) for the outer contours of static obstacles surrounding the unmanned trailer, as well as a series of coordinates (Xi', Yi', T) for the current and predicted outer contours of dynamic obstacles. The state of the unmanned trailer and the location points of its surrounding static and dynamic obstacles are used to form a mapping relationship between the instantaneous position of the unmanned trailer and the surrounding obstacles.
[0123] S2, construct vehicle kinematic constraints:
[0124] Since the unmanned trailer has a low speed and a small tire side slip angle when turning, the vehicle kinematic model is sufficient to describe its motion characteristics when turning. According to the speed v1 of the center point of the rear axle of the tractor and the front wheel angle δ f Create a simplified kinematic diagram of the trailer, such as Figure 2 shown.
[0125] The front axle, rear axle and trailer axle of the tractor are simplified to three points in the geodetic coordinate system, denoted as O f (x f ,y f ), O r (x r ,y r ), O l (x l ,y l By locating the coordinates of the center of the tractor's rear axle, assume that the hinge point between the tractor and the trailer is directly above the midpoint of the tractor's rear axle. Assume that the tractor's wheelbase is l1 and the distance from the hinge point to the midpoint of the trailer's rear axle is l2. Ignoring lateral slip, the axial velocity of the midpoints of all axles of the tractor and the trailer is zero, satisfying the following equation:
[0126]
[0127] According to the geometric position relationship, the midpoint O of the rear axle of the tractor is established. r and the midpoint O of the drag box axis f The relationship between them is as follows:
[0128]
[0129] According to the geometric position relationship, the midpoint O of the rear axle of the tractor is established. r and the midpoint O of the front axle of the tractor l The relationship between them is as follows:
[0130]
[0131] The horizontal and vertical speeds of the midpoint of the rear axle of the tractor in the geodetic coordinate system are expressed as:
[0132]
[0133] The speed of the midpoint of the tractor's front axle is expressed as:
[0134]
[0135] The speed of the midpoint of the trailer axle is expressed as:
[0136]
[0137] The vehicle kinematic equation with the hinge point located at the midpoint of the rear axle of the tractor is obtained from the above formula:
[0138]
[0139] S3, build obstacle avoidance constraints:
[0140] Assume that the distance from the front of the tractor to the front end of the trailer is S, the width of the tractor is W, the length of the trailer is L, and the width is also W. Use a circle and an ellipse to cover the towed tractor and trailer respectively, as shown in the following example: Figure 3 As shown in the figure, based on the geometric dimensions of the tractor and the definition of the largest inscribed rectangle of a circle, the equation of the circle with its center at the origin of the Cartesian coordinate system is:
[0141]
[0142] According to the geometric dimensions of the drag box and the definition of the largest inscribed rectangle of an ellipse, the equation of the ellipse with its center at the origin of the Cartesian coordinate system is:
[0143]
[0144] The vehicle's mapped outer contour in step S1 yields the mapped outer contours of a circle and an ellipse covering the vehicle. Based on these mapped outer contours, along with the mapped outer contours of each static and dynamic obstacle, an improved Minkowski sum method is used to generate an obstacle avoidance zone for the unmanned trailer. Using the improved Minkowski sum method, the entire area enclosed by the unmanned trailer's mapped circle, when the center of the circle orbits the outer contour of the obstacle map generated in step S1, is defined as the collision zone to be avoided by the center of the circle and ellipse, respectively, during planning. Figure 4 The area surrounded by the outermost dotted line in the middle right figure (obstacle avoidance area) is the obstacle avoidance area generated by the improved Minkowski sum method. This area is the area that the center of the ellipse on the left side of the figure (the center of the tow box) must avoid when planning the trajectory.
[0145] Connect the center of the circle / ellipse with the tangent point of the arc and the straight line in the obstacle avoidance area, and check whether the sum of the angles of all the connecting lines is equal to 360°. If it is equal to 360°, it proves that the center of the circle is inside the obstacle avoidance area and the collision constraint condition is not met; if the sum of the angles is not equal to 360°, the collision constraint condition is met. The collision constraint condition is abbreviated as:
[0146] L barrier (X(t))≤0,
[0147] Among them, X(t) is the vehicle state
[0148] S4, construct control quantity constraints:
[0149] The control quantity constraint describes the maximum and minimum values of the vehicle control quantity. In the actual control process, not only the control quantity but also the increment of the control quantity must be constrained to prevent the increment from changing too quickly and ensure the stability of the vehicle. The control quantity constraint satisfies the following formula:
[0150]
[0151] In the above inequality, a is the acceleration of the tractor, jerk is the derivative of the tractor's acceleration (jerk), v1 is the speed at the midpoint of the tractor's rear axle, and δ f is the front wheel turning angle of the tractor, and ω is the angular velocity of the front wheel turning angle of the tractor.
[0152] S5, construct two-point boundary constraints:
[0153] Identify the state and control variables at the initial time t = 0: X(0) = X initial ,U(0)=U initial , reflects the true value at the initial moment;
[0154] Given the state and control variables at target time t=T: X(T)=X aim, U(T)=U aim , together with the state quantity and control quantity at the initial moment, constitute a two-point boundary constraint.
[0155] S6, construct the angle constraint between the tractor and the trailer:
[0156] In order to prevent the tractor from folding and affecting driving safety, the angle between the towing tractor and the trailer must be restricted. The angle constraint conditions between the tractor and the trailer are:
[0157] 0°≤|θ1-θ2|<90°
[0158] S7, construct the optimal control problem:
[0159] From the cost function:
[0160]
[0161] Together with vehicle kinematic constraints, obstacle avoidance constraints, control quantity constraints, two-point boundary constraints, and angle constraints, they constitute the optimal control problem of solving the optimal trajectory.
[0162] S8, by the search-based path planning algorithm A * Generate an initial path connecting the vehicle's starting point and target point, and smooth the initial path using a spline curve. Calculate the slope k of the initial path in the Cartesian coordinate system using the formula:
[0163] δ fr =arctan(k)
[0164] Obtain the reference front wheel turning angle on the path point, and set the reference velocity direction on the reference line along the tangent direction of the reference line to be a constant value v f The initial path has the angle and speed reference control quantity δ r with ν r , adding the control path will have time information (the path with time information is the trajectory), and the initial trajectory will be obtained;
[0165] S9, build vehicle jerk constraints:
[0166] By transforming the inequality constraints in the problem into part of the cost function, an interior point penalty function is constructed. The CILQR algorithm is used to solve nonlinear programming problems under complex constraints.
[0167] The jerk constraint cost is:
[0168] jerk min ≤jerk≤jerk max
[0169] The interior point penalty function is as follows:
[0170]
[0171]
[0172] Where q1 and q2 are coefficients. By adjusting the values of q1 and q2, we can obtain a curve that better meets the requirements of our cost function. If the current acceleration is within the constraint range, the penalty function will result in a very small value; if it is outside the range, an exponential penalty value will be applied. By using the penalty function to represent the constraint function and converting it into part of the cost function, the CILQR problem is transformed into the ILQR problem.
[0173] S10, solve LQR using dynamic programming:
[0174] The vehicle state transition equation is linearized by performing a Taylor first-order expansion at the reference point along the planned trajectory. Solving the ILQR problem is then transformed into solving a series of LQR problems along the planned trajectory.
[0175] The initial trajectory is divided into a series of continuous time periods. The state transfer equation is Taylor expanded in each time period. The state transfer equation and cost function after Taylor expansion are:
[0176]
[0177]
[0178] Introduce Q function, described in X T Take action U in state T The cost:
[0179]
[0180] The V function is introduced to describe the cost of the state itself. Here, the state cost is defined as equal to the Q cost that can make the subsequent state cost optimal:
[0181]
[0182] Rewrite the cost matrix as follows:
[0183]
[0184] (1)Backward process:
[0185] Taking the derivative of the Q function, we can set it to zero:
[0186]
[0187] It can be solved as follows:
[0188]
[0189] make:
[0190] The control quantity can be obtained as:
[0191] u T =K T x T +k T
[0192] According to the above derivation results, we have:
[0193]
[0194] have to:
[0195]
[0196] Where:
[0197]
[0198] Go back to the time T-1, because X T-1 It is not a terminal node state, so the extended Q function is defined as:
[0199]
[0200] By the formula:
[0201]
[0202] Rewriting the V function yields:
[0203]
[0204] The Q function at time T-1 is:
[0205]
[0206] Where:
[0207]
[0208] Derivative the Q function again and set its derivative equal to zero:
[0209]
[0210] The control quantity at time T-1 can be obtained:
[0211] u T-1 =K T-1 x T-1 +kT-1
[0212] By backtracking multiple times, we can get the initial state:
[0213] u1=K1x1+k1
[0214] (2) Forward process:
[0215] In the backward process, a series of control variables are obtained by calculating from back to front. These control variables are then introduced into the vehicle state transition equation from front to back to obtain an optimized trajectory:
[0216]
[0217] S11: Perform iterative LQR solution on the optimized trajectory again. The iterative LQR solution process is as follows: Figure 5 As shown, until the cost function converges to 10 -4 , then stop the iteration and get the optimal trajectory.
Claims
1. A method for planning turning trajectories for an unmanned trailer with obstacle avoidance in a narrow passage, comprising the following steps: S1, in the period t∈[t0, t0+Δt], the state of the unmanned trailer is obtained using the onboard sensor: [x] r ,y r ,θ1,θ2,v1,δ f ] Among them, (x r ,y r ) is the position coordinate of the midpoint of the rear axle of the tractor in the Cartesian coordinate system, θ1 is the yaw angle of the tractor, θ2 is the yaw angle of the trailer, v1 is the speed of the midpoint of the rear axle of the tractor, δ f is the front wheel turning angle of the tractor; Obtain point cloud data around the vehicle and a series of position coordinates (Xi, Yi) of the outer contours of static obstacles around the unmanned trailer, as well as a series of coordinates (X, Y) of the outer contours of the current and predicted positions of dynamic obstacles. i ′,Y i ′, T); Forming an instantaneous position mapping relationship between the unmanned trailer and the surrounding obstacles based on the state quantity of the unmanned trailer and the position point set of the static and dynamic obstacles around it; S2, construct vehicle kinematic constraints: The front axle, rear axle and trailer axle of the tractor are simplified into three points in the geodetic coordinate system, denoted as O f (x f ,y f ), O r (x r ,y r ), O l (x l ,y l ), assuming that the hinge point of the tractor and the trailer is located directly above the midpoint of the tractor's rear axle, the tractor's wheelbase is l1, the distance from the hinge point to the midpoint of the trailer's rear axle is l2, ignoring lateral sliding, and the axial velocity of the midpoints of all axles of the tractor and the trailer is zero, satisfying the following formula: According to the geometric position relationship, the midpoint O of the rear axle of the tractor is established. r and the midpoint O of the drag box axis f The relationship between the rear axle midpoint O of the tractor r and the midpoint O of the front axle of the tractor l The relationship between the two is used to calculate the horizontal and vertical speeds of the midpoint of the tractor's rear axle in the geodetic coordinate system, the speed of the midpoint of the tractor's front axle, and the speed of the midpoint of the trailer axle, and then the kinematic equation of the vehicle with the hinge point at the midpoint of the tractor's rear axle is obtained; S3, build obstacle avoidance constraints: Assume that the distance from the front of the tractor to the front end of the trailer is S, the width of the tractor is W, and the length and width of the trailer are L and W, respectively, and use a circle and an ellipse to cover the tractor and the trailer. Based on the geometric dimensions of the tractor and the definition of the largest inscribed rectangle of a circle, obtain the equation of the circle with the center at the origin of the Cartesian coordinate system and the equation of the ellipse with the center at the origin of the Cartesian coordinate system; The mapped outer contours of a circle and an ellipse covering the vehicle body are obtained from the mapped outer contours of the vehicle in step S1. Based on the mapped outer contours of the circle and the ellipse and the mapped outer contours of each static and dynamic obstacle, an improved Minkowski sum method is used to generate an obstacle avoidance area for the unmanned trailer. The improved Minkowski sum method is used to define the entire area enclosed by the unmanned trailer mapping when the center of the unmanned trailer mapping circles around the outer contour of the obstacle mapping generated in step S1 as the collision area to be avoided by the center of the circle and the center of the ellipse during planning. Connect the center of the circle / ellipse to the tangent point of the arc and the straight line in the obstacle avoidance area, and determine whether the sum of the angles of all the connecting lines is equal to 360°. If it is equal to 360°, the center of the circle is inside the obstacle avoidance area and the collision constraint condition is not satisfied. If the sum of the angles is not equal to 360°, the collision constraint condition is satisfied. The collision constraint condition can be abbreviated as: L barrier (X(t))≤0, Among them, X(t) is the vehicle state; S4, constructing control quantity constraints to constrain the control quantity and its increment to ensure vehicle driving stability; S5, construct two-point boundary constraints: identify the state and control variables at the initial time and the state and control variables at the given target time, which together constitute the two-point boundary constraints; S6, construct the angle constraint between the tractor and the trailer; S7, constructing the optimal control problem: by the cost function Together with the vehicle kinematic constraints, obstacle avoidance constraints, control quantity constraints, two-point boundary constraints, and the angle constraints between the tractor and the trailer, an optimal control problem for solving the optimal trajectory is constructed; S8, by the search-based path planning algorithm A * Generate an initial path connecting the vehicle starting point and the target point, smooth the initial path with a spline curve, and calculate the slope k of the initial path in the Cartesian coordinate system. Obtain the reference front wheel turning angle at the path point; assume that the reference velocity direction on the reference line is along the tangent direction of the reference line, which is a constant value v f ; The initial path has the angle and speed reference control amount δ r With v r , adding the control path will give us time information, thus obtaining the initial trajectory; S9, build vehicle jerk constraints: jerk min ≤jerk≤jerk max Convert it into part of the cost function and construct the interior point penalty function; solve the nonlinear programming problem under complex constraints through the CILQR algorithm; S10, using dynamic programming to solve LQR: the vehicle state transition equation is expanded along the planned trajectory at the reference point by Taylor first order, completing the linearization of the state transition equation. The ILQR problem is transformed into solving a series of LQR problems along the planned trajectory to obtain an optimized trajectory. S11: Perform iterative LQR solution on the optimized trajectory again until the cost function converges to 10 -4 , stop the iteration and get the optimal trajectory.
2. The narrow channel unmanned trailer obstacle avoidance turning trajectory planning method according to claim 1 is characterized in that: In step S2, the rear axle midpoint O of the tractor r and the midpoint O of the drag box axis f The relationship between them is: Tractor rear axle midpoint O r and the midpoint O of the front axle of the tractor l The relationship between them is: The lateral and longitudinal speeds of the midpoint of the rear axle of the tractor in the geodetic coordinate system are: The speed of the midpoint of the tractor's front axle is: The speed of the midpoint of the trailer axle is: The kinematic equation of a vehicle with the hinge point located at the midpoint of the rear axle of the tractor is:
3. The narrow channel unmanned trailer obstacle avoidance turning trajectory planning method according to claim 1 is characterized in that: The circle equation described in step S3 is: The equation of the ellipse is:
4. The narrow channel unmanned trailer obstacle avoidance turning trajectory planning method according to claim 1 is characterized in that: The control quantity constraint in step S4 satisfies the following formula: Where a is the acceleration of the tractor, jerk is the derivative of the tractor's acceleration, v1 is the speed of the tractor's rear axle midpoint, and δ f is the front wheel turning angle of the tractor, and ω is the angular velocity of the front wheel turning angle of the tractor.
5. The narrow channel unmanned trailer obstacle avoidance turning trajectory planning method according to claim 1 is characterized in that: In step S5, the state quantity and control quantity at the initial time t=0 are identified as: X(0)=X initial ,U(0)=U initial ; Given the target time t = T, the state and control variables are: X(T) = X aim , U(T)=U aim .
6. The narrow channel unmanned trailer obstacle avoidance turning trajectory planning method according to claim 1 is characterized in that: In step S6, the angle constraint condition between the tractor and the trailer is: 0°≤|θ1-θ2|<90°.
7. The narrow channel unmanned trailer obstacle avoidance turning trajectory planning method according to claim 1 is characterized in that: In step S9, the jerk constraint cost is: jerk min ≤jerk≤jerk max , The interior point penalty function is as follows: Where q1 and q2 are coefficients; By adjusting the values of q1 and q2 in the formula, a curve with properties that better meet the requirements of the cost function is obtained; if the current acceleration is within the constraint range, the penalty function will obtain a very small value; if it is outside the range, an exponential penalty value will be obtained; the penalty function is used to represent the constraint function and converted into part of the cost function, thus converting the CILQR problem into an ILQR problem.
8. The narrow channel unmanned trailer obstacle avoidance turning trajectory planning method according to claim 1 is characterized in that: The solution process of step S10 is as follows: The initial trajectory is divided into a series of continuous time periods. The state transfer equation is Taylor expanded in each time period. The state transfer equation and cost function after Taylor expansion are: Introduce Q function, described in X T Take action U in state T The cost: The V function is introduced to describe the cost of the state itself. Here, the state cost is defined as equal to the Q cost that can make the subsequent state cost optimal: Rewrite the cost matrix as follows: (1)Backward process: Taking the derivative of the Q function, we can set it to zero: It can be solved as follows: make: The control quantity can be obtained as: u T =K T x T +k T According to the above derivation results, we have: Where: Go back to the time T-1, because X T-1 It is not a terminal node state, so the extended Q function is defined as: By the formula: and Rewriting the V function yields: The Q function at time T-1 is: Where: Derivative the Q function again and set its derivative equal to zero: The control quantity at time T-1 can be obtained: u T-1 =K T-1 x T-1 +k T-1 By backtracking multiple times, we can get the initial state: u1=K1x1+k1 (2) Forward process: In the backward process, a series of control variables are obtained by calculating from back to front. These control variables are then introduced into the vehicle state transition equation from front to back to obtain an optimized trajectory: