|
この記事は、Heart of Autonomous Driving の公開アカウントから許可を得て転載したものです。転載については出典元にお問い合わせください。 このプロジェクトコード: github.com/liangwq/robot_motion_planing 軌道制約におけるソフト制約とハード制約前回の記事で紹介したように、軌道制約の本質は、制約された軌道フィッティングを行うことです。入力はウェイポイントのリストです。制約にはハード制約とソフト制約があります。いわゆるハード制約は、数学的な形式のコスト関数に対応し、ハード制約は、最適秋の制約条件部分に対応します。物理的な意味に対応して、ロボットが歩行するための安全な軌道を得るためには、次のことが必要です。 - コスト関数を通じて軌道を障害物から遠ざける方法
- 障害物の間に歩行可能な凸包回廊がある場合、ロボットの軌道は厳しい制約条件の下で凸包回廊内を歩行する必要がある。
上の図は、ソフト制約とハード制約の下でベジェ曲線フィッティングを解決するための数学的フレームワークと、さまざまな制約を数学的なコスト関数 (ソフト制約) またはソリューション制約 (ソフト制約) に変換する方法を示しています。画像.png 上記は、コスト関数制約のよく使用される表現の例です。画像.png ベジェ曲線フィッティング軌跡以前の記事では、ベジェ曲線フィッティングのさまざまな利点を紹介しました。 - エンドポイント補間。ベジェ曲線は常に最初の制御点から始まり、最後の制御点で終わり、他の制御点を通過することはありません。
- 凸包。ベジェ曲線 ( ) は、すべての制御点によって定義される凸包内の制御点のセットによって完全に制限されます。
- スピードカーブ。ベジェ曲線( )の微分曲線′( )は速度曲線と呼ばれ、これも制御点によって定義されるベジェ曲線です。ここで制御点は∙( +1− )であり、は次数です。
- 固定時間間隔。ベジェ曲線は常に[0,1]で定義されます。
図1. 軌道の一部にベジェ曲線を当てはめた画像.png 上記の 2 つの式に対応するコードは次のとおりです。 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) ベジェ曲線を使用して曲線を表すための式とコード実装は上記で説明しました。ここで不足しているのは、指定された制御点です。制御点を取り込んで、ベジェ曲線式を使用して、指定された終点と終了点に描画する必要がある点の座標を計算します。次のコードは、4 つの制御点と 6 つの制御点を持つベジェ曲線の実装を示しています。曲線を描画するには、線上の 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) ベジェ曲線フィッティング法ができたので、次に行うことは、複数のベジェ曲線を生成して軌道を合成することです。コスト関数法 (ソフト制約) + 指定されたポイントを通過する必要がある + ポイントが連続して接続される必要があることを指定 (ハード制約) して、滑らかな軌道曲線を生成する必要があります。 図2. 障害物なし、境界軌道とスムーズな最適化 Figure_1.png 複数のベジェ曲線を生成するコードは次のとおりです。実際、原理は非常に単純です。複数のウェイポイントを指定すると、隣接する 2 つのウェイポイントごとにベジェ曲線が生成されます。コードは次のとおりです。 # 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 コスト関数の計算には、曲率コスト + 偏差コスト + 距離コスト + 連続性コストのほか、境界条件、軌道がチューブ内になければならないという不等式制約、および問題の最適化ソリューションが含まれます。具体的なコード実装は次のとおりです。 # 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() 障害物のあるベジェ曲線軌道生成障害物があるシーンでは、コスト関数を使用して、生成された曲線が障害物から離れるようにします。このようにして、安全な歩行軌道が得られます。以下は具体的なコード実装です。 optimizer_k のラムダ関数 f は、障害物の近くを通過するときの軌道のコストを見つけるために使用されます。Penalty1 と penalty2 は、障害物の近くを通過する曲線の特定のコスト値を見つけるために使用されます。
b.arc_len(granuality=10)+B.arc_len(granuality=10)+m_k + penalty1 + penalty2 は、軌道の全体的なコストです。 for ループは、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 飛行経路によるベジェ軌道生成ベジェ曲線フィッティングの利点により、ロボットの歩行可能な軌道を、重なり合う領域を持つ複数の凸多面体に変換できれば、軌道は完全に飛行経路内になります。 画像.png - 飛行経路は凸多角形で構成されています。
- 各立方体はベジェ曲線のセグメントに対応します。
- この曲線の制御点はポリゴン内に強制的に配置されます。
- 軌道はすべての点の凸包内に完全に収まります。
障害物マップを適用して実現可能な凸包廊下を生成する方法現在、凸包コリドーを生成する主な方法は 3 つあります。 平行凸クラスター拡張法グリッド マップから開始し、最小凸集合生成アルゴリズムを使用して凸多面体の生成を完了します。アルゴリズムの考え方は、最初に凸集合を取得し、次にそれを凸集合の表面に沿って拡張し、拡張後に凸集合検出を実行して、新しく拡張された集合が凸のままであるかどうかを判断することです。それ以上拡大できなくなるまで拡大を続け、凸集合のエッジ ポイントを抽出し、高速凸集合生成アルゴリズムを使用して凸多面体を生成します。このアルゴリズムの利点は、この拡張アイデアを使用して、空間全体を安全な多面体の体積で可能な限り埋めることができるため、得られる安全な通路が大きくなることです。しかし、計算量が比較的多く、計算時間も比較的長いという欠点もあります。この問題を解決するために、この記事では GPU アクセラレーションを使用して計算を高速化する方法を提案しています。 凸分解に基づく安全なチャネル生成凸分解に基づく安全チャネル生成方法では、楕円体の検出、多面体の検出、境界ボックス、および収縮という 4 つのステップで安全チャネルの生成が完了します。 半正定値計画法の反復領域拡張この方法では、多面体を得るために、まず、選択した点を中心とする単位球で構成される初期楕円体を構築します。次に、障害物を反復処理し、障害物ごとに、障害物に接し、楕円体から分離する超平面を生成します。繰り返しますが、これらの超平面は線形制約のセットを定義し、それらの交差は多面体になります。次に、その多面体の中で最大の楕円体を見つけ、この楕円体を使用して新しい分離超平面のセットを定義し、それによって新しい多面体を定義できます。分離超平面を生成する方法は、反復間で楕円体の体積が決して減少しないように選択されます。このプロセスは、楕円体の成長率が特定のしきい値を下回るまで繰り返すことができ、その時点で多面体と内接楕円体を返します。この方法は反復概念と収束判定基準を持ち、アルゴリズムの収束速度は初期楕円体と密接に関係しています。 図3. 半正定値計画法の反復領域拡張。各行は、楕円体の成長率がしきい値を下回るまで反復操作されます。画像.png この記事では、「半正定値計画法の反復領域拡張」手法を紹介します。具体的なコード実装は次のとおりです。 # 根据输入路径对空间进行凸分解def decomp(self, line_points: list[np.array], obs_points: list[np.array], visualize=True): # 最终结果decomp_polygons = list() # 构建输入障碍物点的kdtree obs_kdtree = KDTree(obs_points) # 进行空间分解for i in range(len(line_points) - 1): # 得到当前线段pf, pr = line_points[i], line_points[i + 1] print(pf) print(pr) # 构建初始多面体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) # 根据初始椭圆构建多面体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 # 构建初始多面体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)) # 构建多面体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]) # 计算旋转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) # 得到椭圆内的障碍物点inside_obs_points = ellipse.insidePoints(obs_points) # 对椭圆进行调整，使得全部障碍物点都在椭圆外while inside_obs_points: # 得到与椭圆距离最近的点closest_obs_point = ellipse.closestPoint(inside_obs_points) # 将最近点转到椭圆坐标系下closest_obs_point = np.dot(np.transpose(rotation), closest_obs_point - ellipse.d_) # 根据最近点，在椭圆长轴不变的情况下对短轴进行改变，使得，障碍物点在椭圆上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))) # 更新椭圆内部障碍物inside_obs_points = ellipse.insidePoints(inside_obs_points, include_bound=False) return ellipse # 进行多面体的构建def findPolygon(self, ellipse: Ellipse, init_polygon: Polygon, obs_points: list[np.array]) -> Polygon: # 多面体由多个超平面构成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) # 构建平面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 個の障害物ポイントが与えられた場合に 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() 凸包を持つ廊下の解凸包回廊によるスムーズな軌道生成。実行可能な凸包回廊は以前に解決されており、最適化ソリューションの不等式条件としてのハード制約として使用できます。必要な滑らかなパスと通過する必要があるポイント。この部分では、通過する必要があるポイントを等式制約として使用し、コスト関数を通じて滑らかなパスを実現できます。このようにして、ソフト制約とハード制約を備えた軌道生成フレームワークのあらゆる種類のスキルポイントを使用できます。 具体的なコード実装を見てみましょう。 # 进行优化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) # 得到分段数量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_) # 进行优化迭代max_inter = 10 cur_iter = 0 while cur_iter < max_inter: # 进行轨迹优化piece_wise_trajectory = self.optimizeIter(start_state, end_state, polygons, time_allocations, segment_num) # 对优化轨迹进行时间调整，以保证轨迹满足运动上限约束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 # 优化迭代def optimizeIter(self, start_state: np.array, end_state: np.array, polygons: list[Polygon], time_allocations: list, segment_num): # 构建目标函数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] ]) # 二次项系数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) # 一次项系数q = np.zeros((self.dim_ * segment_num * self.freedom_,)) # 构建约束条件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,)) # 构建等式约束条件（起点位置、速度、加速度；终点位置、速度；连接处的零、一、二阶导数） # 起点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] # 连接处的零阶导数相等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 # 连接处的一阶导数相等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 # 连接处的二阶导数相等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 # 构建不等式约束条件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!") # 根据参数进行轨迹解析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 まとめ：この論文では、ソフト制約とハード制約を備えた軌道最適化アルゴリズム フレームワークを紹介します。最初の部分では、最適解問題に対応するソフト制約とハード制約を数学的に表現する方法を紹介します。 2 番目の部分では、ベジェ曲線のコード実装を紹介し、具体的なコード実装と説明を示し、障害物のないシーン内のウェイポイントのみを指定して滑らかなベジェ軌道を生成するための簡単なソリューション コードを実装します。 3 番目の部分では、障害物がある場合にソリューションを最適化する方法と、コスト関数を通じて軌道に推力を適用して軌道を障害物から遠ざける方法のコード実装を示します。 4番目の部分は、ソフトとハードの制約を備えた最適な軌道生成のための包括的なソリューションフレームワークを提示する包括的な例です。详细的介绍了如何利用障碍物地图生成最大可行区域的凸包走廊，如何利用Bezier曲线的特性给定凸包两点生成路径一定在凸包中；以及如何利用代价行数来保证轨迹的光滑性、安全性，通过等式、不等式约束实现轨迹必须经过哪些点，某个点的运动状态如何。
この一連の記事は、後で、移動するオブジェクトに遭遇したときに、単一のロボットがどのように導入されているかを簡単に紹介します。ポジショニングと認識の内容に関しては、新しいシリーズを開いて、より最先端の技術的なポイントについて説明して紹介するかどうかを決定します。
最後に、この一連の記事は、次のGitリンクにあります。このプロジェクトは将来的に維持され続け、プロジェクトコード（C ++で実装する必要があります）は、ニーズに応じてライブラリを統合する必要があるかどうかを確認します。 元のリンク：https：//mp.weixin.qq.com/s/0evgyktxlzuj64l5jzmvug |
