PID横向控制和仿真实现

这篇具有很好参考价值的文章主要介绍了PID横向控制和仿真实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

1. PID介绍

PID是一种常见的控制算法,全称为Proportional-Integral-Derivative,即比例-积分-微分控制器。PID控制器是一种线性控制器,它将设定值与实际值进行比较,根据误差的大小,控制器会相应地调整系统的比例、积分和微分系数,以减小误差。

PID控制器的基本公式为:

u ( t ) = K p ∗ e ( t ) + K i ∗ ∫ 0 t e ( τ ) d τ + K d ∗ d d t e ( t ) (1) u(t) = K_p * e(t) + K_i * \int^{t}_{0}{e(\tau)} d\tau + K_d *\frac{d}{dt} e(t) \tag{1} u(t)=Kpe(t)+Ki0te(τ)dτ+Kddtde(t)(1)

其中,$u(t) 是控制器的输出, 是控制器的输出, 是控制器的输出,e(t)$ 是误差信号(设定值与实际值之差), K p K_p Kp K i K_i Ki K d K_d Kd是控制器的比例、积分和微分系数。

PID控制器在工程、科学和工业等领域中有着广泛的应用。例如,在汽车定速巡航系统、空调系统、工业自动化生产线等系统中都可以看到PID控制器的身影。此外,PID控制器还广泛应用于机器人控制、化工生产、航天器控制等领域。

将公式(1)转换为离散形式,则有

u ( n ) = K p ∗ e ( n ) + K i ∗ T ∗ ∑ i = 0 n e ( i ) + K d ∗ e ( n ) − e ( n − 1 ) T (2) u(n) = K_p * e(n) + K_i * T*\sum^{n}_{i=0}{e(i)} + K_d *\frac{e(n)-e(n-1)}{T} \tag{2} u(n)=Kpe(n)+KiTi=0ne(i)+KdTe(n)e(n1)(2)

其中 T T T为采样周期,由此公式可以得到PID代码的实现如下

#PID.py

class PIDController:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.previous_error = 0
        self.integral = 0


    def cal_output(self, error, dt):
        derivative = error - self.previous_error
        u = self.Kp * error + self.Ki * self.integral * dt + self.Kd * derivative / dt
        self.integral += error
        self.previous_error = error
        return u

2. PID横向控制原理

在自动驾驶横向控制中,主要通过控制方向盘的角度来控制车辆的横向距离误差,因此我们可以通过横向距离误差 e y e_y ey来作为PID的输入,输出可以作为方向盘转角 δ f \delta_f δf,结合之前我们的车辆运动学模型(这里我们假设方向盘转角与前轮转角比是1),横向误差计算的几何结构如下图所示:

PID横向控制和仿真实现,自动驾驶-控制,pid,自动驾驶-控制,控制,汽车

图中

  • P P P:当前车辆的目标点车
  • l d l_d ld:车辆后轴中心点 A A A F F F的距离
  • θ \theta θ l d l_d ld与车轴的夹角
  • φ \varphi φ:车辆的航向角
  • e y e_y ey:横向偏差

A A A P P P的坐标可以计算得

β = a r c t a n y 1 − y 0 x 1 − x 0 (3) \beta = arctan\frac{y_1-y_0}{x_1-x_0} \tag{3} β=arctanx1x0y1y0(3)

由图中的几何关系,我们可以得到

e y = l d s i n θ = l d s i n ( β − φ ) (4) e_y = l_d sin\theta =l_d sin(\beta - \varphi)\tag{4} ey=ldsinθ=ldsin(βφ)(4)

其中 φ \varphi φ为车辆的航向角yaw,其实现方法详见bicycle_model.py

3. 算法和仿真实现

bicycle_model.py

#!/usr/bin/python
# -*- coding: UTF-8 -*-

import math
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Camera


