基于卡尔曼滤波的MPC汽车控制器研究（Python代码实现）

最新推荐文章于 2025-12-12 16:26:10 发布

原创最新推荐文章于 2025-12-12 16:26:10 发布 · 67 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#python #汽车 #开发语言

本文介绍了卡尔曼滤波与模型预测控制(MPC)在汽车领域的结合应用，如何通过滤波算法估计汽车状态并优化控制指令，提升汽车性能和安全性。文中还展示了Python代码实例，展示了如何实现在动态系统中的控制器设计。

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Python代码实现

💥1 概述

基于卡尔曼滤波的MPC汽车控制器是一种在汽车控制中应用卡尔曼滤波和模型预测控制（MPC）的方法。卡尔曼滤波是一种适用于线性动态系统的滤波算法，它可以通过对系统状态进行递推和更新，结合测量数据和模型预测，提供对系统状态的估计。而MPC是一种基于模型的控制策略，它通过优化问题求解，结合系统模型和控制目标，预测未来一段时间内的系统响应，并生成最优控制指令。

将卡尔曼滤波与MPC结合应用于汽车控制，可以提高控制器对环境变化和测量噪声的鲁棒性。具体来说，卡尔曼滤波可以用于估计汽车的状态，例如位置、速度和姿态等，提供更准确的状态信息给MPC控制器。MPC控制器则可以根据估计的状态信息和控制目标，动态调整控制指令，实现对汽车的精确控制。

这种基于卡尔曼滤波的MPC汽车控制器在自动驾驶、车辆稳定性控制和路径跟踪等方面有着广泛的应用。通过融合滤波和优化控制的方法，可以提高汽车的行驶性能、安全性和舒适性。然而，该方法的具体研究和实施可能因具体应用场景和需求而有所不同，需要进一步的研究和开发。

📚2 运行结果

直接去掉外框，后续结果图都一律去掉外框。

部分代码：

# SIMULATOR DISPLAY

    # Total Figure
    fig = plt.figure(figsize=(FIG_SIZE[0], FIG_SIZE[1]))
    gs = gridspec.GridSpec(14,8)

    # Elevator plot settings.
    ax = fig.add_subplot(gs[:, :3])

    plt.xlim(0, 8)
    ax.set_ylim([0, 31])
    plt.xticks([])
    #plt.yticks(np.arange(0,31,3))
    plt.title('Kalman 1D')

    # Time display.
    time_text = ax.text(6, 0.5, '', fontsize=15)

    # Main plot info.
    car_l, car_r = ax.plot([], [], 'k-', [], [], 'k-')
    car_t, car_b = ax.plot([], [], 'k-', [], [], 'k-')

    # V Estimate plot.
    ax2     = fig.add_subplot(gs[0:4, 4:])
    __,     = ax2.plot(est_data_t, v_est_data, '-b')
    v_est,  = ax2.plot([], [], '-r')
    v_real, = ax2.plot(v_real_data_x, v_real_data_y, 'k--')
    plt.title('V Estimate')
    # plt.xticks([])
    # plt.xticks([0, v_real_data_x[-1]])
    plt.xlim(0, v_real_data_x[-1])

    # ax2.set_xticks([0, est_data_t[-1]])
    # ax2.set_xlim(0, est_data_t[-1])
    # ax2.set_ylim([0,4])
    # ax2.set_yticks([0,2,4])

    # X Estimate plot.
    ax3 = fig.add_subplot(gs[5:9, 4:])
    ___, = ax3.plot(est_data_t, x_est_data, '-b')
    x_est, = ax3.plot([], [], '-r')
    plt.title('X Estimate Error')
    # plt.xticks([])
    # plt.xticks([0, v_real_data_x[-1]])
    plt.xlim(0, v_real_data_x[-1])

    # ax3.set_xticks([0, est_data_t[-1]])
    # ax3.set_xlim(0, est_data_t[-1])
    # ax3.set_ylim(-4,4)
    # ax3.set_yticks([-4,0,4])

    # real X vs measure vs X estimate
    ax4         = fig.add_subplot(gs[10:14, 4:])
    ___,        = ax4.plot(t, state, '-g')                      # real X
    __,         = ax4.plot(est_data_t, X_measured, '-b')        # measure X
    ____,       = ax4.plot(est_data_t, X_estimated, '-c')        # estimated X

    plt.title('real X vs X estimate')
    plt.xlim(0, est_data_t[-1])
    plt.legend(['real X', 'measure X', 'estimated X'])
    # ax4.set_ylim(-4,4)

    def update_plot(num):
        t_loc = int(t[num])
        if state[num] > 18:
            ax.set_ylim([0+state[num]-18, 31+state[num]-18])
            time_text.set_position([6,state[num]-18+0.5])

        # Car.
        car_l.set_data([2, 2],[state[num], state[num]+10])
        car_r.set_data([6, 6],[state[num], state[num]+10])
        car_t.set_data([2, 6],[state[num]+10, state[num]+10])
        car_b.set_data([2, 6],[state[num], state[num]])

        # if int(t[num]) < 20:
        #     ax2.set_xlim([0,est_data_t[t_loc+1]])
        #     ax3.set_xlim([0,est_data_t[t_loc+1]])
        # else:
        #     ax2.set_xlim([est_data_t[t_loc-20],est_data_t[t_loc+1]])
        #     ax3.set_xlim([est_data_t[t_loc-20],est_data_t[t_loc+1]])

        v_est.set_data(est_data_t[:t_loc], v_est_data[:t_loc])
        x_est.set_data(est_data_t[:t_loc], x_est_data[:t_loc])

        # Timer.
        time_text.set_text(str(np.round(t[num], 1)) + 's')

        return car_l, car_r, car_t, car_b, time_text


    print("Compute Time: ", round(time.perf_counter() - start, 3), "seconds.")
    # Animation.
    car_ani = animation.FuncAnimation(fig, update_plot, frames=range(1,len(t)), interval=100, repeat=False, blit=False)
    #car_ani.save('lines.mp4')

    plt.show()

