混合Astar运动规划算法 路径规划和路径跟踪 MPC算法 LQR算法 PID算法

用于自动驾驶车辆的运动规划算法包括路径规划和路径跟踪。

混合A*运动规划算法：路径规划与路径跟踪

引言

在自动驾驶汽车、机器人导航等领域，路径规划和路径跟踪是两个核心的技术挑战。混合A*（Hybrid A*）算法是一种专为非完整约束（non-holonomic constraints）车辆设计的路径规划算法，特别适合于具有最小转弯半径要求的车辆。路径规划完成后，需要一个有效的路径跟踪控制器来使车辆沿着规划好的路径行驶。常见的路径跟踪方法包括模型预测控制（MPC）、线性二次调节器（LQR）以及比例-积分-微分控制（PID）。本文将详细探讨混合A*算法及其与MPC、LQR和PID相结合的方法。

混合A*算法简介

混合A算法结合了传统的A搜索算法和车辆动力学模型，能够在考虑车辆物理限制的同时找到一条可行路径。其主要特点包括：

  • 节点扩展：每个节点不仅表示一个位置，还包含了车辆的方向和曲率信息。
  • 启发式函数：除了距离目标点的距离外，还考虑了车辆到达目标点所需的转向角度变化。
  • 碰撞检测：使用车辆的几何形状和最小转弯半径来判断路径是否可行。
路径规划与路径跟踪

路径规划负责计算从起点到终点的一条无碰撞路径，而路径跟踪则确保车辆能够准确地跟随这条路径。接下来，我们将讨论几种常用的路径跟踪方法。

模型预测控制（MPC）
  • 原理：MPC是一种多步优化方法，通过预测未来一段时间内的系统行为来决定当前时刻的最佳控制动作。它能够在满足一系列约束条件的同时最小化某个代价函数。
  • 优势：能够处理复杂的约束条件，如速度限制、加速度限制等；适用于非线性系统。
  • 挑战：计算量大，尤其是在状态空间维数较高时；需要精确的系统模型。
线性二次调节器（LQR）
  • 原理：LQR是一种基于线性系统的最优控制方法，通过求解一个Riccati方程来找到一个反馈控制器，使得系统的状态能够快速稳定地收敛到期望值。
  • 优势：计算简单，易于实现；对于线性系统非常有效。
  • 局限性：仅适用于线性系统；对初始状态敏感。
比例-积分-微分控制（PID）
  • 原理：PID控制器是最常见的一种反馈控制器，通过比例、积分和微分三个环节来调节系统的输出，使其尽可能接近设定值。
  • 优势：简单易用，适用范围广；参数调整相对直观。
  • 局限性：对于复杂系统，PID控制器可能难以达到理想的控制效果；容易受到噪声干扰。
案例分析：混合A*与MPC结合

假设我们正在为一辆自动驾驶汽车设计路径规划和跟踪系统。首先，使用混合A*算法生成一条从停车场入口到停车位的路径。然后，利用MPC作为路径跟踪控制器，确保车辆能够平稳、安全地沿着这条路径行驶。

  1. 路径规划：

    • 输入：停车场地图、车辆起始位置与方向、目标停车位。
    • 输出：一条无碰撞的路径，包含一系列轨迹点。

  2. 路径跟踪：

    • MPC配置：定义状态变量（如位置、速度、方向角等），设定目标轨迹，确定控制变量（如转向角、油门/刹车力度），设置代价函数（如路径偏差、速度误差等）。
    • 执行：在每一时刻，MPC根据当前状态预测未来的系统行为，并计算出最优控制动作，使车辆尽量贴近规划路径。
实验结果

实验表明，混合A*与MPC结合的方法能够有效地解决自动驾驶汽车在复杂环境下的路径规划与跟踪问题。与单独使用PID或LQR相比，MPC能够更好地处理非线性和约束条件，提高了系统的鲁棒性和适应性。

结论

混合A*算法结合MPC、LQR或PID等路径跟踪方法，为自动驾驶汽车和机器人提供了强大的路径规划与跟踪解决方案。每种方法都有其独特的优势和局限性，因此在实际应用中应根据具体的系统特性和任务需求选择最合适的技术组合。未来的研究可以进一步探索如何在保证计算效率的同时，提高路径规划与跟踪的整体性能。

  • 路径规划（Path Planning）：路径规划是指在给定地图和起始点到目标点的情况下，确定车辆应该采取的最佳路径。常见的路径规划算法包括 A* 算法、Dijkstra 算法、RRT（Rapidly-exploring Random Tree）等。
  • 路径跟踪（Path Tracking）：路径跟踪是指车辆在实际行驶过程中，根据预先规划好的路径进行控制，使车辆能够沿着设定的路径行驶。常见的路径跟踪算法包括基于模型的控制方法（如PID控制器）、模型预测控制（Model Predictive Control, MPC）等。

