Path planning in autonomous driving is an upper-level module of the top-level navigation problem. It mainly solves the problem of moving the vehicle from the starting point to the end point through a series of lane-level guidance planning. The general path planning and control module is surrounded by a traffic prediction module, whose input is the perceived environmental information, including static environmental information such as lane lines, road curvature, road obstacles, traffic signs, etc., or road moving vehicle information ( Such as speed, acceleration, distance, etc.); while the output information obtained by path prediction includes time information and spatial information, the prediction can optimize the output of the perception module, or preprocess the input of the decision/planning module to the object.

Several high-frequency interview questions

The difference and connection between unmanned vehicle motion planning, path planning, and trajectory planning?

Path planning generally refers to the planning of driving routes, which does not involve the solution of time; trajectory planning should take time into account, that is, when and where the vehicle should be.

At present, the commonly used trajectory solution method is to decouple the path planning and velocity planning first, and finally combine them to form a trajectory.

Path planning gives the spatial sequence of vehicle positions (X(s) and Y(s), s is the mileage station), and trajectory planning gives the space-time sequence of vehicle positions (X(t) and Y(t)) . For the same path, passing at different speeds is actually a different trajectory for the vehicle. Planning the path first and then planning the velocity (called path-velocity decomposition) is a common method of trajectory planning, which is equivalent to writing X(t) and Y(t) as X(s), Y(s) and s(t ) parameter form. Of course, there are also some trajectory planning methods that directly plan X(t) and Y(t), such as methods based on MPC.

Path planning is a plan made on the premise of ignoring temporary or moving obstacles. It solves the problem of what route to take to reach the destination. It is the same as you open the map software and input the starting point and ending point to output the route. Trajectory planning is a planning made on the premise of considering temporary or moving obstacles. It solves the problem of how to follow the path as much as possible while avoiding obstacles. Follow the route but still need to look at the road, the car, the people and the traffic lights. The focus is on avoiding obstacles and obeying traffic regulations. Therefore, it is said that path planning comes first, and trajectory planning comes later. The two are the relationship between the past and the future. In terms of size, the path planning is a large size, and the trajectory planning is a small size. The map used for path planning is static, while the map used for trajectory planning is dynamic. They are all plans for the future based on the current state, and they can all be three-dimensional.

The scope of motion planning results is between two states. Because the motion planning problem of unmanned vehicles is usually how much time it takes to get to where, or just where to go, it is impossible to just how much time it takes. Therefore, the state can have no time but not no space, and the path and trajectory can contain no time information, but only spatial information, which means that speed planning is not necessary. However, in a scene with moving obstacles, the unmanned vehicle must have a specific speed in a specific state to avoid obstacles, so there is a problem of trajectory pacing.

Then answer why some papers do not have path planning. It may be because it is motion planning in small enclosed spaces such as indoor robots and park robots, or other scenes that lack moving obstacles, so the trajectory is approximately equal to the path, and there is no distinction between the two.

The automatic control process of unmanned vehicles is as follows, path planning→trajectory planning→trajectory following. Path planning is mainly based on map information. V2X may be added in the future, and the technology is mature. Trajectory following belongs to the field of traditional vehicle automatic control, and the control targets are vehicle speed and heading, so it can be divided into longitudinal control and lateral control. Trajectory planning is based on a variety of information sources, including sensors, high-precision maps, V2V, etc. Currently, sensors are the mainstay, and there are many technical difficulties, which are the focus of research. There are three directions.

1. Manually design rules, relying on engineers to conduct a large number of experiments to summarize experience and then compile them into codes. Not many people do this anymore, but this method is still widely used as a patching solution. The characteristic is that it has a miraculous effect on specific road conditions.
2. Modeling. Engineers design various mathematical models to describe obstacle avoidance problems, such as artificial potential field method, grid method and so on. But the mathematical model is often not satisfactory, because the sensor is often inaccurate, so the manual design rules mentioned above are needed to patch.
3. Deep learning. If there is no precise sensor information to build a mathematical model, then letting the machine learn actively is also an approach. Here are two more options.

One is supervised learning, which allows machines to learn from human driving experience. Engineers collect a large number of data from people driving cars for machines to learn. The difficulty lies in how to collect enough real data. For special vehicles, such as takeaway unmanned vehicles, there may be no way to get real data.

One is reinforcement learning, which allows machines to learn autonomously in real or simulated environments without the involvement of engineers. The difficulty is that learning in a real environment may either wreck the car or wreck other things. In the simulation environment, the scenes and sensors are not so real, which greatly affects the results of reinforcement learning.