# SIMULATOR DISPLAY

# Total Figure
fig = plt.figure(figsize=(FIG_SIZE[0], FIG_SIZE[1]))
gs = gridspec.GridSpec(14,8)

# Elevator plot settings.
ax = fig.add_subplot(gs[:, :3])

plt.xlim(0, 8)
ax.set_ylim([0, 31])
plt.xticks([])
#plt.yticks(np.arange(0,31,3))
plt.title('Kalman 1D')

# Time display.
time_text = ax.text(6, 0.5, '', fontsize=15)

# Main plot info.
car_l, car_r = ax.plot([], [], 'k-', [], [], 'k-')
car_t, car_b = ax.plot([], [], 'k-', [], [], 'k-')

# V Estimate plot.
ax2     = fig.add_subplot(gs[0:4, 4:])
__,     = ax2.plot(est_data_t, v_est_data, '-b')
v_est,  = ax2.plot([], [], '-r')
v_real, = ax2.plot(v_real_data_x, v_real_data_y, 'k--')
plt.title('V Estimate')
# plt.xticks([])
# plt.xticks([0, v_real_data_x[-1]])
plt.xlim(0, v_real_data_x[-1])

# ax2.set_xticks([0, est_data_t[-1]])
# ax2.set_xlim(0, est_data_t[-1])
# ax2.set_ylim([0,4])
# ax2.set_yticks([0,2,4])

# X Estimate plot.
ax3 = fig.add_subplot(gs[5:9, 4:])
___, = ax3.plot(est_data_t, x_est_data, '-b')
x_est, = ax3.plot([], [], '-r')
plt.title('X Estimate Error')
# plt.xticks([])
# plt.xticks([0, v_real_data_x[-1]])
plt.xlim(0, v_real_data_x[-1])

# ax3.set_xticks([0, est_data_t[-1]])
# ax3.set_xlim(0, est_data_t[-1])
# ax3.set_ylim(-4,4)
# ax3.set_yticks([-4,0,4])

# real X vs measure vs X estimate
ax4         = fig.add_subplot(gs[10:14, 4:])
___,        = ax4.plot(t, state, '-g')                      # real X
__,         = ax4.plot(est_data_t, X_measured, '-b')        # measure X
____,       = ax4.plot(est_data_t, X_estimated, '-c')        # estimated X

plt.title('real X vs X estimate')
plt.xlim(0, est_data_t[-1])
plt.legend(['real X', 'measure X', 'estimated X'])
# ax4.set_ylim(-4,4)

def update_plot(num):
    t_loc = int(t[num])
    if state[num] > 18:
        ax.set_ylim([0+state[num]-18, 31+state[num]-18])
        time_text.set_position([6,state[num]-18+0.5])

    # Car.
    car_l.set_data([2, 2],[state[num], state[num]+10])
    car_r.set_data([6, 6],[state[num], state[num]+10])
    car_t.set_data([2, 6],[state[num]+10, state[num]+10])
    car_b.set_data([2, 6],[state[num], state[num]])

    # if int(t[num]) < 20:
    #     ax2.set_xlim([0,est_data_t[t_loc+1]])
    #     ax3.set_xlim([0,est_data_t[t_loc+1]])
    # else:
    #     ax2.set_xlim([est_data_t[t_loc-20],est_data_t[t_loc+1]])
    #     ax3.set_xlim([est_data_t[t_loc-20],est_data_t[t_loc+1]])

    v_est.set_data(est_data_t[:t_loc], v_est_data[:t_loc])
    x_est.set_data(est_data_t[:t_loc], x_est_data[:t_loc])

    # Timer.
    time_text.set_text(str(np.round(t[num], 1)) + 's')

    return car_l, car_r, car_t, car_b, time_text


print("Compute Time: ", round(time.perf_counter() - start, 3), "seconds.")
# Animation.
car_ani = animation.FuncAnimation(fig, update_plot, frames=range(1,len(t)), interval=100, repeat=False, blit=False)
#car_ani.save('lines.mp4')

plt.show()