Multi-mode lane changing path planning algorithm

Abstract

The invention discloses a multi-mode lane changing path planning algorithm, which is used for planning in combination with the four-wheel independent steering characteristic of an in-port container flat plate transport vehicle, and obtaining a lane center line and boundary information based on an existing high-precision map to construct a reference line; acquiring surrounding obstacle information; point cloud information of an obstacle is output through a laser radar, the obstacle is reflected on a grid with a fixed resolution through point cloud in the form of whether a grid unit is occupied or not, and a drivable space is calculated in the path direction; on the basis of the feasible space obtained in the previous step, designing different lane changing modes, and deciding to select an optimal lane changing mode; and planning a lane changing path by using a sampling algorithm based on a decision result. The lane changing function in a narrow space is achieved by combining multi-mode control methods such as front and rear axle same-direction steering and front and rear axle reverse steering of the vehicle, the passing efficiency is improved, and the port operation efficiency is improved.

Description

Technical Field

[0001] The present invention relates to a path planning algorithm, and in particular to a multi-mode lane-changing path planning algorithm for a flatbed container transport vehicle in a port, and belongs to the technical field of automatic driving. Background Art

[0002] Fully automated ports can improve transportation efficiency, effectively reduce labor costs, and prevent accidents. They are an important direction for the intelligent transformation of traditional terminals. Unmanned container transfer vehicles, as an important part of fully automated terminals, are currently the focus of major commercial vehicle companies. The roads in the container transfer scene in the port are narrow, the traffic is congested, and the operating efficiency is required to be high. One of the effective ways to improve operating efficiency is to avoid congestion by changing lanes. The flatbed trucks for container transfer in the port are large in size and easy to swing when turning. The existing lane changing method occupies a lot of space on the left and right sides, and has high requirements for the clear area in front. Therefore, in the narrow roads in the container area, or in the scene where the vehicle has already stopped, the existing lane changing method cannot achieve collision-free lane changing, which is an urgent problem to be solved in the port scene. Summary of the invention

[0003] The technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a multi-mode lane change path planning algorithm. By combining multi-mode control methods such as the same-direction steering of the front and rear axles of the vehicle and the reverse steering of the front and rear axles, the lane changing function in a narrow space can be realized, the traffic efficiency can be improved, and the port operation efficiency can be improved.

[0004] In order to achieve the above objectives, the specific technical solutions of the present invention are as follows: a multi-mode lane change path planning algorithm is used to plan the path in combination with the four-wheel independent steering characteristics of the flatbed container transport vehicle in the port.

[0005] The specific steps include:

[0006] Step 1: Based on the existing high-precision map, the lane centerline and boundary information are obtained to construct a reference line;

[0007] Step 2: Obtain information about surrounding obstacles; output point cloud information of obstacles through the laser radar. The obstacles are reflected in the form of whether they occupy grid units on a grid with a fixed resolution through the point cloud, and the drivable space along the path direction is calculated;

[0008] Step 3: Based on the feasible space obtained in the previous steps, design different lane-changing modes and decide on the optimal lane-changing mode;

[0009] Step 4 plans the lane change path using a sampling algorithm based on the decision result.

[0010] Furthermore, in step 1, the lanes are at least two lanes, and the reference line greater than two lanes is described as {L middle,L right oeLh left}, each reference line consists of a series of points {P0, P1, ..., P n}, each point is represented by P{x, y, θ, s, Bl, Br}, where x and y are position information, θ is the heading, s is the length, Bl is the distance to the left lane line, and Br is the distance to the right lane line.

[0011] Furthermore, the specific content of step 2 is:

[0012] Step 2.1, obtain the obstacle information described by other non-geometric polygons detected by the laser radar and establish an m*n size grid map;

[0013] Step 2.2: along the reference line direction in step 1, perform longitudinal sampling with the grid map resolution established in the previous step as the sampling distance, calculate the sampling points based on the interpolation algorithm, and for each sampling point, perform horizontal sampling to the left and right along the vertical direction according to the grid map resolution. If the point is occupied by an obstacle, return the horizontal distance as the left and right boundary values ​​of the longitudinal sampling point (OBl, OBr);