Algorithm principle

Path Planning Algorithm Principle

1. Establish trajectory planning coordinate system

First of all, it is to establish a reliable coordinate system for the ego vehicle. Generally, there are two ways to establish the coordinate system. 1) The relatively simple and commonly used coordinate system is the XYZ three-direction. This coordinate system adopts the setting of the American Society of Motor Vehicle Engineers (SAE). The front is positive, the two sides of the vehicle's driving direction are the y-axis, and the right is positive, and the up-down direction of the vehicle's driving direction is the z-axis, and the upward direction is positive.

2) The Frenet coordinate system is a well-understood coordinate establishment method for planning control. This coordinate system uses the road centerline as a reference line, and uses the tangent vector and normal vector of the reference line to establish a coordinate system. The Frenet–Serret formula is used to describe the particles in the The kinematics along a continuously differentiable curve in the three-dimensional Euclidean space R3, this coordinate system is widely used in the planning module.

2. Establish initial planning trajectory

The path planning process includes two levels. One is to establish the corresponding initial motion trajectory planning based on the initial activation state of driving automatic driving, which includes the following steps.

1) Transform the vehicle coordinate system with reference to the coordinate system defined and selected above for the vehicle position and reference line;

2) Select the target point by fixed sampling according to the set rules;

3) Use curve interpolation or fitting methods to generate candidate trajectories;

4) Carry out expansion calculations on the candidate trajectories to eliminate possibly unreasonable trajectories;

5) Select the optimal trajectory based on cost functions such as smoothing of the control quantity or minimum deviation;

The above trajectory planning process will be introduced one by one as follows.

(1) Graph search basic path:

The continuous environmental information recognized by the sensor is generally an analog signal, which needs to be converted into a discrete graph suitable for the selected path planning algorithm by using a certain graph search path planning algorithm first, and then use a certain search algorithm to obtain the basic path. Generally, search algorithms for video graphs include the following:

Dijkstra algorithm: Set the initial node set, start from the initial point where the target car is located, iteratively check all nodes in the node set, and add the unchecked node closest to the node to the node set to be checked. The node set expands outward from the initial node until it reaches the target node.

A* algorithm: set the cost function g(n) to represent the cost from the initial node to node n, and h(n) represents the heuristic evaluation cost from node n to the target point. Combining information blocks of nodes close to the initial point and nodes close to the goal point. When moving from the initial point to the goal point, A* balances the two to find the optimal path.

(2) Sampling point generation path

Sampling point generation is actually to generate sample points for the self-vehicle in a set configuration space, and find a sequence of sample points that meet the task requirements as the planning result; sampling methods are divided into random sampling and fixed sampling. Random sampling refers to the method of generating random sample points in the configuration space. This method randomly selects N nodes in the planning space, then connects each node, and removes the connection with the obstacle, thus obtaining a feasible path. Fixed sampling is to generate a series of sample points to be selected according to clearly given rules, and select the best quality sample through screening. A typical application is to use the whisker algorithm to plan the local trajectory of the vehicle. The whisker process is to try a variety of direction control schemes tentatively, and select the ideal driving path by establishing an evaluation index that comprehensively considers the feasibility, smoothness, and safety performance of the movement. Dangerous or unusable path.

3. Track curve interpolation

In general, for regular roads (that is, there is no sudden change in factors such as road width or curvature), no matter for centering control or lane change, the vehicle trajectory curve is often consistent with the road trajectory curve, as shown in the figure below. The type of lane described by the trajectory curve. Using the arc corresponding to the turning radius of the lane to determine the driving path can be completely adapted to the vehicle driving path. The driving path is explicitly expressed as a mathematical function, and the specific shape of the driving path is determined by determining the parameters of the function.

ü Combination method of curve elements:

(1) Dubins curve;

(2) Reeds-Shepp curve

(3) Combination of Clothoid curve (clothoid curve) and arc segment;

Taking the Rees-Shepp curve as an example, the curve is composed of several arcs with a fixed radius representing the minimum curve radius of the car turning and a straight line representing the straight lane. It is assumed that the vehicle turns with a fixed radius, and the vehicle can move forward and Backward, then the Reeds-Sheep curve is the shortest path from the starting point to the ending point of the vehicle under the above conditions. This curve can not only represent the end point that can be reached, but also ensure that the angle of the vehicle can reach the expected angle at the end point. The Dubins curve and The Reeds-Sheep curve is set in a similar way, but Dubins has an additional constraint that causes the car to only drive forward and cannot go backward (that is, it cannot be reversed).

