Code de ce projet :
github.com/liangwq/robot_motion_planing
Les articles précédents ont déjà introduit que l'essence des contraintes de trajectoire est d'effectuer un ajustement de trajectoire contrainte. L'entrée est la liste des waypoints, et il existe deux types de contraintes : les contraintes strictes et les contraintes souples. Les contraintes dites dures correspondent à la forme mathématique de la fonction de coût, et les contraintes dures correspondent aux conditions de contraintes de l'automne d'optimisation. Correspondant à la signification physique, afin d'obtenir une trajectoire sûre pour que le robot marche, il y a :
La figure ci-dessus montre le cadre mathématique pour résoudre l'ajustement de la courbe de Bézier sous des contraintes douces et dures, et comment convertir diverses contraintes en solution mathématique fonction de coûts (contraintes douces) ou de contraintes à résoudre (contraintes douces). image.png
Les exemples ci-dessus sont des exemples de plusieurs expressions de contraintes de fonction de coût couramment utilisées. image.png
Un article précédent a présenté divers avantages de l'ajustement de la courbe de Bézier :
Fig1. Une section de trajectoire est équipée d'une courbe de Bézier image.png
Le code correspondant aux deux expressions ci-dessus est implémenté comme suit :
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)
Pour utiliser une courbe de Bézier pour représenter un section de courbe, elle a été donnée ci-dessus. Une fois l'expression et l'implémentation du code terminées, ce qui manque maintenant, ce sont les points de contrôle donnés. Apportez les points de contrôle et utilisez les expressions de courbe de Bézier pour calculer les coordonnées des points qui doivent le faire. être dessiné dans le point final et le point final donnés. Le code suivant donne l'implémentation de la courbe de Bézier de 4 points de contrôle et 6 points de contrôle afin de tracer la courbe, 170 points en ligne doivent être calculés ; Le code est le suivant :
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)
Avec la méthode d'ajustement d'une courbe de Bézier, la prochaine chose à faire est de savoir comment générer plusieurs courbes de Bézier pour synthétiser une trajectoire et cela doit se faire via la méthode de la fonction de coût (contraintes souples) + ; doit passer par les points spécifiés + formule Les connexions de points doivent être continues (contraintes dures) pour générer une courbe de trajectoire lisse.
Fig2. Pas d'obstacles, optimisation en douceur avec trajectoire bondaire Figure_1.png
Le code de génération de courbe de Bézier multi-segments est le suivant. En fait, le principe est très simple étant donné plusieurs waypoints, chacun deux wayponit adjacents. génère un segment de courbe de Bezizer, le code est le suivant :
# 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
Le calcul de la fonction de coût comprend : coût de courbure + coût de déviation + coût de distance + coût de continuité, ainsi que les conditions aux limites, les contraintes d'inégalité selon lesquelles la trajectoire doit être à l'intérieur du tube, et solution d'optimisation du problème. L'implémentation spécifique du code est la suivante :
# Objective function of cost to be minimizeddef 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 continuityt = np.zeros((len(k)))dk = self.calc_d(t, k)absolute_dk = np.absolute(dk)continuity_cost = 10.0 * np.mean(absolute_dk)# curvature costabsolute_k = np.absolute(k)curvature_cost = 14.0 * np.mean(absolute_k)# cost of deviation from input waypointsabsolute_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 minimizedef 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.xfor 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()
une scène avec des obstacles, et la fonction de coût est utilisée pour maintenir la courbe générée à l'écart des obstacles. Afin d'obtenir une trajectoire pouvant être parcourue en toute sécurité, voici l'implémentation spécifique du code. La fonction lambda f dans optimiseur_k consiste à résoudre le coût lorsque la trajectoire passe près de l'obstacle. pénalité1 et pénalité2 doivent trouver la valeur de coût spécifique de la courbe passant près de l'obstacle ; arc_len(granuality= 10)+m_k + pénalité1 + pénalité2 est le coût global de la trajectoire. La partie boucle for utilise l'optimisation et la minimisation de Scipy pour résoudre la trajectoire.
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="Génération de trajectoire sous contraintes douces et dures : explication détaillée de la théorie et du code"></p><p>image.png</p>
生成凸包走廊的方法目前有以下三大类的方法：
从栅格地图出发，利用最小凸集生成算法，完成凸多面体的生成。其算法的思想是首先获得一个凸集，再沿着凸集的表面进行扩张，扩张之后再进行凸集检测，判断新扩张的集合是否保持为凸。一直扩张到不能再扩张为止，再提取凸集的边缘点，利用快速凸集生成算法，生成凸多面体。该算法的好处在于可以利用这种扩张的思路，将安全的多面体的体积尽可能的充满整个空间，因此获得的安全通道更大。但其也具有一定的缺点，就是计算量比较大，计算所需要的时间比较长，为了解决这个问题，在该文章中，又提出了采用GPU加速的方法，来加速计算。
基于凸分解的安全通道生成方法由四个步骤完成安全通道的生成，分别为：找到椭球、找到多面体、边界框、收缩。
为了获取多面体，这个方法首先构造一个初始椭球，由一个以选定点为中心的单位球组成。然后，遍历障碍物，为每个障碍物生成一个超平面，该超平面与障碍物相切并将其与椭球分开。再次，这些超平面定义了一组线性约束，它们的交集是一个多面体。然后，可以在那个多面体中找到一个最大的椭球，使用这个椭球来定义一组新的分离超平面，从而定义一个新的多面体。选择生成分离超平面的方法，这样椭圆体的体积在迭代之间永远不会减少。可以重复这个过程，直到椭圆体的增长率低于某个阈值，此时我们返回多面体和内接椭圆体。这个方法具有迭代的思想，并且具有收敛判断的标准，算法的收敛快慢和初始椭球具有很大的关系。
Fig3.半定规划的迭代区域膨胀。每一行即为一次迭代操作，直到椭圆体的增长率低于阈值。image.png
这篇文章介绍的是“半定规划的迭代区域膨胀”方法，具体代码实现如下：
# 根据输入路径对空间进行凸分解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
上面图是给定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 = 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="Génération de trajectoire sous contraintes douces et dures : explication détaillée de la théorie et du code"></p><p>原文链接：https://mp.weixin.qq.com/s/0EVgYKTxLzUj64L5jzMVug</p>
Ce qui précède est le contenu détaillé de. pour plus d'informations, suivez d'autres articles connexes sur le site Web de PHP en chinois!
Quelles compétences sont nécessaires pour travailler dans l'industrie PHP ?
Raisons pour lesquelles le chargement du CSS a échoué
Que faire si le chargement de la DLL échoue
Quelle est la différence entre un routeur et un chat ?
La différence entre mysql et sql_server
Exigences de configuration minimales pour le système win10
Que faire avec la carte vidéo
Comment acheter et vendre du Bitcoin sur okex
![[Web front-end] Démarrage rapide de Node.js](https://img.php.cn/upload/course/000/000/067/662b5d34ba7c0227.png)