[0014] Step 2.3: Combining the information obtained in step 1 and step 2.2, the drivable space is described by a series of points P i (x i ,y i ,θ i ,s i ,ΔOBl i ,ΔOBr i ), where x i ,y i is the position information, θ i is the heading, s i is the length, ΔOBl i =OB1 i -W / 2,ΔOBr i =OBr i -W / 2 is a hard constraint for lateral offset.

[0015] Furthermore, in step 3, the lane changing mode at least includes a crab lane changing mode, an S-shaped lane changing mode and a conventional lane changing mode;

[0016] When the front and rear axles of the flatbed truck turn to the same angle at the same time, the crab lane changing mode is used to reduce the change in the flatbed truck's posture; when the front and rear axles of the flatbed truck turn in opposite directions, the S-type lane changing mode is used to reduce the demand for road space ahead and achieve short-distance lane changing.

[0017] Furthermore, according to the drivable space in step 2, the distance s between the front and rear of the flatbed truck in the current lane and the obstacle is calculated. f ,s r , specifically: If the space ahead is greater than the minimum distance s of the crab lane change mode f >s c +d safe , and the target lane change space satisfies: that is, when changing lanes to the left, P i (ΔOBl i )>ρ cl , when changing lane right i (ΔOBr i )>ρ cl , you can directly use the crab lane change mode;

[0018] If the space ahead is greater than the minimum distance s for S-turn lane change f >s s +d safe , and the target lane change space satisfies: that is, when changing lanes to the left, P i (ΔOBl i )>ρ cl , when changing lane right i (ΔOBr i )>ρ cl , and the lane-changing space on the side satisfies: that is, when changing lanes to the left, P i (ΔOBr i )>ρ min , when changing lane right i (ΔOBl i )>ρ min , then you can directly use the S-type lane change mode;

[0019] If the side lane-changing space does not meet the minimum drift requirement, the lane-changing can be performed by crab-moving for a certain distance, combined with the S-shaped lane-changing mode, to avoid the drift problem while reducing the forward lane-changing space requirement to achieve lane changing.

[0020] Furthermore, when the side lane changing space does not meet the minimum drift requirement space, the lane changing is performed in a crab-like manner, and the feasibility of the S-type lane changing is calculated in real time during the lane changing process. Once the lane changing requirements are met, the S-type lane changing is switched to.

[0021] Furthermore, in step 4, the specific contents are as follows:

[0022] Step 4.1, design lane change path generation strategy:

[0023] Based on the reference line in step 1, perform horizontal and vertical sampling, define the longitudinal sampling length as Δs, and the horizontal sampling interval as Δρ, then the sampling point can be described as (s, ρ), and the sampling trajectory consists of two parts: the crab lane-changing trajectory, which contains two feature points: the crab starting point C0 (s0, ρ0) and the crab ending point C1 (s1, ρ1), and the crab trajectory can be expressed by a straight line; the S-shaped lane-changing trajectory, which contains two feature points: the starting point C2 (s2, ρ2) and the ending point C3 (s3, ρ3), and the S-shaped lane-changing trajectory can be expressed by a spline curve;

[0024] Step 4.2, design the vehicle trajectory:

[0025] Step 4.2.1, the crab walking starting point C0(s0,ρ0) is the closest matching point of the vehicle on the front reference line;

[0026] Step 4.2.2, based on the maximum crab walking angle, sample different lateral distances to obtain the crab walking end point C1(s1,ρ1);

[0027] Step 4.2.3, the starting point C2(s2,ρ2) of the S-shaped lane-changing trajectory is the end point of the crab move, that is, C1=C2;

[0028] Step 4.2.4, the end point C3(s3,ρ3) of the S-shaped lane change trajectory is the target lane sampling point, and the minimum distance from the start point of the S-shaped lane change trajectory is greater than the minimum lane change distance. The spline curve expression of the S-shaped lane change trajectory is as follows:

[0029]

[0030] Step 4.3: perform safety verification on the lane change trajectory calculated in the previous step. For any point on the trajectory, a BOX based on the vehicle model parameters can be obtained, and the position relationship of all obstacles based on the BOX can be calculated;

[0031] If the position relationship satisfies distance(box,obstacle)>0, the lane-changing trajectory meets the collision-free requirement.