As far as a few relatively simple driving scenarios are concerned, the method of combining curve elements can fully meet the fitting requirements. However, in the case of obstacles, the above curve no longer meets the requirements. At present, in the method of obstacle detection for path planning, the combination of clothoid curve and arc segment is a relatively conventional method. For example, in automatic lane changing, the trajectory curve prediction considered by this method needs to first consider the road curvature in the driving scene, and then plan the centering curve of the road in a limited state, and then use the actual distance between the self lane and the target lane The situation predicts the lane change curve. Finally, the seamless connection and combination of the two forms the final trajectory curve. Of course, the arc segment in the actual process can use either the simplest parabola or a relatively simple low-order clothoid. The following figure clearly shows the combined representation of the clothoid curve and the arc curve. In addition to driving path planning, true autonomous driving must also consider the planning of the actual driving path of the vehicle, that is, how the autonomous driving vehicle moves forward along the path we plan. This requires us to have certain curve interpolation fitting methods, and the corresponding curve fitting methods are listed below.

(1) Cubic B-spline curve

(2) Based on the fourth-order Bezier curve

(3) Using a quintic polynomial

For the development of autonomous driving, the above methods of curve interpolation and fitting each have their own advantages and disadvantages, but for different occasions, they need to be considered in terms of fitness, computing power and other related factors. In general, the discrete track reference points generated for the detection of environmental information can be fitted with cubic B-spline curves, the actual feasible trajectory planning of autonomous vehicles can be fitted with fourth-order Bezier curves, and the turns at complex intersections The driving path or the driving path of normal lane change can be fitted by a polynomial of degree 5. The following will use specific examples to explain the complete planning of driving routes based on the combination of sampling and curve interpolation, in which the curve fitting uses a 4th-order Bezier curve. For a given discrete point t, x(y), y(t) can be obtained respectively, that is, the horizontal and vertical coordinates of the position of the car, then the parametric expression of the trajectory represented by the fourth-order Bezier curve is expressed as follows, where P0 Represents the starting point of the curve, P1-P4 represents the control point of the curve (which is represented by the control point parameters d1, d2, d3), the four curve control points affect the degree of distortion (or movement trend) of the formed curve:

Taking the automatic lane change trajectory curve as an example, in order to solve all the parameters in the above equation, the prediction process needs to be set as the initial state and the end state, as one of the solution sets of the above equation. Among them, the initial state is expressed as the current position state of the vehicle, and the end state is the end point information predicted according to the current state and the actual lane line position.

As shown in the figure above, there are only two points P0 and P4 for known points. Thus, the curve passing through the starting point and the ending point are respectively expressed as;

In order to solve the P3 point, it is necessary to set the vehicle target position constraint P4=(xT, yT, ΨT, kT), d4=|P3P4|, and in order to satisfy the heading angle constraint of the target state, the coordinates of the third control point are

The above derivation process has satisfied the initial state and target state constraints of the unmanned vehicle (that is, the fourth-order Bezier curve start point, end point, and three control point coordinates), and the fourth-order Bezier curve is calculated. For the free variables The control parameter p=[d1, d4, x2] can be adjusted to change the direction or degree of distortion of the generated 4th-order Bezier curve.

Vehicle path planning following principle

When the vehicle path is formed in the plan, it is necessary to determine how the vehicle moves along the path to reach the final point. Here we call the vehicle driving plan motion planning, which includes two levels of motion planning:

1) Based on horizontal path planning, horizontal planning generally refers to trajectory shape planning;

After the horizontal planning has been determined, the horizontal planning can be converted into a QP problem, that is, the corresponding optimization result curve can be output for the constraints within the current given range. For the QP problem, it is actually by setting the known conditions that have been detected before, and then through a certain filtering algorithm (such as Kalman filtering) to solve the relevant parameters in the future, and the constraints can be set to continuously optimize the corresponding curve settings. Known conditions: x[t-2], x[t-1], x[t]; Solving: x[1], x[2], ... Constraints: each point represented by x[i] cannot exceed The left and right boundaries, and when a certain obstacle vehicle target is detected, the distance between x[i] and the target should be greater than the safe distance Dsafe. Therefore, the corresponding QP formula and its optimization objective are set as follows:

2) Based on the longitudinal path planning, the longitudinal planning is to allocate the speed in the trajectory planning;

motion planning