class Vehicle:
    def __init__(self,
                 x=0.0,
                 y=0.0,
                 yaw=0.0,
                 v=0.0,
                 dt=0.1,
                 l=3.0):
        self.steer = 0
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.dt = dt
        self.L = l  # 轴距
        self.x_front = x + l * math.cos(yaw)
        self.y_front = y + l * math.sin(yaw)

    def update(self, a, delta, max_steer=np.pi):
        delta = np.clip(delta, -max_steer, max_steer)
        self.steer = delta

        self.x = self.x + self.v * math.cos(self.yaw) * self.dt
        self.y = self.y + self.v * math.sin(self.yaw) * self.dt
        self.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dt

        self.v = self.v + a * self.dt
        self.x_front = self.x + self.L * math.cos(self.yaw)
        self.y_front = self.y + self.L * math.sin(self.yaw)


class VehicleInfo:
    # Vehicle parameter
    L = 3.0  #轴距
    W = 2.0  #宽度
    LF = 3.8  #后轴中心到车头距离
    LB = 0.8  #后轴中心到车尾距离
    MAX_STEER = 0.6  # 最大前轮转角
    TR = 0.5  # 轮子半径
    TW = 0.5  # 轮子宽度
    WD = W  #轮距
    LENGTH = LB + LF  # 车辆长度

def draw_trailer(x, y, yaw, steer, ax, vehicle_info=VehicleInfo, color='black'):
    vehicle_outline = np.array(
        [[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],
         [vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])

    wheel = np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],
                      [vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2, vehicle_info.TW / 2]])

    rr_wheel = wheel.copy() #右后轮
    rl_wheel = wheel.copy() #左后轮
    fr_wheel = wheel.copy() #右前轮
    fl_wheel = wheel.copy() #左前轮
    rr_wheel[1,:] += vehicle_info.WD/2
    rl_wheel[1,:] -= vehicle_info.WD/2

    #方向盘旋转
    rot1 = np.array([[np.cos(steer), -np.sin(steer)],
                     [np.sin(steer), np.cos(steer)]])
    #yaw旋转矩阵
    rot2 = np.array([[np.cos(yaw), -np.sin(yaw)],
                     [np.sin(yaw), np.cos(yaw)]])
    fr_wheel = np.dot(rot1, fr_wheel)
    fl_wheel = np.dot(rot1, fl_wheel)
    fr_wheel += np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])
    fl_wheel += np.array([[vehicle_info.L], [vehicle_info.WD / 2]])

    fr_wheel = np.dot(rot2, fr_wheel)
    fr_wheel[0, :] += x
    fr_wheel[1, :] += y
    fl_wheel = np.dot(rot2, fl_wheel)
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rr_wheel = np.dot(rot2, rr_wheel)
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    rl_wheel = np.dot(rot2, rl_wheel)
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y
    vehicle_outline = np.dot(rot2, vehicle_outline)
    vehicle_outline[0, :] += x
    vehicle_outline[1, :] += y

    ax.plot(fr_wheel[0, :], fr_wheel[1, :], color)
    ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)
    ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)
    ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)

    ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)
    # ax.axis('equal')



if __name__ == "__main__":

    vehicle = Vehicle(x=0.0,
                      y=0.0,
                      yaw=0,
                      v=0.0,
                      dt=0.1,
                      l=VehicleInfo.L)
    vehicle.v = 1
    trajectory_x = []
    trajectory_y = []
    fig = plt.figure()
    # 保存动图用
    # camera = Camera(fig)
    for i in range(600):
        plt.cla()
        plt.gca().set_aspect('equal', adjustable='box')
        vehicle.update(0, np.pi / 10)
        draw_trailer(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)
        trajectory_x.append(vehicle.x)
        trajectory_y.append(vehicle.y)
        plt.plot(trajectory_x, trajectory_y, 'blue')
        plt.xlim(-12, 12)
        plt.ylim(-2.5, 21)
        plt.pause(0.001)
    #     camera.snap()
    # animation = camera.animate(interval=5)
    # animation.save('trajectory.gif')

main.py

from scipy.spatial import KDTree
from bicycle_model import Vehicle, VehicleInfo, draw_trailer
from PID import PIDController
import numpy as np
import matplotlib.pyplot as plt
import math
import imageio

MAX_SIMULATION_TIME = 200.0  # 程序最大运行时间200*dt
PID = PIDController(2, 0.001, 3)


def NormalizeAngle(angle):
    a = math.fmod(angle + np.pi, 2 * np.pi)
    if a < 0.0:
        a += (2.0 * np.pi)
    return a - np.pi