在这里插入图片描述

混合A*

  • 我们描述了一种实用的路径规划算法，为在未知环境中运行的自动驾驶车辆生成平滑的路径。
  • 该算法通过机器人的传感器在线检测障碍物。这项工作是受到2007年DARPA城市挑战赛的启发，并在该比赛中进行了实验验证，其中机器人车辆必须自主导航停车场。
  • 我们的方法包括两个主要步骤。第一步使用变体的著名A搜索算法，应用于车辆的三维运动状态空间，但采用修改后的状态更新规则，将车辆的连续状态捕捉到A的离散节点中（从而保证路径的运动可行性）.
  • 然后，第二步通过数值非线性优化改进解决方案的质量，得到局部（经常是全局）最优解。
  • 本文描述的路径规划算法被斯坦福赛车队的机器人Junior在城市挑战赛中使用。Junior在复杂的一般路径规划任务中表现出色，如导航停车场和在封闭道路上执行U型转弯，典型的完整循环重规划时间为50-300ms

。

在这里插入图片描述

线性模型预测控制（Linear Model Predictive Control，简称LMPC）

  • 算法是一种基于模型预测的控制算法，用于解决连续状态和动作空间下的轨迹跟踪问题。该算法将轨迹跟踪问题转化为一个优化问题，通过优化控制序列来最小化当前状态与目标状态之间的误差。
  • LMPC算法的基本思想是构建一个线性动态系统模型，并使用该模型进行多步预测。在每个时间步长，LMPC算法使用预测结果计算一个最优的控制序列，执行第一个控制动作，并重新计算当前状态。然后，算法采用新的当前状态和控制序列重新计算多步预测，并重复这个过程直到到达目标状态为止。
  • LMPC算法的优点是可以处理非线性和非光滑的运动模式，并且可以在不同的环境中适应不同的控制任务。然而，由于LMPC算法需要在线解决一个优化问题，因此计算开销较大，并且需要足够快的计算速度以保证实时性。

在这里插入图片描述

LQR与Pid

  • LQR（Linear Quadratic Regulator）是一种经典的线性控制器设计方法，用于设计连续状态空间下的最优反馈控制器。而PID（Proportional-Integral-Derivative）是一种常见的经典控制器，用于处理简单的线性和部分非线性控制问题。
  • LQR + PID算法是将LQR和PID两种控制器结合起来使用的一种控制策略。该算法的基本思想是使用LQR控制器来提供系统的快速稳定性和优化性能，同时使用PID控制器来处理系统的静态偏差和纠正快速变化的扰动。
  • 具体来说，LQR控制器通过优化系统的状态反馈增益矩阵，使系统的性能指标最小化。这种最优化的设计使得LQR控制器能够在系统响应速度和稳定性之间找到一个平衡点。
  • 然而，LQR控制器通常无法完全消除系统的静态偏差，而且对于快速变化的扰动响应不够灵敏。为了解决这些问题，PID控制器可以添加到LQR控制器中。PID控制器可以根据系统误差的比例、积分和导数来调整控制信号，以实现更好的静态补偿和快速扰动响应。

在这里插入图片描述

通过将LQR和PID控制器结合起来，LQR + PID算法可以综合利用两者的优点，提供更快的响应速度、更好的稳定性和更好的静态补偿能力。

混合A star 代码

def calc_rs_path_cost(rspath): cost = 0.0 for lr in rspath.lengths: if lr >= 0: cost += 1 else: cost += abs(lr) * C.BACKWARD_COST for i in range(len(rspath.lengths) - 1): if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0: cost += C.GEAR_COST for ctype in rspath.ctypes: if ctype != "S": cost += C.STEER_ANGLE_COST * abs(C.MAX_STEER) nctypes = len(rspath.ctypes) ulist = [0.0 for _ in range(nctypes)] for i in range(nctypes): if rspath.ctypes[i] == "R": ulist[i] = -C.MAX_STEER elif rspath.ctypes[i] == "WB": ulist[i] = C.MAX_STEER for i in range(nctypes - 1): cost += C.STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i]) return cost def calc_hybrid_cost(node, hmap, P): cost = node.cost + \ C.H_COST * hmap[node.xind - P.minx][node.yind - P.miny] return cost def calc_motion_set(): s = np.arange(C.MAX_STEER / C.N_STEER, C.MAX_STEER, C.MAX_STEER / C.N_STEER) steer = list(s) + [0.0] + list(-s) direc = [1.0 for _ in range(len(steer))] + [-1.0 for _ in range(len(steer))] steer = steer + steer return steer, direc def is_same_grid(node1, node2): if node1.xind != node2.xind or \ node1.yind != node2.yind or \ node1.yawind != node2.yawind: return False return True def calc_index(node, P): ind = (node.yawind - P.minyaw) * P.xw * P.yw + \ (node.yind - P.miny) * P.xw + \ (node.xind - P.minx) return ind def calc_parameters(ox, oy, xyreso, yawreso, kdtree): minx = round(min(ox) / xyreso) miny = round(min(oy) / xyreso) maxx = round(max(ox) / xyreso) maxy = round(max(oy) / xyreso) xw, yw = maxx - minx, maxy - miny minyaw = round(-C.PI / yawreso) - 1 maxyaw = round(C.PI / yawreso) yaww = maxyaw - minyaw return Para(minx, miny, minyaw, maxx, maxy, maxyaw, xw, yw, yaww, xyreso, yawreso, ox, oy, kdtree)