When the virtual person starts a roaming, firstly, the global planner performs global static planning based on the existing long-term information to determine the optimal route that the virtual person should go through. The global planner then controls the execution system to move along this path. During the movement, the perception system will continue to perceive the surrounding environment. When dynamic objects or unknown obstacles are detected, the local planner determines short-term motion based on these perceived local information. When the priority of obstacle avoidance is higher than that of advancing along the original path, the local planner can obtain the control right of the execution system through competition, so that the virtual human can move according to the local planning result. After completing the avoidance behavior of the current perception obstacle, the global planner takes control of the execution system again, so that the virtual human returns to the global planning path and continues to move towards the target point.

What is Motion Planning

Motion Planning is a method of navigating an autonomous vehicle from its current location to its destination while following road traffic rules.

In the actual open principle scenario, the scenarios that autonomous driving has to deal with are very complicated: empty road scenes, scenes that share the same logic with pedestrians and obstacles, empty intersections, busy intersections, pedestrians/vehicles violating traffic rules, normal Moving vehicles/pedestrians etc. Although the scene is complex, it can be disassembled into a series of simple behaviors (behavior):

Combining these simple behaviors, complex driving behaviors can be completed.

Motion Planning constraints (constraints)

Motion Planning is a complex problem, and its execution process needs to meet many constraints:

Vehicle Kinematic Constraints

Vehicle movement is subject to kinematic constraints. For example, it cannot achieve instantaneous sideways movement. Front-drive vehicles must rely on the steering of the front wheels to achieve lane change, steering, etc., and cannot be too fast on curves, etc. Usually we use the bicycle model (Bicycle Model) to model the vehicle movement.

Static Obstacle constraints

Static Obstacle (Static Obstacle) is the area where vehicles such as stationary vehicles on the road and stone piers in the middle of the road cannot drive. Motion Planning needs to avoid these static obstacles and avoid collisions with them. There are roughly two ways to solve collisions:

1) Express the static obstacle (Static Obstacle) in the grid occupancy map, and then detect whether the planned route intersects with the static obstacle area.

2) Expand the outline of the vehicle, for example, into a circle, and then detect whether the obstacle collides with the Circle.

Dynamic Obstacle Constraints

Motion Planning needs to deal with various moving obstacles such as pedestrians and vehicles in real time to avoid collision accidents with obstacles.

road traffic rules

Vehicles driving on the road must abide by the lane line restriction rules (such as left-turn lanes can only turn left, solid lines cannot change lanes, and intersections must obey the instructions of traffic lights) and the instructions of various signs and signs.

Motion Planning optimization goals

After understanding the constraints of Motion Planning, it is necessary to construct an objective optimization function, and then minimize the objective function to obtain the optimal motion trajectory in the current environment. There are many types of objective functions, some commonly used objective functions are enumerated below.

1) Pay attention to the path length (Path Length) and seek the shortest path to the destination.

By combining and designing your own objective optimization function, you can get a better Planning effect.

Hierarchical Motion Planning

Motion Planning is an extremely complex problem, so we usually divide it into a series of sub-problems (Sub Problem). Such as Mission Planner, Behavior Planner, Local Planner, Vehicle Control, etc.

Mission Planner

Mission Planner focuses on High-Level map-level planning; through the Graph Based graph search algorithm, the planning of automatic driving paths is realized.

Behavior Planner

Behavior Planner mainly focuses on traffic rules, other road traffic participants (bicycles, pedestrians, social vehicles), etc., and decides what actions should be taken in the current scene (such as parking to give way, speeding up, avoiding pedestrians, etc.).

There are several common implementations of Behavior Planner: Finite State Machines, Rule Based System, and Reinforcement Learning.

The State in the finite state machine is each behavioral decision, which switches between states according to the perception of the external environment and the constraints of traffic rules. For example, in the scene of traffic lights at the intersection, when the traffic light at the intersection is red and impassable, the vehicle will first switch to the Decelerate to Stop state, then completely stop at the intersection stop line, enter the Stop state, and continue to wait in the Stop state until the traffic light Turning green to allow vehicles to pass, the vehicle enters the Track Speed state and continues to move forward.

The Rule-Based System determines the next decision-making behavior through a series of hierarchical rule matching. For example, the traffic light is green -> pass; the traffic light is red -> stop and wait. The Behavior Planner system based on reinforcement learning is as follows:

Local Planner

Local Planner focuses on how to generate a comfortable, collision-avoiding driving path and comfortable movement speed, so Local Planner can be split into two sub-problems: Path Planner and Velocity Profile Generation . Path Planner is divided into Sampling-Based Planner, Variational Planner and Lattice Planner.

The most classic Sampling-Based Planner algorithm is Rapidly Exploring Random Tree, RRT algorithm.

Lattice Planner limits the space search to the Action Space that is feasible for the vehicle. Velocity Profile Generation should take into account speed limit, speed smoothness, etc.