[0032] Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention combines the four-wheel independent steering characteristics of the flatbed container transport vehicle in the port to provide a multi-mode lane-changing solution based on different driving modes. Compared with other algorithms, the present invention realizes the lane-changing function in a narrow space by combining multi-mode control methods such as the same-direction steering of the front and rear axles of the vehicle and the reverse steering of the front and rear axles, thereby improving the traffic efficiency and the port operation efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 Flowchart of the invention.

[0034] Figure 2Schematic diagram of multi-mode lane change path planning in the present invention, wherein (a) is the crab lane change mode, (b) is the S-type lane change mode, and (c) is the combination of the crab lane change and the S-type lane change mode.

[0035] Figure 3 Schematic diagram of lane changing path trajectory in the present invention. DETAILED DESCRIPTION

[0036] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings:

[0037] A multi-mode lane change path planning algorithm proposed in this embodiment mainly combines the four-wheel independent steering characteristics of the inner container flatbed transport vehicle, and includes the following steps:

[0038] Step 1: Obtain lane centerline and boundary information based on high-precision maps to construct reference lines.

[0039] The reference line can be composed of a series of points {P0, P1, ..., P n} description, each point can be expressed as P{x,y,θ,s,Bl,Br}, where x,y are position information, θ is heading, s is length, Bl is the distance to the left lane line, and Br is the distance to the right lane line. In this embodiment, the lane width is set to 3m, and the distance to the left and right boundaries is 1.5m. The closest matching point from the vehicle to the reference line is taken as the coordinate origin, and the point s=0. In the three-lane model, the center line distance between two adjacent lanes is 3m.

[0040] Step 2: Obtain surrounding obstacle information and calculate the lateral drivable space along the path direction.

[0041] Step 2.1, Obtain obstacle information detected by the laser radar. In this embodiment, a 400*1000 grid map is established with the vehicle as the center point of the grid map, and the grid map resolution is set to 0.2m, that is, 100m in front and behind the vehicle and 20m in left and right.

[0042] Step 2.2, along the direction of the reference line in step 1, that is, the frenet coordinate system constructed based on the reference line, the horizontal axis is the reference line length S, the vertical axis is the deviation from the reference line L, and the vertical sampling is performed with the grid map resolution of 0.2m as the sampling distance. The maximum vertical sampling value is 100m. Based on the interpolation algorithm, a series of sampling points with an s interval of 0.2m are calculated. For each sampling point, along the vertical direction, according to the grid map resolution of 0.2m, horizontal sampling is performed to the left and right respectively. The maximum horizontal sampling value is 20m. If the point is occupied by an obstacle, the horizontal distance is returned as the left and right boundary values ​​of the vertical sampling point (OBl, OBr), otherwise the horizontal boundary is set to the maximum value of 20m.

[0043] Step 2.3: Combining the information from step 1 and step 2.2, the drivable space can be described by a series of points P i (x i ,y i ,θ i ,s i ,ΔBl i ,ΔBr i ,ΔOBl i ,ΔOBr i ), where x i ,y i is the position information, θ i is the heading, s i is the length, ΔOBl i =OB1 i -W / 2,ΔOBr i =OBr i -W / 2 is a hard constraint for lateral offset, where the vehicle size is set to be 15m long and 2.8m wide.

[0044] Step 3: Based on the feasible space, design different lane-changing modes and decide on the optimal lane-changing method.

[0045] Step 3.1, design the scene for the working lane scene in the container area of ​​the port. The left side of the working lane is adjacent to the container area, with a lateral space of 0.5m. In front are other working waiting vehicles, with a following distance of 8m. The right lane is a borrowing lane without any obstacles, and long waiting can be avoided by changing lanes.

[0046] Based on the four-wheel independent steering characteristics of the flatbed container transporter in the port, different basic lane-changing methods can be designed, including:

[0047] Method 1: Crab-style lane change, that is, the front and rear axles turn to the same angle at the same time to achieve crab-style lane change; to reduce the change of the flatbed truck's posture and prevent risks such as tail swinging, such as Figure 2 (a) shown.

[0048] Method 2: S-shaped lane change, that is, the front and rear axles rotate in opposite directions to achieve S-shaped lane change, which can reduce the space demand on the road ahead and achieve short-distance lane change, such as Figure 2 (b) as shown.

