无人机及车辆路径规划算法 无人机路径规划算法 混合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作为路径跟踪控制器确保车辆能够平稳、安全地沿着这条路径行驶。路径规划输入停车场地图、车辆起始位置与方向、目标停车位。输出一条无碰撞的路径包含一系列轨迹点。路径跟踪MPC配置定义状态变量如位置、速度、方向角等设定目标轨迹确定控制变量如转向角、油门/刹车力度设置代价函数如路径偏差、速度误差等。执行在每一时刻MPC根据当前状态预测未来的系统行为并计算出最优控制动作使车辆尽量贴近规划路径。实验结果实验表明混合A*与MPC结合的方法能够有效地解决自动驾驶汽车在复杂环境下的路径规划与跟踪问题。与单独使用PID或LQR相比MPC能够更好地处理非线性和约束条件提高了系统的鲁棒性和适应性。结论混合A*算法结合MPC、LQR或PID等路径跟踪方法为自动驾驶汽车和机器人提供了强大的路径规划与跟踪解决方案。每种方法都有其独特的优势和局限性因此在实际应用中应根据具体的系统特性和任务需求选择最合适的技术组合。未来的研究可以进一步探索如何在保证计算效率的同时提高路径规划与跟踪的整体性能。路径规划Path Planning路径规划是指在给定地图和起始点到目标点的情况下确定车辆应该采取的最佳路径。常见的路径规划算法包括 A* 算法、Dijkstra 算法、RRTRapidly-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与PidLQRLinear Quadratic Regulator是一种经典的线性控制器设计方法用于设计连续状态空间下的最优反馈控制器。而PIDProportional-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_cgteer_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## 最后↓↓↓更多有用知识代码见主页