Vehicle Control converts Planner's planning results into vehicle motion behavior.

Best First Search (BFS) Algorithm

BFS operates in a similar fashion, except that it can estimate (called heuristics) the cost of any node to the goal. Instead of choosing the closest node to the initial node, it chooses the closest node to the goal. BFS cannot guarantee to find a shortest path. However, it is much faster than Dijkstra's algorithm because it uses a heuristic function (heuristic function) to quickly guide the target node. For example, if the goal is south of the starting point, BFS will tend towards paths leading south. In the graph below, yellower nodes represent higher heuristic values (higher cost of moving to the goal), while darker nodes represent lower heuristic values (lower cost of moving to the goal). This shows that BFS runs faster compared to Dijkstra's algorithm.

The A* algorithm combines the respective advantages of Dijkstra and BFS, and combines the information blocks of the Dijkstra algorithm (nodes close to the initial point) and the BFS algorithm (nodes close to the target point).

Random Roadmap PRM

It is a method based on graph search. Probabilistic Road Maps (PRM) is to randomly select N nodes in the planning space, then connect each node, and remove the connection with obstacles, thus obtaining a random road map. .

Obviously, when the sampling points are too few, or the distribution is unreasonable, the PRM algorithm is not complete, but with the increase of the sampling points, it can also reach completeness. So PRM is probabilistically complete and not optimal.

Rapidly Extended Random Tree RRT

It is a search algorithm based on a tree structure. The RRT algorithm expands a tree structure outward from the starting point, and the expansion direction of the tree structure is determined by randomly picking points in the planning space. Similar to PRM, this method is probabilistically complete and not optimal.

Rapidly Extended Random Tree RRT

Although sampling-based planning algorithms (such as PRM and RRT) are fast, they also have a fatal shortcoming, which is the randomness introduced by random sampling. Using RRT and PRM algorithms for motion planning, users cannot predict the planning results, and the results of each planning are different, which makes automatic planning robots unable to enter the industrial field (extreme pursuit of stability).
Therefore, the current planning field is mainly focused on the improvement of PRM and RRT . Everyone wants to solve the uncertainty of these algorithms as much as possible, and even achieve some optimization goals, such as RRT , Informed-RRT , SBL, etc.

Path Planning in Autonomous Mobile Robots

The first step is to transform the possible continuous environment model into a discrete graph suitable for the selected path planning algorithm. There are three general strategies: road graph, unit decomposition, and potential field.

road map

visibility map

Consists of lines connecting all pairs of vertices that are visible to each other, connecting these unobstructed vertices is the shortest distance between them.

This method is only suitable for sparse target groups and allows the robot to get as close to obstacles as possible.
A Voronoi diagram
tends to maximize the distance between the robot and obstacles in the diagram relative to the visualization diagram.

It also maximizes the distance between the robot and objects in the environment, making possible hazards invisible to short-range sensors on the robot.

Unit Decomposition Path Planning

The main idea is to distinguish between geometric regions (also called cells), that is, to distinguish cells into free and object-occupied regions.
Mainly divided into exact element decomposition and
Precise unit decomposition: based on the following idea: in each unit of free space, the specific position of the robot is not important, what is important is the ability of the robot to move from each free unit to its adjacent free unit.

In large sparse environments, where the number of units is small, the implementation works quite well. But once the number of units is huge, the difficulty of implementation will increase dramatically.
Approximate cell decomposition
The size of cells does not depend on special objects in the environment, and the computational complexity of path planning is low. is the generality of stack-based environment representations.

Potential Field Path Planning

The main idea: treat the robot as a point under the influence of the artificial potential field, like a ball rolling down a hill, the robot moves with the field. The robot is attracted towards the goal while also being repelled by previously known obstacles.
If new obstacles appear, the potential field should be updated in time.

The basic potential field includes the potential field with a certain gradient from the starting point to the target and the repulsive potential field centered on the obstacle.

Extended Potential Field Method

On the basic potential field, two fields are added: rotation potential field and task potential field.

The 3D problem can be decomposed into two separate 2D problems, which is solved by separating the longitudinal and transverse components of the trajectory. One of the 2D trajectories is the longitudinal trajectories with time stamps called ST trajectories, and the other 2D trajectories are lateral offsets relative to the longitudinal trajectories called SL trajectories.

Rotational potential field: When the obstacle is parallel to the direction in which the robot is walking, the repulsive force is reduced, because such an object will not pose a timely threat to the trajectory of the robot. The result is enhanced tracking along walls.
Task potential field: The current robot speed is considered, and obstacles that have no effect on the robot speed according to the recent potential energy are excluded. The result is a smoother trajectory through space.
planning