[0049] Step 3.2: At this time, the minimum required front space for the crab lane change is 10m, the minimum required front space for the S-shaped lane change is 6m, and the minimum safety distance is 0.5m. f >s s +d safe , and the target lane change space satisfies: when changing lanes left, p i (ΔOBl i )>ρ cl, when changing lane right i (ΔOBr i )>ρ cl , but the side lane changing space is not satisfied: when changing lanes right, the space on the left side of the vehicle is less than the minimum required space of 1.5m, then you can design a crab lane change for a distance, combined with an S-shaped lane change, and finally achieve the lane change to the target lane, such as Figure 2 (c) as shown.

[0050] Step 4: Based on the decision result, a sampling algorithm is used to plan the lane change path.

[0051] A. The crab walking starting point C0(s0,ρ0) is the closest matching point C0(0,0) on the front reference line of the vehicle. Step 4.1, initialize the parameters, set the maximum crab walking angle to 17°, the sampling lateral granularity to 0.2m, and the sampling longitudinal granularity to 1m.

[0052] Set the crab start point C0(s0,ρ0) to the nearest matching point C0(0,0) on the front reference line.

[0053] Based on the maximum crab angle of 17°, different lateral distances Δs={0, 0.2, 0.4, 0.6, ..., 1.2, 1.4} are sampled, and the crab end point is C1(s1, ρ1)=(Δs, Δstan17°).

[0054] The S-shaped lane change end point C3(s3,ρ3) is the target lane sampling point, and the minimum distance from the start point of the S-shaped lane change trajectory must be greater than the minimum lane change distance of 6m. The S-shaped lane change trajectory can be expressed by a spline curve Control point P i It is {C2(s2,ρ2),P((s2+s3),ρ2),P((s2+s3),ρ3),C3(s3,ρ3)}.

[0055] Step 4.2, perform safety check on the calculated lane change trajectory. For any point on the trajectory, a BOX based on the vehicle model parameters can be obtained. The position relationship of all obstacles based on the BOX is calculated. If distance(box,obstacle)>0, then the lane change trajectory meets the collision-free requirement, such as Figure 3 shown.

[0056] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above specific embodiments. The above specific embodiments and the description in the specification are only for further illustrating the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of the present invention to be protected is defined by the claims and their equivalents.

Claims

1. A multi-mode lane change path planning algorithm, characterized by: The planning is based on the four-wheel independent steering characteristics of the flatbed container transporter in the port. The specific steps include: Step 1: Based on the existing high-precision map, the lane centerline and boundary information are obtained to construct a reference line; Step 2: Obtain information about surrounding obstacles; output point cloud information of obstacles through the laser radar. The obstacles are reflected in the form of whether they occupy grid units on a grid with a fixed resolution through the point cloud, and the drivable space along the path direction is calculated; Step 3: Based on the feasible space obtained in the previous steps, design different lane-changing modes and decide on the optimal lane-changing mode; Step 4 plans the lane change path using a sampling algorithm based on the decision result.

2. The multi-mode lane change path planning algorithm according to claim 1, characterized in that: In step 1, the lanes are at least two lanes, and the reference line greater than two lanes is described as {L middle ,L right or L left }, each reference line consists of a series of points {P0, P1, ..., P n }, each point is represented by P{x, y, θ, s, Bl, Br}, where x and y are position information, θ is the heading, s is the length, Bl is the distance to the left lane line, and Br is the distance to the right lane line.

3. The multi-mode lane change path planning algorithm according to claim 1, characterized in that: The specific content of step 2 is: Step 2.1, obtain the obstacle information described by other non-geometric polygons detected by the laser radar and establish an m*n size grid map; Step 2.2: along the reference line direction in step 1, perform longitudinal sampling with the grid map resolution established in the previous step as the sampling distance, calculate the sampling points based on the interpolation algorithm, and for each sampling point, perform horizontal sampling to the left and right along the vertical direction according to the grid map resolution. If the point is occupied by an obstacle, return the horizontal distance as the left and right boundary values ​​of the longitudinal sampling point (OBl, OBr); Step 2.3: Combining the information obtained in step 1 and step 2.2, the drivable space is described by a series of points P i (x i ,y i ,θ i ,s i ,ΔOBl i ,ΔOBr i ), where x i ,y i is the position information, θ i is the heading, s i is the length, ΔOBl i =OB1 i -W / 2,ΔOBr i =OBr i -W / 2 is a hard constraint for lateral offset.

