【自动驾驶】运动学模型的线性离散化

1. 运动学模型线性化

之前讲解了车辆的运动学模型,这边以以后轴中心为车辆中心的单车运动学模型为例进行线性化。

以后轴中心为车辆中心的单车运动学模型如下:
【自动驾驶】运动学模型的线性离散化
【自动驾驶】运动学模型的线性离散化

选取状态量为 【自动驾驶】运动学模型的线性离散化 ,控制量为 【自动驾驶】运动学模型的线性离散化 ,则对于参考轨迹的任意一个参考点,用 【自动驾驶】运动学模型的线性离散化 表示,上式可以改写为:
【自动驾驶】运动学模型的线性离散化
其中 【自动驾驶】运动学模型的线性离散化 。对上式在参考点采用泰勒级数展开,并忽略高阶项:
【自动驾驶】运动学模型的线性离散化

【自动驾驶】运动学模型的线性离散化【自动驾驶】运动学模型的线性离散化 求雅克比矩阵,有:
【自动驾驶】运动学模型的线性离散化
【自动驾驶】运动学模型的线性离散化

则状态量误差的变化量 【自动驾驶】运动学模型的线性离散化 表示为 :
【自动驾驶】运动学模型的线性离散化

上式表明状态误差量可以构成线性状态空间。

2. 线性模型离散化

对等式(6)进行前向欧拉离散化,得到
【自动驾驶】运动学模型的线性离散化
整理后,得到:
【自动驾驶】运动学模型的线性离散化

式中,【自动驾驶】运动学模型的线性离散化为采样步长,【自动驾驶】运动学模型的线性离散化为单位矩阵,维度与矩阵【自动驾驶】运动学模型的线性离散化一致。

3. python实现

import math
import numpy as np

class KinematicModel_3:
  """假设控制量为转向角delta_f和加速度a
  """

  def __init__(self, x, y, psi, v, L, dt):
    self.x = x
    self.y = y
    self.psi = psi
    self.v = v
    self.L = L
    # 实现是离散的模型
    self.dt = dt

  def update_state(self, a, delta_f):
    self.x = self.x+self.v*math.cos(self.psi)*self.dt
    self.y = self.y+self.v*math.sin(self.psi)*self.dt
    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt
    self.v = self.v+a*self.dt

  def get_state(self):
    return self.x, self.y, self.psi, self.v

  def state_space(self, ref_delta, ref_yaw):
    """将模型离散化后的状态空间表达

    Args:
        delta (_type_): 参考输入

    Returns:
        _type_: _description_
    """
    A = np.matrix([
      [1.0,0.0,-self.v*self.dt*math.sin(ref_yaw)],
      [0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],
      [0.0,0.0,1.0]])
    
    B = np.matrix([
        [self.dt*math.cos(ref_yaw), 0],
        [self.dt*math.sin(ref_yaw), 0],
      [self.dt*math.tan(ref_delta), self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
                  ])
    return A,B


文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
上一篇 2022年5月30日 上午10:56
下一篇 2022年5月30日 上午10:58

相关推荐

本站注重文章个人版权,不会主动收集付费或者带有商业版权的文章,如果出现侵权情况只可能是作者后期更改了版权声明,如果出现这种情况请主动联系我们,我们看到会在第一时间删除!本站专注于人工智能高质量优质文章收集,方便各位学者快速找到学习资源,本站收集的文章都会附上文章出处,如果不愿意分享到本平台,我们会第一时间删除!