route plan

path and speed

Trajectory planning is divided into two steps: path planning and speed planning.

Candidate curves are first generated in the path planning step, which is the path that the vehicle can drive. Each path is evaluated using a cost function that incorporates smoothness, safety, deviation from lane center, and any other factors the developer wants to take into account. The paths are then ranked by cost and the path with the lowest cost is chosen.

Then there is the determination of the speed to travel along this route. We may wish to vary the velocity along that path, so what really needs to be selected is a range of velocities relative to the waypoint, rather than a single velocity. We call this sequence a "velocity profile". We can use the optimization function to choose a good velocity profile subject to various constraints for the path. Vehicle trajectories are constructed by combining paths and velocity profiles.

path

To generate candidate paths, road segments are first segmented into cells.

The points in these cells are then randomly sampled. By taking a point from each cell and connecting the points, we create candidate paths.

Multiple candidate paths can be constructed by repeating this process. These paths are evaluated and the path with the lowest cost is selected using a cost function that may take into account: deviation from the center of the lane, distance from obstacles, changes in speed and curvature, stress on the vehicle, or desired inclusion any other factors.

ST diagram

The next step after selecting a path is to select the velocity profile associated with that path, a tool called an "ST map" can assist in the design and selection of the velocity profile.

In the ST diagram, "s" represents the longitudinal displacement of the vehicle, and "t" represents time.

The curves on the ST diagram are a description of the vehicle's motion because it illustrates the vehicle's position at different times.

Since velocity is the rate of change of position, velocity can be inferred from an ST graph by looking at the slope of the curve. Steeper slopes represent greater movement in a shorter time period, corresponding to faster speeds.

speed planning

In order to construct the optimal velocity curve, it is necessary to discretize the ST map into multiple cells.

The velocity varies between cells but remains constant within each cell, which simplifies the construction of velocity curves and maintains the approximation of the curves.

Obstacles can be drawn in ST diagrams as rectangles that block certain parts of the road for a certain period of time. For example, suppose the prediction module predicts which lane a vehicle will drive into during the time period t0 to t1. Since the car will occupy positions s0 to s1 during this period, a rectangle is drawn on the ST map that will block positions s0 to s1 during time period t0 to t1. To avoid collisions, the velocity curve must not intersect this rectangle. Now that we have an ST graph with various cells blocked, an optimization engine can be used to select the best velocity curve for the graph. Optimization algorithms use complex mathematical operations to search for low-cost solutions subject to various constraints. These constraints may include: legal constraints, such as speed limits; distance constraints, such as distance from obstacles; and physical constraints of the car, such as acceleration limits.

optimization

Path-velocity planning depends heavily on discretization.

Path selection involves dividing the road into cells, and speed profile construction involves dividing the ST map into cells.

Although discretization makes these problems easier to solve, the solution generates trajectories that are not smooth

To convert a discrete solution to a smooth trajectory, the technique of "Quadratic Programming" can be used.
.
Quadratic programming fits smooth nonlinear curves to these piecewise linear segments.

Although the mathematics behind quadratic programming is complex, the details are not necessary for our purposes.

We simply use one of several different optimization packages, including a running scheme introduced by Apollo to generate smooth trajectories, which can then be used to construct 3D trajectories once the path and velocity profiles are in place.

Trajectory Generation for Path-Velocity Planning

Suppose we are driving on the road and the perception system observes that a slow-moving vehicle is getting closer to us.

First, multiple candidate routes are generated around the car, these candidate routes are evaluated using a cost function and the route with the lowest cost is selected. Then the ST map is used for speed planning, and part of the ST map is blocked according to the position of other vehicles over time.

An optimization engine helps determine the optimal velocity profile for this graph, subject to constraints and a cost function.

We can use quadratic programming to smooth the path and velocity curves.

Finally, the trajectory is constructed by merging the path and velocity curves. Here the trajectory is red when faster and blue when slower. We use this trajectory to safely avoid other vehicles and continue our journey.

Lattice planning

The environment can be projected onto the vertical and horizontal axes by using Frenet coordinates, with the goal of generating 3D trajectories:
vertical dimension
horizontal dimension
time dimension.

Lattice planning has two steps that first establish ST and SL trajectories separately, and then combine them to generate longitudinal and transverse two-dimensional trajectories.

The initial vehicle state is first projected into the ST coordinate system and the SL coordinate system by sampling multiple candidate final states in a preselected pattern. to select the final vehicle state. For each candidate final state a set of trajectories is constructed to transition the vehicle from its initial state to the final state, these trajectories are evaluated using a cost function and the lowest cost trajectory is selected.

