混合A*运动规划算法:路径规划和路径跟踪-MPC-LQR-PID算法

简介: 混合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
相关文章
|
17天前
|
监控 算法 数据安全/隐私保护
基于三帧差算法的运动目标检测系统FPGA实现,包含testbench和MATLAB辅助验证程序
本项目展示了基于FPGA与MATLAB实现的三帧差算法运动目标检测。使用Vivado 2019.2和MATLAB 2022a开发环境,通过对比连续三帧图像的像素值变化,有效识别运动区域。项目包括完整无水印的运行效果预览、详细中文注释的代码及操作步骤视频,适合学习和研究。
|
5月前
|
存储 SQL 算法
LeetCode题目113:多种算法实现 路径总和ll
LeetCode题目113:多种算法实现 路径总和ll
|
21天前
|
数据采集 监控 安全
厂区地图导航制作:GIS技术与路径导航算法融合
在智能化、数字化时代,GIS技术为厂区的运营管理带来了革命性变化。本文探讨了如何利用GIS技术,通过数据采集、地图绘制、路径规划、位置定位和信息查询等功能,打造高效、精准的智能厂区地图导航系统,提升企业的竞争力和管理水平。
28 0
厂区地图导航制作:GIS技术与路径导航算法融合
|
28天前
|
算法 数据可视化 新制造
Threejs路径规划_基于A*算法案例完整版
这篇文章详细介绍了如何在Three.js中完整实现基于A*算法的路径规划案例,包括网格构建、路径寻找算法的实现以及路径可视化展示等方面的内容。
56 0
Threejs路径规划_基于A*算法案例完整版
|
25天前
|
算法 决策智能
基于prim算法求出网络最小生成树实现网络社团划分和规划
该程序使用MATLAB 2022a版实现路线规划,通过排序节点权值并运用Prim算法生成最小生成树完成网络规划。程序基于TSP问题,采用遗传算法与粒子群优化算法进行路径优化。遗传算法通过编码、选择、交叉及变异操作迭代寻优;粒子群优化算法则通过模拟鸟群觅食行为,更新粒子速度和位置以寻找最优解。
|
28天前
|
存储 算法 机器人
Threejs路径规划_基于A*算法案例V2
这篇文章详细介绍了如何在Three.js中使用A*算法进行高效的路径规划,并通过三维物理电路的实例演示了路径计算和优化的过程。
52 0
|
3月前
|
自然语言处理 算法
HanLP — HMM隐马尔可夫模型 - 路径规划算法 - 求解最短路径 - 维特比(Viterbi)算法
HanLP — HMM隐马尔可夫模型 - 路径规划算法 - 求解最短路径 - 维特比(Viterbi)算法
55 0
HanLP — HMM隐马尔可夫模型 - 路径规划算法 - 求解最短路径 - 维特比(Viterbi)算法
|
3月前
|
算法
基于多路径路由的全局感知网络流量分配优化算法matlab仿真
本文提出一种全局感知网络流量分配优化算法,针对现代网络中多路径路由的需求,旨在均衡分配流量、减轻拥塞并提升吞吐量。算法基于网络模型G(N, M),包含N节点与M连接,并考虑K种不同优先级的流量。通过迭代调整每种流量在各路径上的分配比例,依据带宽利用率um=Σ(xm,k * dk) / cm来优化网络性能,确保高优先级流量的有效传输同时最大化利用网络资源。算法设定收敛条件以避免陷入局部最优解。
|
3月前
|
算法 定位技术
路径规划算法 - 求解最短路径 - A*(A-Star)算法
路径规划算法 - 求解最短路径 - A*(A-Star)算法
59 0
|
3月前
|
算法
路径规划算法 - 求解最短路径 - Dijkstra(迪杰斯特拉)算法
路径规划算法 - 求解最短路径 - Dijkstra(迪杰斯特拉)算法
61 0