def main():
    # 设置跟踪轨迹
    ref_path = np.zeros((1000, 2))
    ref_path[:, 0] = np.linspace(0, 50, 1000)  # x坐标
    ref_path[:, 1] = 10 * np.sin(ref_path[:, 0] / 20.0)  # y坐标
    ref_tree = KDTree(ref_path)
    # 假设车辆初始位置为(0,0),航向角yaw=0.0,速度为2m/s,时间周期dt为0.1秒
    vehicle = Vehicle(x=0.0,
                      y=0.0,
                      yaw=np.pi/2,
                      v=2.0,
                      dt=0.1,
                      l=VehicleInfo.L)

    time = 0.0  # 初始时间

    # 记录车辆轨迹
    trajectory_x = []
    trajectory_y = []
    lat_err = []  # 记录横向误差

    i = 0
    image_list = []  # 存储图片
    plt.figure(1)

    last_idx = ref_path.shape[0] - 1  # 跟踪轨迹的最后一个点的索引
    old_idx = 0  # 记录上一次的索引点
    target_ind = 0  # 第一个目标点的索引
    while MAX_SIMULATION_TIME >= time and last_idx > target_ind:
        time += vehicle.dt  # 累加一次时间周期
        vehicle_positon = np.zeros(2)
        vehicle_positon[0] = vehicle.x
        vehicle_positon[1] = vehicle.y
        distance, target_ind = ref_tree.query(vehicle_positon)  # 在跟踪轨迹上查找离车辆最近的点
        if old_idx > target_ind:
            print("ERROR: Find the point behind the vehicle.")
            target_ind = old_idx + 1  # 查找到车辆后面的点,将目标点索引置为上一次的索引点idx+1
        old_idx = target_ind  # 记录本次索引点idx
        alpha = math.atan2(
            ref_path[target_ind, 1] - vehicle_positon[1], ref_path[target_ind, 0] - vehicle_positon[0])
        l_d = np.linalg.norm(ref_path[target_ind] - vehicle_positon)  # 目标点与车定位点距离l_d

        theta_e = NormalizeAngle(alpha - vehicle.yaw)
        e_y = l_d * math.sin(theta_e)  # 计算实际误差,0为目标距离
        lat_err.append(e_y)
        delta_f = PID.cal_output(e_y, vehicle.dt)

        vehicle.update(0.0, delta_f, np.pi / 10)  # 由于假设纵向匀速运动,所以加速度a=0.0
        trajectory_x.append(vehicle.x)
        trajectory_y.append(vehicle.y)

        # 显示动图
        plt.cla()
        plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)
        draw_trailer(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)

        plt.plot(trajectory_x, trajectory_y, "-r", label="trajectory")
        plt.plot(ref_path[target_ind, 0], ref_path[target_ind, 1], "go", label="target")
        plt.axis("equal")
        plt.grid(True)
        plt.pause(0.001)
        plt.savefig("temp.png")
        i += 1
        if (i % 50) > 0:
            image_list.append(imageio.imread("temp.png"))

    imageio.mimsave("display.gif", image_list, duration=0.1)

    plt.figure(2)
    plt.subplot(2, 1, 1)
    plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)
    plt.plot(trajectory_x, trajectory_y, 'r')
    plt.title("actual tracking effect")

    plt.subplot(2, 1, 2)
    plt.plot(lat_err)
    plt.title("lateral error")
    plt.show()


if __name__ == '__main__':
    main()

运行效果如下

PID横向控制和仿真实现,自动驾驶-控制,pid,自动驾驶-控制,控制,汽车

跟踪效果和控制误差

PID横向控制和仿真实现,自动驾驶-控制,pid,自动驾驶-控制,控制,汽车

文章首发公众号:iDoitnow如果喜欢话,可以关注一下文章来源地址https://www.toymoban.com/news/detail-789774.html