Termination states of ST trajectories

The states can be divided into 3 groups according to the situation:

cruise
follow
stop

Cruising means that the vehicle will travel at a constant speed after completing the planning steps, actually sampling points on the graph, where the horizontal axis represents time and the vertical axis represents speed. For a point on this graph, this means that the car will go into cruise, cruising at time t with speed s, and for this mode, all final states will have zero acceleration

Following a vehicle, in this case sampling the position and time state, and trying to appear behind a certain vehicle at time t, when following a vehicle, you need to keep a safe distance from the vehicle in front, at this time the speed and acceleration will depend on Depending on the vehicle to follow, this means that in this mode both speed and acceleration are corrected.

The last mode is stop, for this mode it is only necessary to sample when and where the car stops, here speed and acceleration are corrected to 0.

Termination state of SL trajectory

No matter what end state the vehicle enters, the vehicle should be stably aligned with the centerline of the lane.

This means that only a small area needs to be sampled for the lateral termination position.

Specifically, the locations around the centerlines of adjacent lanes on the road are sampled. To ensure stability, the end state of the car's heading should coincide with the center of the lane.

When plotting lateral position versus longitudinal position, the desired candidate trajectory should end up with the vehicle aligned with the lane and driving straight. To achieve this terminal state, both the first and second derivatives of the car's orientation and position should be zero. This means that the vehicle is neither moving laterally, which is the first derivative, nor accelerating laterally, which is the second derivative. This means the vehicle is going straight along the lane.

Trajectory Generation for Lattice Planning

Once you have both ST and SL trajectories, you need to retransform them into Cartesian coordinates.

They can then be combined to construct a 3D trajectory consisting of 2D waypoints and 1D timestamps. The ST trajectory is the longitudinal displacement over time, and the SL trajectory is the lateral offset of each point on the longitudinal trajectory. Since both trajectories have an ordinate S, the trajectories can be merged by matching their S values.

Applications

Autonomous driving path planning - environment configuration

1. Update the mirror source
First, update the mirror source , such as Tsinghua source of ubuntu20.04

2. Install gcc, g++
Here, we need to add another software source to install gcc, g++. What I am talking about here is the gcc and g++ installation of ubuntu20. It seems that there are no versions 9 and 10 for 18. I remember that it is 7. But you can install these two compilers yourself, and Baidu yourself.

sudo gedit /etc/apt/sources.list

add

deb http://cn.archive.ubuntu.com/ubuntu xenial main

Note that this source is from ubuntu20, which may be different from 18.
implement

sudo apt-get update

Second, install gcc and g++:

sudo apt-get install gcc-9 g++-9

sudo apt-get install gcc-10 g++-10

3. Install ros and python3 environment
There are too many online tutorials, you can install the corresponding ros version according to your own system. Mine is ubuntu20.04, and ros-noetic is installed. It is recommended not to use ubuntu16, because I have not tried it, use 18 or 20, that is, melodic and noetic.

4. Install the osqp solver
Run install_osqp.sh in /src/planning/installers

sudo ./install_osqp.sh

5. Install dependent libraries: ADOL-C
first follow method 1 (there may be a version mismatch problem), if an error is reported or there is a problem with the operation, then uninstall and use method 2 again. Method 1
:
Install Colpack first

git clone http://github.com/CSCsw/ColPack.git
cd ColPack/build/automake
autoreconf -vif
mkdir build
cd build
../configure --prefix=usr/local
make -j
sudo make install

Install ADOL-c again

git clone http://github.com/coin-or/ADOl-C.git
./configure prefix=usr/local
make -j
sudo make install

Method 2:
Run install_adolc.sh in /src/installers
to open the install_adolc.sh file, first modify the path marked in red as shown in the figure below to be the absolute path downloaded by your computer ADOL-C-2.6.3. Otherwise lib64 will not be found.

Then execute:

sudo ./install_adolc.sh

6. Install the dependent library: ipopt solver

Recently, it was found that the download link of the library that comes with ipopt is invalid, so the installation of this package is more troublesome.

7. Install matplotcpp
Run install-matplotcpp.sh in /src/installers

sudo ./install-matplotcpp.sh

8. Install other ros package dependencies
Run install_base.sh in /src/installers

sudo ./install_base.sh

Note that there is ros-noetic in it, which is for the ros-noetic version, and the melodic version should be changed to ros-melodic.

9. Install Qt5 (optional)
Reference: https://blog.csdn.net/Joker__123/article/details/122438138