4. The multi-mode lane change path planning algorithm according to claim 1, characterized in that: In step 3, the lane changing mode includes at least a crab lane changing mode, an S-shaped lane changing mode and a normal lane changing mode; When the front and rear axles of the flatbed truck turn to the same angle at the same time, the crab lane changing mode is used to reduce the change in the flatbed truck's posture; when the front and rear axles of the flatbed truck turn in opposite directions, the S-type lane changing mode is used to reduce the demand for road space ahead and achieve short-distance lane changing.

5. The multi-mode lane change path planning algorithm according to claim 3, characterized in that: According to the drivable space in step 2, calculate the distance s between the front and rear of the flatbed truck in the current lane and the obstacle. f ,s r , specifically: If the space ahead is greater than the minimum distance s of the crab lane change mode f >s c +d safe , and the target lane change space satisfies: that is, when changing lanes to the left, P i (ΔOBl i )>ρ cl , when changing lane right i (ΔOBr i )>ρ cl , you can directly use the crab lane change mode; If the space ahead is greater than the minimum distance s for S-turn lane change f >s s +d safe , and the target lane change space satisfies: that is, when changing lanes to the left, P i (ΔOBl i )>ρ cl , when changing lane right i (ΔOBr i )>ρ cl , and the lane-changing space on the side satisfies: that is, when changing lanes to the left, P i (ΔOBr i )>ρ min , when changing lane right i (ΔOBl i )>ρ min , you can directly use the S-shaped lane change mode; If the side lane-changing space does not meet the minimum drift requirement, the lane-changing can be performed by crab-moving for a certain distance, combined with the S-shaped lane-changing mode, to avoid the drift problem while reducing the forward lane-changing space requirement to achieve lane changing.

6. The multi-mode lane change path planning algorithm according to claim 5, characterized in that: When the side lane changing space does not meet the minimum tail-swinging requirement space, the lane changing is performed in a crab-like manner, and the feasibility of S-shaped lane changing is calculated in real time during the lane changing process. Once the lane changing requirements are met, the lane changing is switched to S-shaped lane changing.

7. The multi-mode lane change path planning algorithm according to claim 1, characterized in that: In step 4, the specific contents are as follows: Step 4.1, design lane change path generation strategy: Based on the reference line in step 1, perform horizontal and vertical sampling, define the longitudinal sampling length as Δs, and the horizontal sampling interval as Δρ, then the sampling point can be described as (s, ρ), and the sampling trajectory consists of two parts: the crab lane-changing trajectory, which contains two feature points: the crab starting point C0 (s0, ρ0) and the crab ending point C1 (s1, ρ1), and the crab trajectory can be expressed by a straight line; the S-shaped lane-changing trajectory, which contains two feature points: the starting point C2 (s2, ρ2) and the ending point C3 (s3, ρ3), and the S-shaped lane-changing trajectory can be expressed by a spline curve; Step 4.2, design the vehicle trajectory: Step 4.2.1, the crab walking starting point C0(s0,ρ0) is the closest matching point of the vehicle on the front reference line; Step 4.2.2, based on the maximum crab walking angle, sample different lateral distances to obtain the crab walking end point C1(s1,ρ1); Step 4.2.3, the starting point C2(s2,ρ2) of the S-shaped lane-changing trajectory is the end point of the crab move, that is, C1=C2; Step 4.2.4, the end point C3(s3,ρ3) of the S-shaped lane change trajectory is the target lane sampling point, and the minimum distance from the start point of the S-shaped lane change trajectory is greater than the minimum lane change distance. The spline curve expression of the S-shaped lane change trajectory is as follows: Step 4.3: perform safety verification on the lane change trajectory calculated in the previous step. For any point on the trajectory, a BOX based on the vehicle model parameters can be obtained, and the position relationship of all obstacles based on the BOX can be calculated; If the position relationship satisfies distance(box,obstacle)>0, the lane-changing trajectory meets the collision-free requirement.

Images