到了这里,关于PID横向控制和仿真实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • Matlab实现PID控制仿真(附上30个完整仿真源码+数据)

    本文介绍了如何使用Matlab实现PID控制器的仿真。首先,我们将简要介绍PID控制器的原理和控制算法。然后,我们将使用Matlab编写一个简单的PID控制器,并使用仿真环境来验证其性能。最后,我们将通过调整PID控制器的参数来优化控制系统的响应。 PID控制器是一种经典的控制算

    2024年02月07日
    浏览(55)
  • MPC自动驾驶横向控制算法实现 c++

    参考博客: (1)无人车系统(十一):轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】 (2)【自动驾驶】模型预测控制(MPC)实现轨迹跟踪 (3)自动驾驶——模型预测控制(MPC)理解与实践 (4)MPC算法学习(1) 0 前言 前面介绍的PID、Pure pursuit、Stanley都只是利用当前的

    2024年02月22日
    浏览(44)
  • 【PID控制与模糊PID控制的比较】(带仿真和代码链接)

    目  录 一、序言 二、PID控制器的设计         1.PID控制原理图         2.PID控制器传递函数的一般表达式 三、模糊控制器的设计         1.模糊控制原理图         2.模糊控制器传递函数一般表达形式 四、系统仿真 五、总结 ——————————————

    2024年02月03日
    浏览(41)
  • 【Simulink】仿真_PID控制器调谐/调参/整定

    如何使用PID调谐器自动调优PID控制器块? 模型下载: 转速闭环 PID调谐器提供了一种快速和广泛适用的 single-loop PID通过Simulink控制块的整定方法。通过这种方法,可以调优PID控制器参数,以实现具有所需响应时间(response time)的鲁棒(robust)设计。 PID调谐器的典型设计工作流程包括

    2024年02月05日
    浏览(50)
  • 毕业设计-基于 PID 控制算法仿真算法研究- Matlab

    目录 前言 课题背景和意义 实现技术思路 一、 基本原理  二、无超调 PID 控制器的设计 三、无超调 PID 设计的验证 代码 实现效果图样例 最后     📅大四是整个大学期间最忙碌的时光,一边要忙着备考或实习为毕业后面临的就业升学做准备,一边要为毕业设计耗费大量精力。

    2024年02月06日
    浏览(51)
  • 1-径向基(RBF)神经网络PID控制器仿真

    1、内容简介 略 1-可以交流、咨询、答疑 2、内容说明 略 3、仿真分析 4、参考论文 略 5、下载链接 链接:https://pan.baidu.com/s/1mNySkJC4voazGMLEEfIjDw  提取码:2exo

    2024年02月08日
    浏览(40)
  • 浅谈BP神经网络PID控制算法及matlab仿真

    本文是对BP神经网络PID控制算法的数学描述及仿真实验,若有错误之处,欢迎指正! 老规矩不废话,直接上链接 BP神经网络维基百科 BP神经网络是人工神经网络中的一种常用结构,其由输入层(input)-隐含层(hidding)-输出层三层构成(output)。 上图中, B 1 B1 B 1 是输入层, B 2 B2 B

    2024年02月05日
    浏览(38)
  • m基于PID控制算法的四旋翼无人机飞行控制simulink仿真

    目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB   无人机采用常见的四旋翼无人飞行器,如图1所示。       PID 控制器,即控制器的控制方式为 P 比例调整, I 积分调整以及 D 微分调整三个部分构成, PID 控制器是目前为止应用最为广泛的控制方式。 PID 控制器具

    2023年04月22日
    浏览(75)
  • 反激式开关电源输出电压的PID控制之MATLAB仿真

    反激 是开关电源中最常见的电路之一。像手机充电器、笔记本电脑的电源适配器,电动车充电器大都采用该电路。基本作用就是将高压直流将至低压直流。 反激电路的特点表现在结构简单,体积小。缺点是输出功率很难做到很大,一般只适用于150W以下的情况。 其基本电路如

    2024年01月17日
    浏览(41)
  • 双容水箱液位模糊PID控制系统设计与仿真(Matlab/Simulink)

    前些天发现了十分不错的人工智能学习网站,通俗易懂,风趣幽默,没有广告,分享给大家,大家可以自行看看。(点击跳转人工智能学习资料) 微信公众号:创享日记 发送:双容模糊 获取完整无水印报告+仿真源文件+相关文献 双容水箱液位控制系统的设计与仿真 1、基于

    2024年02月07日
    浏览(49)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包