10. Install the qpoases solver

sudo ./install_qp_oases.sh

11. Install yaml read

sudo ./install_yaml.sh

Error solution

Question 1

CMake Error at /opt/ros/noetic/share/catkin/cmake/empy.cmake:30 (message):
  Unable to find either executable 'empy' or Python module 'em'...  try
  installing the package 'python3-empy'

Note that this problem may occur because I installed the conda environment and compiled it under base . The solution:

pip install empy

Question 2

ImportError: "from catkin_pkg.package import parse_package" failed: No module named 'catkin_pkg'
Make sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.
CMake Error at /opt/ros/noetic/share/catkin/cmake/safe_execute_process.cmake:11 (message):
  execute_process(/usr/local/anaconda3/bin/python3
  "/opt/ros/noetic/share/catkin/cmake/parse_package_xml.py"
  "/opt/ros/noetic/share/catkin/cmake/../package.xml"
  "/home/cg/Automatic_driving/build/catkin_tools_prebuild/catkin/catkin_generated/version/package.cmake")
  returned error code 1

Note that this problem may occur because I installed the conda environment and compiled it under base . The solution:

pip install catkin_pkg

Again

catkin build

Question 3

/usr/bin/ld: /lib/x86_64-linux-gnu/libapr-1.so.0: undefined reference to `uuid_generate@UUID_1.0'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/ui_console.dir/build.make:150：/home/cg/Automatic_driving/devel/.private/ui_console/lib/ui_console/ui_console] 错误 1
make[1]: *** [CMakeFiles/Makefile2:166：CMakeFiles/ui_console.dir/all] 错误 2
make: *** [Makefile:141：all] 错误 2

The reason for this error is because of the conda environment. QT refers to the qt in anaconda, because I use qt to draw pictures in ui_console. Of course, you can delete this folder to solve it. Solution:

Go to anaconda/lib under the anaconda installation folder:

sudo mkdir libuuid_bk
mv libuuid* libuuid_bk/

Question 4

Traceback (most recent call last):
  File "/opt/ros/noetic/lib/joint_state_publisher/joint_state_publisher", line 35, in <module>
    import rospy
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/__init__.py", line 49, in <module>
    from .client import spin, myargv, init_node, \
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/client.py", line 52, in <module>
    import roslib
  File "/opt/ros/noetic/lib/python3/dist-packages/roslib/__init__.py", line 50, in <module>
    from roslib.launcher import load_manifest  # noqa: F401
  File "/opt/ros/noetic/lib/python3/dist-packages/roslib/launcher.py", line 42, in <module>
    import rospkg
ModuleNotFoundError: No module named 'rospkg'

The reason for this error is still because of the conda environment, the solution:

pip install rospkg

Question 5

The link library cannot be found, whether it is ladolc, libqpOASES or other l or lib. Solution:
a. Open **/etc/ld.so.conf** file:

sudo gedit /etc/ld.so.conf

b. Add the directory where the dynamic library file is located: add under include ld.so.conf.d/*.conf

/usr/local/lib

c. After saving, execute on the command line terminal:

sudo ldconfig

chmod o+x ./xxx.sh
and then execute

sudo ./xxx.sh

Intensive lecture on practical application cases of MATLAB algorithm - [autonomous driving] path planning

foreword

Several high-frequency interview questions

The difference and connection between unmanned vehicle motion planning, path planning, and trajectory planning?

Algorithm principle

Path Planning Algorithm Principle

1. Establish trajectory planning coordinate system

2. Establish initial planning trajectory

3. Track curve interpolation

Vehicle path planning following principle

motion planning

What is Motion Planning

Motion Planning constraints (constraints)

Vehicle Kinematic Constraints

Static Obstacle constraints

Dynamic Obstacle Constraints

road traffic rules

Motion Planning optimization goals

Hierarchical Motion Planning

Mission Planner

Behavior Planner

Local Planner

Best First Search (BFS) Algorithm

Random Roadmap PRM

Rapidly Extended Random Tree RRT

Rapidly Extended Random Tree RRT

Path Planning in Autonomous Mobile Robots

road map

Unit Decomposition Path Planning

Potential Field Path Planning

Extended Potential Field Method

route plan

path and speed

path

ST diagram

speed planning

optimization

Trajectory Generation for Path-Velocity Planning

Lattice planning

Termination states of ST trajectories

Termination state of SL trajectory

Trajectory Generation for Lattice Planning

Applications

Autonomous driving path planning - environment configuration

Error solution

Question 1

Question 2

Question 3

Question 4

Question 5

Guess you like