The graph model used for path search established by algorithms such as A* and RRT does not consider the dynamic constraints of the robot. The path between two adjacent nodes of the graph model constructed may be infeasible or difficult to be determined. Robot tracking, now we want to make the connection between two adjacent nodes in the graph a feasible connection that satisfies dynamic constraints, so how to build such a graph?
There are usually two methods to construct such a graph: the first is based on control space sampling (forward method) , discretizing the robot's control space, and then driving the robot to move forward to obtain trajectories one by one, such as the commonly used DWA Algorithm, the second one is based on state space sampling (reverse method) , discretizing the robot's state space to generate many states, and then looking for connections from one state to another, such as the commonly used TEB algorithm
The algorithms such as A* and RRT introduced earlier can also be regarded as discretization of the state space when searching or sampling to construct a graph. Now we establish a state equation describing the robot's motion s˙ = f ( s , u ) \dot { s}=f(s,u)s˙=f(s,u ) , where s represents all possible states, such as position information x, y, speed informationx ˙ , y ˙ \color{red}{\dot{x}}, {\dot{y}}x˙、y˙etc., u represents the control input. Of course we also need to know the initial state of the robot s 0 s_0s0
Based on the current state of the robot, when we give different input u, different motion trajectories will naturally occur, as shown in the left figure below. The advantage of this control space-based sampling (forward method) is that it is simple and easy to implement. Its disadvantage is that it is not task-oriented and does not care where the target point is. Although it can produce better planning results, the planning efficiency is low. ; If the current state of the robot and the state of the target point (such as position information, attitude information) are known, how to solve the middle path (edge) from these two states, and obtain the control variable u at time T, as shown in the right As shown in the figure, this is sampling based on state space (reverse method). Its advantage is that it is very task-oriented, but the difficulty is that it is difficult to implement.
In the example below, if the state of the drone is selected as three-dimensional position and three-dimensional velocity, and the input is selected as three-dimensional acceleration, the state equation s˙ = A ⋅ s + B ⋅ u \dot{s}=A\cdot s+ is constructed. B\cdot us˙=A⋅s+B⋅u , using different input values, the trajectory obtained based on the sampling of the control space is shown in the right figure below
–
If the state is selected as three-dimensional position, three-dimensional velocity, and three-dimensional acceleration, and the input is selected as three-dimensional jerk, different input values will be used. The trajectory obtained based on the sampling of the control space is as shown in the right figure below:
From the state equation s ˙ = A ⋅ s + B ⋅ u \dot{s}=A\cdot s+B\cdot us˙=A⋅s+B⋅u , the specific state transition equation can be solved as follows. If A is a nilpotent matrix, thenA m A^mAAfter mth power, they are all 0, so that e A te^{At}can be easily calculated through Taylor expansion.eA t item.
With the above foundation, let's officially start the introduction of the State Lattice Planner. Given an initial state, given different control inputs, and simulated for a period of time, you can get a series of arrivals at different end positions. Trajectory of (The picture on the left divides u into 9 types, and the picture on the right divides u into 25 types):
It should be noted that when the lattice graph is constructed online, it is not necessary to continuously expand outward in all directions as shown in the figure above. We can use heuristic information to prioritize expansion in the direction of the target, which can reduce time and improve efficiency. Different from the raster map built in advance, the lattice graph is constructed while searching, that is, only the required nodes are constructed, which reduces calculation time and storage space.
The figure below gives an example of sampling (forward method) based on control space. A search tree is constructed while sampling. If the edge generated by the sampling input does not intersect with an obstacle, the edge is added to the search tree.
Next, let’s look at an example of sampling (reverse method) based on state space. Assume that the car model in the figure below is the Reeds-Sheep model. That is, the car can only choose to go straight forward, turn left, or turn left based on the current state. Turn right forward, reverse backward, turn left backward, and turn right backward. These six forms of movement are as shown in Figure b below. In this way, after we get the target point, we can move between the current position and the target position. Perform state sampling, and then calculate which of the six motion forms should be used to connect two adjacent state points, and finally form a complete path to the target point.
The figure below shows a Lattice graph with only two layers. First, several sampling points are taken on a circle with the robot as the center and a certain distance as the radius. These sampling points are used as intermediate states, and the connections between the robot and these intermediate states are back calculated. method, the first layer of Lattice graph is formed, and then several sampling points are taken on a circle with the robot as the center and a larger distance as the radius, and the connection method of these sampling points to the mid-state is back-calculated to form the second layer of Lattice graph. . At this point, using the middle state as a springboard, we can get the trajectory from the current position to any sampling point, and get the result of the two-layer Lattice graph shown in the figure below.
值得注意的是该两层Lattice graph中，第一层，从机器人当前位置到达中状态的轨迹是不确定的，这取决于机器人的当前状态，然而第二层，从中状态到达大圆上采样点的轨迹可以是确定的，他不受机器人当前状态的影响。
下图中给出了一些在状态空间采样和在控制空间采样的不同轨迹对比，可以看出相比于在控制空间采样，在状态空间采样的路径更优，但也存在着计算两个状态点之间轨迹不那么容易得问题。
下图中给出了在状态空间采样和在控制空间采样的示意图，对于在状态空间采样，可以离线生成Lattice图，然后在线进行搜索求解
采用单层网格规划是一种常见的局部避碰方法，如DWA算法。也可以采用轨迹库的形式，生成一系列轨迹，然后基于多项成本函数，对每个轨迹进行评级打分，选择出轨迹库中的最优轨迹。
使用Lattice Planner进行规划时，也可以使用启发函数来提高规划效率，我们设计启发式函数时有两种，一种是不考虑障碍物（下面的左图），另一种是不考虑机器人的动力学模型（下面的右图），在工程上常采用两者的集合，进行叠加或者取最大值
下图中给出了使用欧氏距离作为启发函数和使用考虑动力学模型但不考虑障碍物的启发函数的效果对比：
最后，我们来看一种特殊的Lattice Planner，在无人驾驶中常使用Frenet-serret坐标系下进行规划，在道路环境中存在着车道线，我们常常希望汽车去做一个车道线的跟随，此时我们可以沿着车道线去分解设定坐标系，沿着车道线方向为s轴，其径向方向为d轴，如下图所示，依然可以采用之前的思路，对运动方程进行参数化
d ( t ) = a d 0 + a d 1 t + a d 2 t 2 + a d 3 t 3 + a d 4 t 4 + a d 5 t 5 s ( t ) = a s 0 + a s 1 t + a s 2 t 2 + a s 3 t 3 + a s 4 t 4 + a s 5 t 5 \begin{aligned}d(t)&=a_{d0}+a_{d1}t+a_{d2}t^2+a_{d3}t^3+a_{d4}t^4+a_{d5}t^5\\s(t)&=a_{s0}+a_{s1}t+a_{s2}t^2+a_{s3}t^3+a_{s4}t^4+a_{s5}t^5\end{aligned} d(t)s(t)=ad0+ad1t+ad2t2+ad3t3+ad4t4+ad5t5=as0+as1t+as2t2+as3t3+as4t4+as5t5
For the radial direction, which satisfies the current attitude and the desired final attitude, we expect the car to be on the lane line at the final moment, that is, we expect its radial velocity and acceleration to be 0, D ( T ) = ( df 0 0 ) D (T)=\begin{pmatrix}d_f&0&0\end{pmatrix}D(T)=(df00) , and then use OBVP to solve the optimal control problem.
For the tangent direction, the situation is more complicated. There are many situations, such as parking tasks, lane changing tasks, and speed maintenance. Different strategies are generally adopted for different situations.
Here is the Lattice Planner source code in the zhm_real/MotionPlanning motion planning library. It should be noted that the source code calls other programs in the zhm_real/MotionPlanning motion planning library. All the source codes below are just to help understand the Lattice Planner algorithm. If you want to run If so, it is recommended to download the complete zhm_real/MotionPlanning motion planning library, the link is as follows:
https://github.com/zhm-real/MotionPlanning/tree/master
import os
import sys
import math
import copy
import numpy as np
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../MotionPlanning/")
from CurvesGenerator import cubic_spline, quintic_polynomial, quartic_polynomial
import LatticePlanner.env as env
import LatticePlanner.draw as draw
class C:
# Parameter
MAX_SPEED = 50.0 / 3.6
MAX_ACCEL = 8.0
MAX_CURVATURE = 2.5
ROAD_WIDTH = 8.0
ROAD_SAMPLE_STEP = 1.0
T_STEP = 0.15
MAX_T = 5.0
MIN_T = 4.0
TARGET_SPEED = 30.0 / 3.6
SPEED_SAMPLE_STEP = 5.0 / 3.6
# cost weights for Cruising
K_JERK = 0.1
K_TIME = 0.1
K_V_DIFF = 1.0
K_OFFSET = 1.5
K_COLLISION = 500
# cost weights for Stopping
# K_JERK = 0.1
# K_TIME = 0.1
# K_V_DIFF = 200
# K_OFFSET = 1.0
# K_COLLISION = 500
# parameters for vehicle
K_SIZE = 0.9
RF = 4.5 * K_SIZE # [m] distance from rear to vehicle front end of vehicle
RB = 1.0 * K_SIZE # [m] distance from rear to vehicle back end of vehicle
W = 3.0 * K_SIZE # [m] width of vehicle
WD = 0.7 * W # [m] distance between left-right wheels
WB = 3.5 * K_SIZE # [m] Wheel base
TR = 0.5 * K_SIZE # [m] Tyre radius
TW = 1 * K_SIZE # [m] Tyre width
MAX_STEER = 0.6 # [rad] maximum steering angle
class Path:
def __init__(self):
self.t = []
self.l = []
self.l_v = []
self.l_a = []
self.l_jerk = []
self.s = []
self.s_v = []
self.s_a = []
self.s_jerk = []
self.x = []
self.y = []
self.yaw = []
self.ds = []
self.curv = []
self.cost = 0.0
def sampling_paths_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path):
PATHS = dict()
for s1_v in np.arange(C.TARGET_SPEED * 0.6, C.TARGET_SPEED * 1.4, C.TARGET_SPEED * 0.2):
for t1 in np.arange(4.5, 5.5, 0.2):
path_pre = Path()
path_lon = quartic_polynomial.QuarticPolynomial(s0, s0_v, s0_a, s1_v, 0.0, t1)
path_pre.t = list(np.arange(0.0, t1, C.T_STEP))
path_pre.s = [path_lon.calc_xt(t) for t in path_pre.t]
path_pre.s_v = [path_lon.calc_dxt(t) for t in path_pre.t]
path_pre.s_a = [path_lon.calc_ddxt(t) for t in path_pre.t]
path_pre.s_jerk = [path_lon.calc_dddxt(t) for t in path_pre.t]
for l1 in np.arange(-C.ROAD_WIDTH, C.ROAD_WIDTH, C.ROAD_SAMPLE_STEP):
path = copy.deepcopy(path_pre)
path_lat = quintic_polynomial.QuinticPolynomial(l0, l0_v, l0_a, l1, 0.0, 0.0, t1)
path.l = [path_lat.calc_xt(t) for t in path_pre.t]
path.l_v = [path_lat.calc_dxt(t) for t in path_pre.t]
path.l_a = [path_lat.calc_ddxt(t) for t in path_pre.t]
path.l_jerk = [path_lat.calc_dddxt(t) for t in path_pre.t]
path.x, path.y = SL_2_XY(path.s, path.l, ref_path)
path.yaw, path.curv, path.ds = calc_yaw_curv(path.x, path.y)
l_jerk_sum = sum(np.abs(path.l_jerk))
s_jerk_sum = sum(np.abs(path.s_jerk))
v_diff = abs(C.TARGET_SPEED - path.s_v[-1])
path.cost = C.K_JERK * (l_jerk_sum + s_jerk_sum) + \
C.K_V_DIFF * v_diff + \
C.K_TIME * t1 * 2 + \
C.K_OFFSET * abs(path.l[-1]) + \
C.K_COLLISION * is_path_collision(path)
PATHS[path] = path.cost
return PATHS
def sampling_paths_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path):
PATHS = dict()
for s1_v in [-1.0, 0.0, 1.0, 2.0]:
for t1 in np.arange(1.0, 16.0, 1.0):
path_pre = Path()
path_lon = quintic_polynomial.QuinticPolynomial(s0, s0_v, s0_a, 55.0, s1_v, 0.0, t1)
path_pre.t = list(np.arange(0.0, t1, C.T_STEP))
path_pre.s = [path_lon.calc_xt(t) for t in path_pre.t]
path_pre.s_v = [path_lon.calc_dxt(t) for t in path_pre.t]
path_pre.s_a = [path_lon.calc_ddxt(t) for t in path_pre.t]
path_pre.s_jerk = [path_lon.calc_dddxt(t) for t in path_pre.t]
for l1 in [0.0]:
path = copy.deepcopy(path_pre)
path_lat = quintic_polynomial.QuinticPolynomial(l0, l0_v, l0_a, l1, 0.0, 0.0, t1)
path.l = [path_lat.calc_xt(t) for t in path_pre.t]
path.l_v = [path_lat.calc_dxt(t) for t in path_pre.t]
path.l_a = [path_lat.calc_ddxt(t) for t in path_pre.t]
path.l_jerk = [path_lat.calc_dddxt(t) for t in path_pre.t]
path.x, path.y = SL_2_XY(path.s, path.l, ref_path)
path.yaw, path.curv, path.ds = calc_yaw_curv(path.x, path.y)
if path.yaw is None:
continue
l_jerk_sum = sum(np.abs(path.l_jerk))
s_jerk_sum = sum(np.abs(path.s_jerk))
v_diff = (path.s_v[-1]) ** 2
path.cost = C.K_JERK * (l_jerk_sum + s_jerk_sum) + \
C.K_V_DIFF * v_diff + \
C.K_TIME * t1 * 2 + \
C.K_OFFSET * abs(path.l[-1]) + \
50.0 * sum(np.abs(path.s_v))
PATHS[path] = path.cost
return PATHS
def SL_2_XY(s_set, l_set, ref_path):
pathx, pathy = [], []
for i in range(len(s_set)):
x_ref, y_ref = ref_path.calc_position(s_set[i])
if x_ref is None:
break
yaw = ref_path.calc_yaw(s_set[i])
x = x_ref + l_set[i] * math.cos(yaw + math.pi / 2.0)
y = y_ref + l_set[i] * math.sin(yaw + math.pi / 2.0)
pathx.append(x)
pathy.append(y)
return pathx, pathy
def calc_yaw_curv(x, y):
yaw, curv, ds = [], [], []
for i in range(len(x) - 1):
dx = x[i + 1] - x[i]
dy = y[i + 1] - y[i]
ds.append(math.hypot(dx, dy))
yaw.append(math.atan2(dy, dx))
if len(yaw) == 0:
return None, None, None
yaw.append(yaw[-1])
ds.append(ds[-1])
for i in range(len(yaw) - 1):
curv.append((yaw[i + 1] - yaw[i]) / ds[i])
return yaw, curv, ds
def is_path_collision(path):
index = range(0, len(path.x), 5)
x = [path.x[i] for i in index]
y = [path.y[i] for i in index]
yaw = [path.yaw[i] for i in index]
for ix, iy, iyaw in zip(x, y, yaw):
d = 1.8
dl = (C.RF - C.RB) / 2.0
r = math.hypot((C.RF + C.RB) / 2.0, C.W / 2.0) + d
cx = ix + dl * math.cos(iyaw)
cy = iy + dl * math.sin(iyaw)
for i in range(len(C.obs)):
xo = C.obs[i][0] - cx
yo = C.obs[i][1] - cy
dx = xo * math.cos(iyaw) + yo * math.sin(iyaw)
dy = -xo * math.sin(iyaw) + yo * math.cos(iyaw)
if abs(dx) < r and abs(dy) < C.W / 2 + d:
return 1.0
return 0.0
def verify_path(path):
# if any([v > C.speed_max for v in path.s_v]) or \
# any([abs(a) > C.acceleration_max for a in path.s_a]):
# return False
if any([v > C.MAX_SPEED for v in path.s_v]) or \
any([abs(a) > C.MAX_ACCEL for a in path.s_a]) or \
any([abs(curv) > C.MAX_CURVATURE for curv in path.curv]):
return False
return True
def extract_optimal_path(paths):
if len(paths) == 0:
return None
while len(paths) > 1:
path = min(paths, key=paths.get)
paths.pop(path)
if verify_path(path) is False:
continue
else:
return path
return paths[-1]
def lattice_planner_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path):
paths = sampling_paths_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path)
path = extract_optimal_path(paths)
return path
def lattice_planner_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path):
paths = sampling_paths_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path)
path = extract_optimal_path(paths)
return path
def get_reference_line(x, y):
index = range(0, len(x), 3)
x = [x[i] for i in index]
y = [y[i] for i in index]
cubicspline = cubic_spline.Spline2D(x, y)
s = np.arange(0, cubicspline.s[-1], 0.1)
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = cubicspline.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(cubicspline.calc_yaw(i_s))
rk.append(cubicspline.calc_curvature(i_s))
return rx, ry, ryaw, rk, cubicspline
def pi_2_pi(theta):
if theta > math.pi:
return theta - 2.0 * math.pi
if theta < -math.pi:
return theta + 2.0 * math.pi
return theta
def main_Crusing():
ENV = env.ENVCrusing()
wx, wy = ENV.ref_line
bx1, by1 = ENV.bound_in
bx2, by2 = ENV.bound_out
C.obs = np.array([[50, 10], [96, 25], [70, 40],
[40, 50], [25, 75]])
obs_x = [x for x, y in C.obs]
obs_y = [y for x, y in C.obs]
rx, ry, ryaw, rk, ref_path = get_reference_line(wx, wy)
l0 = 2.0 # current lateral position [m]
l0_v = 0.0 # current lateral speed [m/s]
l0_a = 0.0 # current lateral acceleration [m/s]
s0 = 0.0 # current course position
s0_v = 20.0 / 3.6 # current speed [m/s]
s0_a = 0.0
while True:
path = lattice_planner_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path)
if path is None:
print("No feasible path found!!")
break
l0 = path.l[1]
l0_v = path.l_v[1]
l0_a = path.l_a[1]
s0 = path.s[1]
s0_v = path.s_v[1]
s0_a = path.s_a[1]
if np.hypot(path.x[1] - rx[-1], path.y[1] - ry[-1]) <= 2.0:
print("Goal")
break
dy = (path.yaw[2] - path.yaw[1]) / path.ds[1]
steer = pi_2_pi(math.atan(-C.WB * dy))
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(rx, ry, linestyle='--', color='gray')
plt.plot(bx1, by1, linewidth=1.5, color='k')
plt.plot(bx2, by2, linewidth=1.5, color='k')
plt.plot(path.x[1:], path.y[1:], linewidth='2', color='royalblue')
plt.plot(obs_x, obs_y, 'ok')
draw.draw_car(path.x[1], path.y[1], path.yaw[1], steer, C)
plt.title("[Crusing Mode] v :" + str(s0_v * 3.6)[0:4] + " km/h")
plt.axis("equal")
plt.pause(0.0001)
plt.pause(0.0001)
plt.show()
def main_Stopping():
ENV = env.ENVStopping()
wx, wy = ENV.ref_line
bx1, by1 = ENV.bound_up
bx2, by2 = ENV.bound_down
C.ROAD_WIDTH = ENV.road_width
rx, ry, ryaw, rk, ref_path = get_reference_line(wx, wy)
l0 = 0.0 # current lateral position [m]
l0_v = 0.0 # current lateral speed [m/s]
l0_a = 0.0 # current lateral acceleration [m/s]
s0 = 0.0 # current course position
s0_v = 30.0 / 3.6 # current speed [m/s]
s0_a = 0.0
while True:
path = lattice_planner_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path)
if path is None:
print("No feasible path found!!")
break
l0 = path.l[1]
l0_v = path.l_v[1]
l0_a = path.l_a[1]
s0 = path.s[1]
s0_v = path.s_v[1]
s0_a = path.s_a[1]
if np.hypot(path.x[1] - 56.0, path.y[1] - 0) <= 1.5:
print("Goal")
break
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(rx, ry, linestyle='--', color='gray')
plt.plot(bx1, by1, linewidth=1.5, color='k')
plt.plot(bx2, by2, linewidth=1.5, color='k')
plt.plot(path.x[1:], path.y[1:], linewidth='2', color='royalblue')
draw.draw_car(path.x[1], path.y[1], path.yaw[1], 0.0, C)
plt.title("[Stopping Mode] v :" + str(s0_v * 3.6)[0:4] + " km/h")
plt.axis("equal")
plt.pause(0.0001)
plt.pause(0.0001)
plt.show()
plt.plot(rx, ry, linestyle='--', color='gray')
plt.plot(bx1, by1, linewidth=1.5, color='k')
plt.plot(bx2, by2, linewidth=1.5, color='k')
plt.axis("equal")
plt.show()
if __name__ == '__main__':
main_Crusing()
# main_Stopping()
The running effect is as follows:
Lattice test in MotionPlanning
References:
1. Deep Blue College-Mobile Robot Motion Planning
2. zhm_real/MotionPlanning motion planning library