軟硬約束下的軌跡如何生成，理論&代碼詳解！
作者：遠(yuǎn)洋之帆
這篇文章介紹了帶軟硬約束的軌跡優(yōu)化算法框架。第一部份介紹了軟硬約束對應(yīng)到最優(yōu)求解問題數(shù)學(xué)上如何表示，第二部份介紹了貝賽爾曲線的代碼實現(xiàn)。
本文經(jīng)自動駕駛之心公眾號授權(quán)轉(zhuǎn)載，轉(zhuǎn)載請聯(lián)系出處。
本項目代碼：
github.com/liangwq/robot_motion_planing
軌跡約束中的軟硬約束
前面的幾篇文章已經(jīng)介紹了，軌跡約束的本質(zhì)就是在做帶約束的軌跡擬合。輸入就是waypoint點list，約束條件有兩種硬約束和軟約束。所謂硬約束對應(yīng)到數(shù)學(xué)形式就是代價函數(shù)，硬約束對應(yīng)的就是最優(yōu)化秋季的約束條件部分。對應(yīng)到物理意義就是，為了獲得機器人可行走的安全的軌跡有：
- 把軌跡通過代價函數(shù)推離障礙物的方式
- 給出障礙物之間的可行走凸包走廊，通過硬約束讓機器人軌跡必須在凸包走廊行走
上圖展示的是軟硬約束下Bezier曲線擬合的求解的數(shù)學(xué)框架，以及如何把各種的約束條件轉(zhuǎn)成數(shù)學(xué)求解的代價函數(shù)（軟約束）或者是求解的約束條件（軟約束）。image.png
上面是對常用的代價函數(shù)約束的幾種表示方式的舉例。image.png
Bezier曲線擬合軌跡
前面已經(jīng)一篇文章介紹過貝賽爾曲線擬合的各種優(yōu)點：
- 端點插值。貝塞爾曲線始終從第一個控制點開始，結(jié)束于最后一個控制點，并且不會經(jīng)過任何其他控制點。
- 凸包。貝塞爾曲線 ( ) 由一組控制點 完全限制在由所有這些控制點定義的凸包內(nèi)。
- 速度曲線。貝塞爾曲線 ( ) 的導(dǎo)數(shù)曲線 ′( ) 被稱為速度曲線，它也是一個由控制點定義的貝塞爾曲線，其中控制點為 ? ( +1? )，其中 是階數(shù)。
- 固定時間間隔。貝塞爾曲線始終在 [0,1] 上定義。
Fig1.一段軌跡用bezier曲線擬合image.png
上面的兩個表達(dá)式對應(yīng)的代碼實現(xiàn)如下：
def bernstein_poly(n, i, t):
"""
Bernstein polynom.
:param n: (int) polynom degree
:param i: (int)
:param t: (float)
:return: (float)
"""
return scipy.special.comb(n, i) * t ** i * (1 - t) ** (n - i)
def bezier(t, control_points):
"""
Return one point on the bezier curve.
:param t: (float) number in [0, 1]
:param control_points: (numpy array)
:return: (numpy array) Coordinates of the point
"""
n = len(control_points) - 1
return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)要用Bezier曲線來表示一段曲線，上面已經(jīng)給出了表達(dá)式和代碼實現(xiàn)，現(xiàn)在缺的就是給定控制點，把控制點帶進來用Bezier曲線表達(dá)式來算出給定終點和結(jié)束點中需要畫的點坐標(biāo)。下面代碼給出了4個控制點、6個控制點的Bezier曲線實現(xiàn)；其中為了畫曲線需要算170個線上點。代碼如下：
def calc_4points_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset):
"""
Compute control points and path given start and end position.
:param sx: (float) x-coordinate of the starting point
:param sy: (float) y-coordinate of the starting point
:param syaw: (float) yaw angle at start
:param ex: (float) x-coordinate of the ending point
:param ey: (float) y-coordinate of the ending point
:param eyaw: (float) yaw angle at the end
:param offset: (float)
:return: (numpy array, numpy array)
"""
dist = np.hypot(sx - ex, sy - ey) / offset
control_points = np.array(
[[sx, sy],
[sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],
[ex - dist * np.cos(eyaw), ey - dist * np.sin(eyaw)],
[ex, ey]])
path = calc_bezier_path(control_points, n_points=170)
return path, control_points
def calc_6points_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset):
"""
Compute control points and path given start and end position.
:param sx: (float) x-coordinate of the starting point
:param sy: (float) y-coordinate of the starting point
:param syaw: (float) yaw angle at start
:param ex: (float) x-coordinate of the ending point
:param ey: (float) y-coordinate of the ending point
:param eyaw: (float) yaw angle at the end
:param offset: (float)
:return: (numpy array, numpy array)
"""
dist = np.hypot(sx - ex, sy - ey) * offset
control_points = np.array(
[[sx, sy],
[sx + 0.25 * dist * np.cos(syaw), sy + 0.25 * dist * np.sin(syaw)],
[sx + 0.40 * dist * np.cos(syaw), sy + 0.40 * dist * np.sin(syaw)],
[ex - 0.40 * dist * np.cos(eyaw), ey - 0.40 * dist * np.sin(eyaw)],
[ex - 0.25 * dist * np.cos(eyaw), ey - 0.25 * dist * np.sin(eyaw)],
[ex, ey]])
path = calc_bezier_path(control_points, n_points=170)
return path, control_points
def calc_bezier_path(control_points, n_points=100):
"""
Compute bezier path (trajectory) given control points.
:param control_points: (numpy array)
:param n_points: (int) number of points in the trajectory
:return: (numpy array)
"""
traj = []
for t in np.linspace(0, 1, n_points):
traj.append(bezier(t, control_points))
return np.array(traj)有了一段Bezier曲線的擬合方式，接下來要做的就是如何生成多段Bezier曲線合成一條軌跡；并且是需要可以通過代價函數(shù)方式（軟約束）+必須經(jīng)過指定點+制定點銜接要連續(xù)（硬約束）來生成一條光滑的軌跡曲線。
Fig2.無障礙物，帶bondary軌跡做了smooth優(yōu)化Figure_1.png
多段Bezier曲線生成代碼如下，其實原理很簡單，給定多個waypoint點，每相鄰兩個wayponit生成一段Bezizer曲線，代碼如下：
# Bezier path one as per the approach suggested in
# https://users.soe.ucsc.edu/~elkaim/Documents/camera_WCECS2008_IEEE_ICIAR_58.pdf
def cubic_bezier_path(self, ax, ay):
dyaw, _ = self.calc_yaw_curvature(ax, ay)
cx = []
cy = []
ayaw = dyaw.copy()
for n in range(1, len(ax)-1):
yaw = 0.5*(dyaw[n] + dyaw[n-1])
ayaw[n] = yaw
last_ax = ax[0]
last_ay = ay[0]
last_ayaw = ayaw[0]
# for n waypoints, there are n-1 bezier curves
for i in range(len(ax)-1):
path, ctr_points = calc_4points_bezier_path(last_ax, last_ay, ayaw[i], ax[i+1], ay[i+1], ayaw[i+1], 2.0)
cx = np.concatenate((cx, path.T[0][:-2]))
cy = np.concatenate((cy, path.T[1][:-2]))
cyaw, k = self.calc_yaw_curvature(cx, cy)
last_ax = path.T[0][-1]
last_ay = path.T[1][-1]
return cx, cy代價函數(shù)計算包括：曲率代價 + 偏差代價 + 距離代價 + 連續(xù)性代價，同時還有邊界條件，軌跡必須在tube內(nèi)的不等式約束，以及問題優(yōu)化求解。具體代碼實現(xiàn)如下：
# Objective function of cost to be minimized
def cubic_objective_func(self, deviation):
ax = self.waypoints.x.copy()
ay = self.waypoints.y.copy()
for n in range(0, len(deviation)):
ax[n+1] -= deviation[n]*np.sin(self.waypoints.yaw[n+1])
ay[n+1] += deviation[n]*np.cos(self.waypoints.yaw[n+1])
bx, by = self.cubic_bezier_path(ax, ay)
yaw, k = self.calc_yaw_curvature(bx, by)
# cost of curvature continuity
t = np.zeros((len(k)))
dk = self.calc_d(t, k)
absolute_dk = np.absolute(dk)
continuity_cost = 10.0 * np.mean(absolute_dk)
# curvature cost
absolute_k = np.absolute(k)
curvature_cost = 14.0 * np.mean(absolute_k)
# cost of deviation from input waypoints
absolute_dev = np.absolute(deviation)
deviation_cost = 1.0 * np.mean(absolute_dev)
distance_cost = 0.5 * self.calc_path_dist(bx, by)
return curvature_cost + deviation_cost + distance_cost + continuity_cost
# Minimize objective function using scipy optimize minimize
def optimize_min_cubic(self):
print("Attempting optimization minima")
initial_guess = [0, 0, 0, 0, 0]
bnds = ((-self.bound, self.bound), (-self.bound, self.bound), (-self.bound, self.bound), (-self.bound, self.bound), (-self.bound, self.bound))
result = optimize.minimize(self.cubic_objective_func, initial_guess, bounds=bnds)
ax = self.waypoints.x.copy()
ay = self.waypoints.y.copy()
if result.success:
print("optimized true")
deviation = result.x
for n in range(0, len(deviation)):
ax[n+1] -= deviation[n]*np.sin(self.waypoints.yaw[n+1])
ay[n+1] += deviation[n]*np.cos(self.waypoints.yaw[n+1])
x, y = self.cubic_bezier_path(ax, ay)
yaw, k = self.calc_yaw_curvature(x, y)
self.optimized_path = Path(x, y, yaw, k)
else:
print("optimization failure, defaulting")
exit()帶障礙物的Bezier曲線軌跡生
帶有障礙物的場景，通過代價函數(shù)讓生成的曲線遠(yuǎn)離障礙物。從而得到一條可以安全行走的軌跡，下面是具體的代碼實現(xiàn)。optimizer_k中l(wèi)ambda函數(shù)f就是在求解軌跡在經(jīng)過障礙物附近時候的代價，penalty1、penalty2就是在求曲線經(jīng)過障礙物附近的具體代價值；
b.arc_len(granuality=10)+B.arc_len(granuality=10)+m_k + penalty1 + penalty2就是軌跡的整體代價。for循環(huán)部分用scipy的optimize的minimize來求解軌跡。
def optimizer_k(cd, k, path, i, obs, curve_penalty_multiplier, curve_penalty_divider, curve_penalty_obst):
"""Bezier curve optimizer that optimizes the curvature and path length by changing the distance of p1 and p2 from
points p0 and p3, respectively. """
p_tmp = copy.deepcopy(path)
if i+3 > len(path)-1:
b = CubicBezier()
b.p0 = p_tmp[i]
x, y = calc_p1(p_tmp[i], p_tmp[i + 1], p_tmp[i - 1], i, cd[0])
b.p1 = Point(x, y)
x, y = calc_p2(p_tmp[i-1], p_tmp[i + 0], p_tmp[i + 1], i, cd[1])
b.p2 = Point(x, y)
b.p3 = p_tmp[i + 1]
B = CubicBezier()
else:
b = CubicBezier()
b.p0 = p_tmp[i]
x, y = calc_p1(p_tmp[i],p_tmp[i+1],p_tmp[i-1], i, cd[0])
b.p1 = Point(x, y)
x, y = calc_p2(p_tmp[i],p_tmp[i+1],p_tmp[i+2], i, cd[1])
b.p2 = Point(x, y)
b.p3 = p_tmp[i + 1]
B = CubicBezier()
B.p0 = p_tmp[i]
x, y = calc_p1(p_tmp[i+1], p_tmp[i + 2], p_tmp[i], i, 10)
B.p1 = Point(x, y)
x, y = calc_p2(p_tmp[i+1], p_tmp[i + 2], p_tmp[i + 3], i, 10)
B.p2 = Point(x, y)
B.p3 = p_tmp[i + 1]
m_k = b.max_k()
if m_k>k:
m_k= m_k*curve_penalty_multiplier
else:
m_k = m_k/curve_penalty_divider
f = lambda x, y: max(math.sqrt((x[0] - y[0].x) ** 2 + (x[1] - y[0].y) ** 2) * curve_penalty_obst, 10) if math.sqrt(
(x[0] - y[0].x) ** 2 + (x[1] - y[0].y) ** 2) < y[1] else 0
b_t = b.calc_curve(granuality=10)
b_t = zip(b_t[0],b_t[1])
B_t = B.calc_curve(granuality=10)
B_t = zip(B_t[0], B_t[1])
penalty1 = 0
penalty2 = 0
for o in obs:
for t in b_t:
penalty1 = max(penalty1,f(t,o))
for t in B_t:
penalty2 = max(penalty2,f(t,o))
return b.arc_len(granuality=10)+B.arc_len(granuality=10)+m_k + penalty1 + penalty2
# Optimize the initial path for n_path_opt cycles
for m in range(n_path_opt):
if m%2:
for i in range(1,len(path)-1):
x0 = [0.0, 0.0]
bounds = Bounds([-1, -1], [1, 1])
res = minimize(optimizer_p, x0, args=(path, i, obs, path_penalty), method='TNC', tol=1e-7, bounds=bounds)
x, y = res.x
path[i].x += x
path[i].y += y
else:
for i in range(len(path)-1,1):
x0 = [0.0, 0.0]
bounds = Bounds([-1, -1], [1, 1])
res = minimize(optimizer_p, x0, args=(path, i, obs, path_penalty), method='TNC', tol=1e-7, bounds=bounds)
x, y = res.x
path[i].x += x
path[i].y += y帶飛行走廊的Bezier軌跡生成
得益于貝賽爾曲線擬合的優(yōu)勢，如果我們可以讓機器人可行走的軌跡轉(zhuǎn)成多個有重疊區(qū)域的凸多面體，那么軌跡完全位于飛行走廊內(nèi)。
image.png
- 飛行走廊由凸多邊形組成。
- 每個立方體對應(yīng)于一段貝塞爾曲線。
- 此曲線的控制點被強制限制在多邊形內(nèi)部。
- 軌跡完全位于所有點的凸包內(nèi)。
如何通過把障礙物地圖生成可行凸包走廊
生成凸包走廊的方法目前有以下三大類的方法：
平行凸簇膨脹方法
從柵格地圖出發(fā)，利用最小凸集生成算法，完成凸多面體的生成。其算法的思想是首先獲得一個凸集，再沿著凸集的表面進行擴張，擴張之后再進行凸集檢測，判斷新擴張的集合是否保持為凸。一直擴張到不能再擴張為止，再提取凸集的邊緣點，利用快速凸集生成算法，生成凸多面體。該算法的好處在于可以利用這種擴張的思路，將安全的多面體的體積盡可能的充滿整個空間，因此獲得的安全通道更大。但其也具有一定的缺點，就是計算量比較大，計算所需要的時間比較長，為了解決這個問題，在該文章中，又提出了采用GPU加速的方法，來加速計算。
基于凸分解的安全通道生成
基于凸分解的安全通道生成方法由四個步驟完成安全通道的生成，分別為：找到橢球、找到多面體、邊界框、收縮。
半定規(guī)劃的迭代區(qū)域膨脹
為了獲取多面體，這個方法首先構(gòu)造一個初始橢球，由一個以選定點為中心的單位球組成。然后，遍歷障礙物，為每個障礙物生成一個超平面，該超平面與障礙物相切并將其與橢球分開。再次，這些超平面定義了一組線性約束，它們的交集是一個多面體。然后，可以在那個多面體中找到一個最大的橢球，使用這個橢球來定義一組新的分離超平面，從而定義一個新的多面體。選擇生成分離超平面的方法，這樣橢圓體的體積在迭代之間永遠(yuǎn)不會減少。可以重復(fù)這個過程，直到橢圓體的增長率低于某個閾值，此時我們返回多面體和內(nèi)接橢圓體。這個方法具有迭代的思想，并且具有收斂判斷的標(biāo)準(zhǔn)，算法的收斂快慢和初始橢球具有很大的關(guān)系。
Fig3.半定規(guī)劃的迭代區(qū)域膨脹。每一行即為一次迭代操作，直到橢圓體的增長率低于閾值。image.png
這篇文章介紹的是“半定規(guī)劃的迭代區(qū)域膨脹”方法，具體代碼實現(xiàn)如下：
# 根據(jù)輸入路徑對空間進行凸分解
def decomp(self, line_points: list[np.array], obs_points: list[np.array], visualize=True):
# 最終結(jié)果
decomp_polygons = list()
# 構(gòu)建輸入障礙物點的kdtree
obs_kdtree = KDTree(obs_points)
# 進行空間分解
for i in range(len(line_points) - 1):
# 得到當(dāng)前線段
pf, pr = line_points[i], line_points[i + 1]
print(pf)
print(pr)
# 構(gòu)建初始多面體
init_polygon = self.initPolygon(pf, pr)
print(init_polygon.getInterPoints())
print(init_polygon.getVerticals())
# 過濾障礙物點
candidate_obs_point_indexes = obs_kdtree.query_ball_point((pf + pr) / 2, np.linalg.norm([np.linalg.norm(pr - pf) / 2 + self.consider_range_, self.consider_range_]))
local_obs_points = list()
for index in candidate_obs_point_indexes:
if init_polygon.inside(obs_points[index]):
local_obs_points.append(obs_points[index])
# 得到初始橢圓
ellipse = self.findEllipse(pf, pr, local_obs_points)
# 根據(jù)初始橢圓構(gòu)建多面體
polygon = self.findPolygon(ellipse, init_polygon, local_obs_points)
# 進行保存
decomp_polygons.append(polygon)
if visualize:
# 進行可視化
plt.figure()
# 繪制路徑段
plt.plot([pf[1], pr[1]], [pf[0], pr[0]], color="red")
# 繪制初始多面體
verticals = init_polygon.getVerticals()
# 繪制多面體頂點
plt.plot([v[1] for v in verticals] + [verticals[0][1]], [v[0] for v in verticals] + [verticals[0][0]], color="blue", linestyle="--")
# 繪制障礙物點
plt.scatter([p[1] for p in local_obs_points], [p[0] for p in local_obs_points], marker="o")
# 繪制橢圓
ellipse_x, ellipse_y = list(), list()
for theta in np.linspace(-np.pi, np.pi, 1000):
raw_point = np.array([np.cos(theta), np.sin(theta)])
ellipse_point = np.dot(ellipse.C_, raw_point) + ellipse.d_
ellipse_x.append(ellipse_point[0])
ellipse_y.append(ellipse_point[1])
plt.plot(ellipse_y, ellipse_x, color="orange")
# 繪制最終多面體
# 得到多面體頂點
verticals = polygon.getVerticals()
# 繪制多面體頂點
plt.plot([v[1] for v in verticals] + [verticals[0][1]], [v[0] for v in verticals] + [verticals[0][0]], color="green")
plt.show()
return decomp_polygons
# 構(gòu)建初始多面體
def initPolygon(self, pf: np.array, pr: np.array) -> Polygon:
# 記錄多面體的平面
polygon_planes = list()
# 得到線段方向向量
dire = self.normalize(pr - pf)
# 得到線段法向量
dire_h = np.array([dire[1], -dire[0]])
# 得到平行范圍
p_1 = pf + self.consider_range_ * dire_h
p_2 = pf - self.consider_range_ * dire_h
polygon_planes.append(Hyperplane(dire_h, p_1))
polygon_planes.append(Hyperplane(-dire_h, p_2))
# 得到垂直范圍
p_3 = pr + self.consider_range_ * dire
p_4 = pf - self.consider_range_ * dire
polygon_planes.append(Hyperplane(dire, p_3))
polygon_planes.append(Hyperplane(-dire, p_4))
# 構(gòu)建多面體
polygon = Polygon(polygon_planes)
return polygon
# 得到初始橢圓
def findEllipse(self, pf: np.array, pr: np.array, obs_points: list[np.array]) -> Ellipse:
# 計算長軸
long_axis_value = np.linalg.norm(pr - pf) / 2
axes = np.array([long_axis_value, long_axis_value])
# 計算旋轉(zhuǎn)
rotation = self.vec2Rotation(pr - pf)
# 計算初始橢圓
C = np.dot(rotation, np.dot(np.array([[axes[0], 0], [0, axes[1]]]), np.transpose(rotation)))
d = (pr + pf) / 2
ellipse = Ellipse(C, d)
# 得到橢圓內(nèi)的障礙物點
inside_obs_points = ellipse.insidePoints(obs_points)
# 對橢圓進行調(diào)整，使得全部障礙物點都在橢圓外
while inside_obs_points:
# 得到與橢圓距離最近的點
closest_obs_point = ellipse.closestPoint(inside_obs_points)
# 將最近點轉(zhuǎn)到橢圓坐標(biāo)系下
closest_obs_point = np.dot(np.transpose(rotation), closest_obs_point - ellipse.d_)
# 根據(jù)最近點，在橢圓長軸不變的情況下對短軸進行改變，使得，障礙物點在橢圓上
if Compare.small(closest_obs_point[0], axes[0]):
axes[1] = np.abs(closest_obs_point[1]) / np.sqrt(1 - (closest_obs_point[0] / axes[0]) ** 2)
# 更新橢圓
ellipse.C_ = np.dot(rotation, np.dot(np.array([[axes[0], 0], [0, axes[1]]]), np.transpose(rotation)))
# 更新橢圓內(nèi)部障礙物
inside_obs_points = ellipse.insidePoints(inside_obs_points, include_bound=False)
return ellipse
# 進行多面體的構(gòu)建
def findPolygon(self, ellipse: Ellipse, init_polygon: Polygon, obs_points: list[np.array]) -> Polygon:
# 多面體由多個超平面構(gòu)成
polygon_planes = copy.deepcopy(init_polygon.hyper_planes_)
# 初始化范圍超平面
remain_obs_points = obs_points
while remain_obs_points:
# 得到與橢圓最近障礙物
closest_point = ellipse.closestPoint(remain_obs_points)
# 計算該處的切平面的法向量
norm_vector = np.dot(np.linalg.inv(ellipse.C_), np.dot(np.linalg.inv(ellipse.C_), (closest_point - ellipse.d_)))
norm_vector = self.normalize(norm_vector)
# 構(gòu)建平面
hyper_plane = Hyperplane(norm_vector, closest_point)
# 保存到多面體平面中
polygon_planes.append(hyper_plane)
# 去除切平面外部的障礙物
new_remain_obs_points = list()
for point in remain_obs_points:
if Compare.small(hyper_plane.signDist(point), 0):
new_remain_obs_points.append(point)
remain_obs_points = new_remain_obs_points
polygon = Polygon(polygon_planes)
return polygon
上面圖是給定16個障礙物點，必經(jīng)6個路徑點后得到的凸包可行走廊，具體代碼如下：
def main():
# 路徑點
line_points = [np.array([-1.5, 0.0]), np.array([0.0, 0.8]), np.array([1.5, 0.3]), np.array([5, 0.6]), np.array([6, 1.2]), np.array([7.6, 2.2])]
# 障礙物點
obs_points = [
np.array([4, 2.0]),
np.array([6, 3.0]),
np.array([2, 1.5]),
np.array([0, 1]),
np.array([1, 0]),
np.array([1.8, 0]),
np.array([3.8, 2]),
np.array([0.5, 1.2]),
np.array([4.3, 0]),
np.array([8, 0.9]),
np.array([2.8, -0.3]),
np.array([6, -0.9]),
np.array([-0.5, -0.5]),
np.array([-0.75 ,-0.5]),
np.array([-1, -0.5]),
np.array([-1, 0.8])
]
convex_decomp = ConvexDecomp(2)
decomp_polygons = convex_decomp.decomp(line_points, obs_points, False)
#convex_decomp.decomp(line_points, obs_points,False)
plt.figure()
# 繪制障礙物點
plt.scatter([p[0] for p in obs_points], [p[1] for p in obs_points], marker="o")
# 繪制邊界
for polygon in decomp_polygons:
verticals = polygon.getVerticals()
# 繪制多面體頂點
plt.plot([v[0] for v in verticals] + [verticals[0][0]], [v[1] for v in verticals] + [verticals[0][1]], color="green")
#plt.plot(x_samples, y_samples)
plt.show()帶凸包走廊求解
帶凸包走廊的光滑軌跡生成。前面已經(jīng)求解得到了可行的凸包走廊，這部分可以做為硬約束作為最優(yōu)化求解的不等式條件。要求的光滑路徑和必須經(jīng)過點的點，這部分可以把必須經(jīng)過點作為等式約束，光滑路徑可以通過代價函數(shù)來實現(xiàn)。這樣就可以把帶軟硬約束的軌跡生成框架各種技能點都用上了。
下面看具體代碼實現(xiàn)：
# 進行優(yōu)化
def optimize(self, start_state: np.array, end_state: np.array, line_points: list[np.array], polygons: list[Polygon]):
assert(len(line_points) == len(polygons) + 1)
# 得到分段數(shù)量
segment_num = len(polygons)
assert(segment_num >= 1)
# 計算初始時間分配
time_allocations = list()
for i in range(segment_num):
time_allocations.append(np.linalg.norm(line_points[i+1] - line_points[i]) / self.vel_max_)
# 進行優(yōu)化迭代
max_inter = 10
cur_iter = 0
while cur_iter < max_inter:
# 進行軌跡優(yōu)化
piece_wise_trajectory = self.optimizeIter(start_state, end_state, polygons, time_allocations, segment_num)
# 對優(yōu)化軌跡進行時間調(diào)整，以保證軌跡滿足運動上限約束
cur_iter += 1
# 計算每一段軌跡的最大速度，最大加速度，最大jerk
condition_fit = True
for n in range(segment_num):
# 得到最大速度，最大加速度，最大jerk
t_samples = np.linspace(0, time_allocations[n], 100)
v_max, a_max, j_max = self.vel_max_, self.acc_max_, self.jerk_max_
for t_sample in t_samples:
v_max = max(v_max, np.abs(piece_wise_trajectory.trajectory_segments_[n][0].derivative(t_sample)), np.abs(piece_wise_trajectory.trajectory_segments_[n][1].derivative(t_sample)))
a_max = max(a_max, np.abs(piece_wise_trajectory.trajectory_segments_[n][0].secondOrderDerivative(t_sample)), np.abs(piece_wise_trajectory.trajectory_segments_[n][1].secondOrderDerivative(t_sample)))
j_max = max(j_max, np.abs(piece_wise_trajectory.trajectory_segments_[n][0].thirdOrderDerivative(t_sample)), np.abs(piece_wise_trajectory.trajectory_segments_[n][1].thirdOrderDerivative(t_sample)))
# 判斷是否滿足約束條件
if Compare.large(v_max, self.vel_max_) or Compare.large(a_max, self.acc_max_) or Compare.large(j_max, self.jerk_max_):
ratio = max(1, v_max / self.vel_max_, (a_max / self.acc_max_)**0.5, (j_max / self.jerk_max_)**(1/3))
time_allocations[n] = ratio * time_allocations[n]
condition_fit = False
if condition_fit:
break
return piece_wise_trajectory
# 優(yōu)化迭代
def optimizeIter(self, start_state: np.array, end_state: np.array, polygons: list[Polygon], time_allocations: list, segment_num):
# 構(gòu)建目標(biāo)函數(shù) inter (jerk)^2
inte_jerk_square = np.array([
[720.0, -1800.0, 1200.0, 0.0, 0.0, -120.0],
[-1800.0, 4800.0, -3600.0, 0.0, 600.0, 0.0],
[1200.0, -3600.0, 3600.0, -1200.0, 0.0, 0.0],
[0.0, 0.0, -1200.0, 3600.0, -3600.0, 1200.0],
[0.0, 600.0, 0.0, -3600.0, 4800.0, -1800.0],
[-120.0, 0.0, 0.0, 1200.0, -1800.0, 720.0]
])
# 二次項系數(shù)
P = np.zeros((self.dim_ * segment_num * self.freedom_, self.dim_ * segment_num * self.freedom_))
for sigma in range(self.dim_):
for n in range(segment_num):
for i in range(self.freedom_):
for j in range(self.freedom_):
index_i = sigma * segment_num * self.freedom_ + n * self.freedom_ + i
index_j = sigma * segment_num * self.freedom_ + n * self.freedom_ + j
P[index_i][index_j] = inte_jerk_square[i][j] / (time_allocations[n] ** 5)
P = P * 2
P = sparse.csc_matrix(P)
# 一次項系數(shù)
q = np.zeros((self.dim_ * segment_num * self.freedom_,))
# 構(gòu)建約束條件
equality_constraints_num = 5 * self.dim_ + 3 * (segment_num - 1) * self.dim_
inequality_constraints_num = 0
for polygon in polygons:
inequality_constraints_num += self.freedom_ * len(polygon.hyper_planes_)
A = np.zeros((equality_constraints_num + inequality_constraints_num, self.dim_ * segment_num * self.freedom_))
lb = -float("inf") * np.ones((equality_constraints_num + inequality_constraints_num,))
ub = float("inf") * np.ones((equality_constraints_num + inequality_constraints_num,))
# 構(gòu)建等式約束條件（起點位置、速度、加速度；終點位置、速度；連接處的零、一、二階導(dǎo)數(shù)）
# 起點x位置
A[0][0] = 1
lb[0] = start_state[0]
ub[0] = start_state[0]
# 起點y位置
A[1][segment_num * self.freedom_] = 1
lb[1] = start_state[1]
ub[1] = start_state[1]
# 起點x速度
A[2][0] = -5 / time_allocations[0]
A[2][1] = 5 / time_allocations[0]
lb[2] = start_state[2]
ub[2] = start_state[2]
# 起點y速度
A[3][segment_num * self.freedom_] = -5 / time_allocations[0]
A[3][segment_num * self.freedom_ + 1] = 5 / time_allocations[0]
lb[3] = start_state[3]
ub[3] = start_state[3]
# 起點x加速度
A[4][0] = 20 / time_allocations[0]**2
A[4][1] = -40 / time_allocations[0]**2
A[4][2] = 20 / time_allocations[0]**2
lb[4] = start_state[4]
ub[4] = start_state[4]
# 起點y加速度
A[5][segment_num * self.freedom_] = 20 / time_allocations[0]**2
A[5][segment_num * self.freedom_ + 1] = -40 / time_allocations[0]**2
A[5][segment_num * self.freedom_ + 2] = 20 / time_allocations[0]**2
lb[5] = start_state[5]
ub[5] = start_state[5]
# 終點x位置
A[6][segment_num * self.freedom_ - 1] = 1
lb[6] = end_state[0]
ub[6] = end_state[0]
# 終點y位置
A[7][self.dim_ * segment_num * self.freedom_ - 1] = 1
lb[7] = end_state[1]
ub[7] = end_state[1]
# 終點x速度
A[8][segment_num * self.freedom_ - 1] = 5 / time_allocations[-1]
A[8][segment_num * self.freedom_ - 2] = -5 / time_allocations[-1]
lb[8] = end_state[2]
ub[8] = end_state[2]
# 終點y速度
A[9][self.dim_ * segment_num * self.freedom_ - 1] = 5 / time_allocations[-1]
A[9][self.dim_ * segment_num * self.freedom_ - 2] = -5 / time_allocations[-1]
lb[9] = end_state[3]
ub[9] = end_state[3]
# 連接處的零階導(dǎo)數(shù)相等
constraints_index = 10
for sigma in range(self.dim_):
for n in range(segment_num - 1):
A[constraints_index][sigma * segment_num * self.freedom_ + n * self.freedom_ + self.freedom_ - 1] = 1
A[constraints_index][sigma * segment_num * self.freedom_ + (n+1) * self.freedom_] = -1
lb[constraints_index] = 0
ub[constraints_index] = 0
constraints_index += 1
# 連接處的一階導(dǎo)數(shù)相等
for sigma in range(self.dim_):
for n in range(segment_num - 1):
A[constraints_index][sigma * segment_num * self.freedom_ + n * self.freedom_ + self.freedom_ - 1] = 5 / time_allocations[n]
A[constraints_index][sigma * segment_num * self.freedom_ + n * self.freedom_ + self.freedom_ - 2] = -5 / time_allocations[n]
A[constraints_index][sigma * segment_num * self.freedom_ + (n+1) * self.freedom_] = 5 / time_allocations[n + 1]
A[constraints_index][sigma * segment_num * self.freedom_ + (n+1) * self.freedom_ + 1] = -5 / time_allocations[n + 1]
lb[constraints_index] = 0
ub[constraints_index] = 0
constraints_index += 1
# 連接處的二階導(dǎo)數(shù)相等
for sigma in range(self.dim_):
for n in range(segment_num - 1):
A[constraints_index][sigma * segment_num * self.freedom_ + n * self.freedom_ + self.freedom_ - 1] = 20 / time_allocations[n]**2
A[constraints_index][sigma * segment_num * self.freedom_ + n * self.freedom_ + self.freedom_ - 2] = -40 / time_allocations[n]**2
A[constraints_index][sigma * segment_num * self.freedom_ + n * self.freedom_ + self.freedom_ - 3] = 20 / time_allocations[n]**2
A[constraints_index][sigma * segment_num * self.freedom_ + (n+1) * self.freedom_] = -20 / time_allocations[n + 1]**2
A[constraints_index][sigma * segment_num * self.freedom_ + (n+1) * self.freedom_ + 1] = 40 / time_allocations[n + 1]**2
A[constraints_index][sigma * segment_num * self.freedom_ + (n+1) * self.freedom_ + 2] = -20 / time_allocations[n + 1]**2
lb[constraints_index] = 0
ub[constraints_index] = 0
constraints_index += 1
# 構(gòu)建不等式約束條件
for n in range(segment_num):
for k in range(self.freedom_):
for hyper_plane in polygons[n].hyper_planes_:
A[constraints_index][n * self.freedom_ + k] = hyper_plane.n_[0]
A[constraints_index][segment_num * self.freedom_ + n * self.freedom_ + k] = hyper_plane.n_[1]
ub[constraints_index] = np.dot(hyper_plane.n_, hyper_plane.d_)
constraints_index += 1
assert(constraints_index == equality_constraints_num + inequality_constraints_num)
A = sparse.csc_matrix(A)
# 進行qp求解
prob = osqp.OSQP()
prob.setup(P, q, A, lb, ub, warm_start=True)
res = prob.solve()
if res.info.status != "solved":
raise ValueError("OSQP did not solve the problem!")
# 根據(jù)參數(shù)進行軌跡解析
trajectory_x_params, trajectory_y_params = list(), list()
for n in range(segment_num):
trajectory_x_params.append(res.x[self.freedom_ * n: self.freedom_ * (n+1)])
trajectory_y_params.append(res.x[segment_num * self.freedom_ + self.freedom_ * n: segment_num * self.freedom_ + self.freedom_ * (n+1)])
piece_wise_trajectory = PieceWiseTrajectory(trajectory_x_params, trajectory_y_params, time_allocations)
return piece_wise_trajectory小結(jié)：
這篇文章介紹了帶軟硬約束的軌跡優(yōu)化算法框架。第一部份介紹了軟硬約束對應(yīng)到最優(yōu)求解問題數(shù)學(xué)上如何表示。第二部份介紹了貝賽爾曲線的代碼實現(xiàn)，給出了具體的代碼實現(xiàn)和講解；并針對沒有障礙物場景只給定waypoint點，生成光滑的Bezier軌跡的樸素求解代碼實現(xiàn)。第三部份給出了帶障礙物情況下如何做最優(yōu)化求解，如何通過代價函數(shù)的方式來給軌跡施加推力讓軌跡遠(yuǎn)離障礙物的代碼實現(xiàn)。第四部分是一個綜合性的例子，把軟硬約束最優(yōu)軌跡生成的求解框架做了一個綜合呈現(xiàn)。詳細(xì)的介紹了如何利用障礙物地圖生成最大可行區(qū)域的凸包走廊，如何利用Bezier曲線的特性給定凸包兩點生成路徑一定在凸包中；以及如何利用代價行數(shù)來保證軌跡的光滑性、安全性，通過等式、不等式約束實現(xiàn)軌跡必須經(jīng)過哪些點，某個點的運動狀態(tài)如何。
這一系列的文章已經(jīng)進入結(jié)尾的階段，后面會簡單介紹在碰到移動的物體時候單機器人如何處理；以及在多個機器人運行環(huán)境如何協(xié)同，最后會給出一個Motion Planning的綜合實現(xiàn)例子講解實際環(huán)境數(shù)據(jù)輸入、前端規(guī)劃、后端軌跡生成。至于定位和感知部分的內(nèi)容后面可以根據(jù)情況而定是否在開一個新的系列來講解介紹，對于更前沿的技術(shù)點會跟進論文做些文章分享。
最后本系列文章的代碼在以下git鏈接，這部分代碼相對零碎主要是配合文章理論來講的，里面很多片段直接來源于網(wǎng)絡(luò)整合。后面這可項目會持續(xù)維護，把項目代碼（應(yīng)該是c++實現(xiàn)，更體系）、整合進來，根據(jù)需要在看看有沒必要整合出一個庫。
原文鏈接：https://mp.weixin.qq.com/s/0EVgYKTxLzUj64L5jzMVug
責(zé)任編輯：張燕妮
來源：
自動駕駛之心
