Penjanaan trajektori di bawah kekangan lembut dan keras: penjelasan terperinci tentang teori dan kod

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) - 1return np.sum([bernstein_poly(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)

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) / offsetcontrol_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_pointsdef 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) * offsetcontrol_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_pointsdef 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 path one as per the approach suggested in# https://users.soe.ucsc.edu/~elkaim/Documents/camera_WCECS2008_IEEE_ICIAR_58.pdfdef 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] = yawlast_ax = ax[0]last_ay = ay[0]last_ayaw = ayaw[0]# for n waypoints, there are n-1 bezier curvesfor 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

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_multiplierelse:m_k = m_k/curve_penalty_dividerf = 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) <h2><span>带飞行走廊的Bezier轨迹生成</span></h2><p>得益于贝赛尔曲线拟合的优势，如果我们可以让机器人可行走的轨迹转成多个有重叠区域的凸多面体，那么轨迹完全位于飞行走廊内。</p><p style="text-align:center;"><img src="https://img.php.cn/upload/article/000/887/227/170524425539093.png" alt="Penjanaan trajektori di bawah kekangan lembut dan keras: penjelasan terperinci tentang teori dan kod"></p><p>image.png</p>

# 根据输入路径对空间进行凸分解def decomp(self, line_points: list[np.array], obs_points: list[np.array], visualize=True):# 最终结果decomp_polygons = list()# 构建输入障碍物点的kdtreeobs_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_hp_2 = pf - self.consider_range_ * dire_hpolygon_planes.append(Hyperplane(dire_h, p_1))polygon_planes.append(Hyperplane(-dire_h, p_2))# 得到垂直范围p_3 = pr + self.consider_range_ * direp_4 = pf - self.consider_range_ * direpolygon_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) / 2axes = 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) / 2ellipse = 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_pointswhile 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_pointspolygon = Polygon(polygon_planes)return polygon

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 = 10cur_iter = 0while cur_iter <h2><span>小结：</span></h2><p>这篇文章介绍了带软硬约束的轨迹优化算法框架。第一部份介绍了软硬约束对应到最优求解问题数学上如何表示。第二部份介绍了贝赛尔曲线的代码实现，给出了具体的代码实现和讲解；并针对没有障碍物场景只给定waypoint点，生成光滑的Bezier轨迹的朴素求解代码实现。第三部份给出了带障碍物情况下如何做最优化求解，如何通过代价函数的方式来给轨迹施加推力让轨迹远离障碍物的代码实现。第四部分是一个综合性的例子，把软硬约束最优轨迹生成的求解框架做了一个综合呈现。详细的介绍了如何利用障碍物地图生成最大可行区域的凸包走廊，如何利用Bezier曲线的特性给定凸包两点生成路径一定在凸包中；以及如何利用代价行数来保证轨迹的光滑性、安全性，通过等式、不等式约束实现轨迹必须经过哪些点，某个点的运动状态如何。<br>这一系列的文章已经进入结尾的阶段，后面会简单介绍在碰到移动的物体时候单机器人如何处理；以及在多个机器人运行环境如何协同，最后会给出一个Motion Planning的综合实现例子讲解实际环境数据输入、前端规划、后端轨迹生成。至于定位和感知部分的内容后面可以根据情况而定是否在开一个新的系列来讲解介绍，对于更前沿的技术点会跟进论文做些文章分享。<br>最后本系列文章的代码在以下git链接，这部分代码相对零碎主要是配合文章理论来讲的，里面很多片段直接来源于网络整合。后面这可项目会持续维护，把项目代码（应该是c++实现，更体系）、整合进来，根据需要在看看有没必要整合出一个库。</p><p style="text-align:center;"><img src="https://img.php.cn/upload/article/000/887/227/170524425687764.jpg" alt="Penjanaan trajektori di bawah kekangan lembut dan keras: penjelasan terperinci tentang teori dan kod"></p><p>原文链接：https://mp.weixin.qq.com/s/0EVgYKTxLzUj64L5jzMVug</p>