运动学模型推导及其线性化和离散化

文章目录

• • 从阿克曼转向几何模型到自行车模型
  • 无人车运动学模型
  • 无人车运动学模型线性化
  • 线性模型离散化
  • 代码实现

1.从阿克曼转向几何模型到自行车模型

        相对无人机与机械臂来说，无人车系统的运动学模型非常简洁。尽管简洁，无人车的运动学模型也是非线性的。应用于具体控制算法时，有必要对原始运动学模型进行变形或线性化。本篇主要介绍无人车的运动学模型，并对原始非线性模型进行线性化。

汽车采用阿克曼转向轮，因此模型为如下图所示的阿克曼转向几何模型。

由以上阿克曼转向模型可得进一步简化为车辆单轨模型——自行车模型。

注意：以上公式可以准确估计低速场景下的车辆运动。 但是，对于高速情况，该公式估计的运动与实际无人车运行的情况有一定的差异，这与路面、轮胎材料都有关系。

---

2. 无人车运动学模型

以后轴中心为车辆中心的单车运动学模型

 参考：汽车运动学模型的线性化推导过程_车辆运动学 线性化-CSDN博客

 补充：推导方式二

参考：无人车系统（一）：运动学模型及其线性化_无人集卡阿克曼转角模型-CSDN博客

---

3. 运动学模型线性化

为什么要线性化？

        车辆动力学模型线性化的主要原因是为了设计控制器。非线性系统往往难以控制，因此需要将其转化为线性系统，以便于设计控制器。然而，需要注意的是，并非所有的车辆动力学模型都需要进行线性化。例如，车辆的运动学模型通常是非线性的，因此在设计控制器之前需要进行模型线性化。相反，车辆的动力学模型本身就是线性化模型，所以不需要进行线性化。

        由于车辆的运动学模型是非线性的，所以在设计控制器之前需要进行模型线性化，而车辆的动力学模型本身就是线性化模型，所以不需要进行线性化。（所谓线性化，就是把不能写成X_dot=AX+Bu形式的非线性方程组，写成这种形式）

参照汽车运动学模型的线性化推导过程，可得到线性化后的无人车运动学模型。

1 线形化推导理论过程

2 运动学模型线形化

参考：汽车运动学模型的线性化推导过程_车辆运动学 线性化-CSDN博客

补充：推导方式二

参考：无人车系统（一）：运动学模型及其线性化_无人集卡阿克曼转角模型-CSDN博客

说明

1.只有当在静态点附近非常小的变化时，线性化才有意义，否则线性化误差会非常大，即线性化的小偏差思想；
2.当工作点不同时，线性化的结果也不同；
3.线性化后的方程一般为增量形式。

---

4. 线性模型离散化

为什么要离散化？

        由于计算机能够处理的数据都是离散化的，所以紧接着还要进行模型的离散化，主要使用积分中值定理和欧拉法可以进行推导。

离散化方法？

参考：【自动驾驶】运动学模型的线性离散化（含python实现）_运动学模型离散化-CSDN博客

 补充：推导方式二

参考：无人车系统（一）：运动学模型及其线性化_无人集卡阿克曼转角模型-CSDN博客

代码实现

1 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.L, self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
                  ])
    return A,B

2 C++版本

KinematicModel.h



#ifndef CHHROBOTICS_CPP_KINEMATICMODEL_H
#define CHHROBOTICS_CPP_KINEMATICMODEL_H
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <vector>

using namespace std;
using namespace Eigen;

class KinematicModel {
public:
  double x, y, psi, v, L, dt;

public:
  KinematicModel();

  KinematicModel(double x, double y, double psi, double v, double l, double dt);

  vector<double> getState();

  void updateState(double accel, double delta_f);

  vector<MatrixXd> stateSpace(double ref_delta, double ref_yaw);
};

#endif // CHHROBOTICS_CPP_KINEMATICMODEL_H

KinematicModel.cpp


#include "stanley/KinematicModel.h"
/**
 * 机器人运动学模型构造
 * @param x 位置x
 * @param y 位置y
 * @param psi 偏航角
 * @param v 速度
 * @param l 轴距
 * @param dt 采样时间
 */
KinematicModel::KinematicModel(double x, double y, double psi, double v, double l, double dt) : x(x), y(y), psi(psi),
                                                                                                v(v), L(l), dt(dt) {}
/**
 * 控制量为转向角delta_f和加速度a
 * @param accel 加速度
 * @param delta_f 转向角控制量
 */
void KinematicModel::updateState(double accel, double delta_f) {
    x = x + v* cos(psi)*dt;
    y = y + v*sin(psi)*dt;
    psi = psi + v / L * tan(delta_f)*dt;
    v = v + accel*dt;
}

/**
 * 状态获取
 * @return
 */
vector<double> KinematicModel::getState() {
    return {x,y,psi,v};
}

/**
 * 将模型离散化后的状态空间表达
 * @param ref_delta 名义控制输入
 * @param ref_yaw 名义偏航角
 * @return
 */
vector<MatrixXd> KinematicModel::stateSpace(double ref_delta, double ref_yaw) {
    MatrixXd A(3,3),B(3,2);
    A<<1.0,0.0,-v*dt*sin(ref_yaw),
        0.0,1.0,v*dt*cos(ref_yaw),
        0.0,0.0,1.0;
    B<<dt*cos(ref_yaw),0,
        dt*sin(ref_yaw),0,
        dt*tan(ref_delta)/L,v*dt/(L*cos(ref_delta)*cos(ref_delta));
    return {A,B};
}

main.cpp

double x0 = 0.0, y0=-1.0,psi = 0.5,v=2,L=2,dt=0.1; //初始参数

KinematicModel ugv(x0,y0,psi,v,L,dt);//初始化参数

//机器人状态
vector<double>robot_state(4);

robot_state = ugv.getState();  //获取车辆状态

ugv.updateState(0,delta_index[0]); //更新当前状态   //加速度设为0，恒速  

x_.push_back(ugv.x);
y_.push_back(ugv.y);

注意：运动学模型必须线性化之后再离散化才可供控制算法使用。

本文仅对运动学模型的推导和线性化离散化参考上述博主文献进行了整合，仅作为学习总结记录，如需详细理解请参考对应博主的讲解！