线性LQR控制代码

class TrajectoryAnalyzer: def __init__(self, x, y, yaw, k): self.x_ = x self.y_ = y self.yaw_ = yaw self.k_ = k self.ind_old = 0 self.ind_end = len(x) def ToTrajectoryFrame(self, vehicle_state): """ errors to trajectory frame theta_e = yaw_vehicle - yaw_ref_path e_cg = lateral distance of center of gravity (cg) in frenet frame :param vehicle_state: vehicle state (class VehicleState) :return: theta_e, e_cg, yaw_ref, k_ref """ x_cg = vehicle_state.x y_cg = vehicle_state.y yaw = vehicle_state.yaw # calc nearest point in ref path dx = [x_cg - ix for ix in self.x_[self.ind_old: self.ind_end]] dy = [y_cg - iy for iy in self.y_[self.ind_old: self.ind_end]] ind_add = int(np.argmin(np.hypot(dx, dy))) dist = math.hypot(dx[ind_add], dy[ind_add]) # calc lateral relative position of vehicle to ref path vec_axle_rot_90 = np.array([[math.cos(yaw + math.pi / 2.0)], [math.sin(yaw + math.pi / 2.0)]]) vec_path_2_cg = np.array([[dx[ind_add]], [dy[ind_add]]]) if np.dot(vec_axle_rot_90.T, vec_path_2_cg) > 0.0: e_cg = 1.0 * dist # vehicle on the right of ref path else: e_cg = -1.0 * dist # vehicle on the left of ref path # calc yaw error: theta_e = yaw_vehicle - yaw_ref self.ind_old += ind_add yaw_ref = self.yaw_[self.ind_old] theta_e = pi_2_pi(yaw - yaw_ref) # calc ref curvature k_ref = self.k_[self.ind_old] return theta_e, e_cg, yaw_ref, k_ref class LatController: """ Lateral Controller using LQR """ def ComputeControlCommand(self, vehicle_state, ref_trajectory): """ calc lateral control command. :param vehicle_state: vehicle state :param ref_trajectory: reference trajectory (analyzer) :return: steering angle (optimal u), theta_e, e_cg """ ts_ = ts e_cg_old = vehicle_state.e_cg theta_e_old = vehicle_state.theta_e theta_e, e_cg, yaw_ref, k_ref = \ ref_trajectory.ToTrajectoryFrame(vehicle_state) matrix_ad_, matrix_bd_ = self.UpdateMatrix(vehicle_state) matrix_state_ = np.zeros((state_size, 1)) matrix_r_ = np.diag(matrix_r) matrix_q_ = np.diag(matrix_q) matrix_k_ = self.SolveLQRProblem(matrix_ad_, matrix_bd_, matrix_q_, matrix_r_, eps, max_iteration) matrix_state_[0][0] = e_cg matrix_state_[1][0] = (e_cg - e_cg_old) / ts_ matrix_state_[2][0] = theta_e matrix_state_[3][0] = (theta_e - theta_e_old) / ts_ steer_angle_feedback = -(matrix_k_ @ matrix_state_)[0][0] steer_angle_feedforward = self.ComputeFeedForward(k_ref) steer_angle = steer_angle_feedback + steer_angle_feedforward return steer_angle, theta_e, e_cg

teer_angle_feedback = -(matrix_k_ @ matrix_state_)[0][0]

steer_angle_feedforward = self.ComputeFeedForward(k_ref) steer_angle = steer_angle_feedback + steer_angle_feedforward return steer_angle, theta_e, e_cg
## 最后，↓↓↓更多有用知识代码见主页！