【【自动驾驶】车辆运动学模型】

【自动驾驶】车辆运动学模型

• • [1. 引言](#1. 引言)
  • [2. 以车辆重心为中心的单车模型](#2. 以车辆重心为中心的单车模型)
  • • [2.1 模型介绍](#2.1 模型介绍)
    • [2.2 滑移角 β \beta β 的推导](#2.2 滑移角 β \beta β 的推导)
    • [2.2 航向角 ψ \psi ψ推导过程：](#2.2 航向角 ψ \psi ψ推导过程：)
    • [2.3 滑移角 β \beta β](#2.3 滑移角 β \beta β)
    • [2.3 Python代码实现](#2.3 Python代码实现)
    • [2.4 C++代码实现](#2.4 C++代码实现)
  • [3. 前轮驱动的单车模型](#3. 前轮驱动的单车模型)
  • • [3.1 模型介绍](#3.1 模型介绍)
    • [3.3 Python代码实现](#3.3 Python代码实现)
    • [3.4 C++代码实现](#3.4 C++代码实现)
  • [4. 以后轴中心为车辆中心的单车模型](#4. 以后轴中心为车辆中心的单车模型)
  • • [4.1 模型介绍](#4.1 模型介绍)
    • [4.3 Python代码实现](#4.3 Python代码实现)
    • [4.4 C++代码实现](#4.4 C++代码实现)
  • [5. 阿克曼转向几何](#5. 阿克曼转向几何)
  • • [5.1 模型介绍](#5.1 模型介绍)
    • [5.2 滑移角 β \beta β 的推导](#5.2 滑移角 β \beta β 的推导)
    • [5.3 Python代码实现](#5.3 Python代码实现)
    • [5.4 C++代码实现](#5.4 C++代码实现)
  • [6. 总结](#6. 总结)
  • 参考资料

1. 引言

在自动驾驶技术的发展过程中，对车辆运动的理解和建模至关重要。车辆运动学模型用于描述车辆的运动特性，尤其是在低速条件下，通过控制输入来预测车辆的未来状态。本文将详细介绍几种常见的车辆运动学模型，并提供Python和C++的代码实现。

好的，我们将详细推导并更新其他模型中的侧滑角 β \beta β。以下是每个模型的详细推导和代码实现。

2. 以车辆重心为中心的单车模型

2.1 模型介绍

在自动驾驶领域，通常采用单车模型来简化实际车辆的复杂性。此模型将车辆视为具有两个转动轴的刚体，即前轮和后轮，每个轴上的左右轮合并为一个轮子来考虑。模型的关键参数包括：

• 车辆重心：点C，代表车辆的质量中心。
• 前后轮距离 ： l f l_f lf 和 l r l_r lr 分别从前轮中心到重心的距离和从后轮中心到重心的距离。
• 轴距 ： L = l f + l r L = l_f + l_r L=lf+lr。
• 速度 ： V V V，表示车辆重心的速度。
• 滑移角 ： β \beta β，车辆速度矢量与车辆纵向轴之间的夹角。
• 航向角 ： ψ \psi ψ，车身与X轴的夹角。
• 转向角 ： δ f \delta_f δf 和 δ r \delta_r δr，分别表示前轮和后轮的转向角。对于大多数车辆， δ r \delta_r δr 可以设置为0。

2.2 滑移角 β \beta β 的推导

1. 几何关系 ：
   • 假设车辆的重心位于点C，前轮中心位于点F，后轮中心位于点R。
   • 前轮的转向角 δ f \delta_f δf 导致前轮的行驶方向与车辆纵向轴之间有一个夹角。
   • 后轮的转向角 δ r \delta_r δr 导致后轮的行驶方向与车辆纵向轴之间有一个夹角。

2.2 航向角 ψ \psi ψ推导过程：

• 如上图所示，在三角形 O C A O C A OCA中，根据正弦定理，有：
  s i n ( δ f − β ) l f = s i n ( π 2 − δ f ) R (1) \frac{sin(\delta_f - \beta)}{l_f} = \frac{sin(\frac{\pi}{2} - \delta_f)}{R} \tag{1} lfsin(δf−β)=Rsin(2π−δf)(1)
• 在三角形 O B C OBC OBC中，根据正弦定理，有：
  s i n ( β − δ r ) l r = s i n ( π 2 + δ r ) R (2) \frac{sin(\beta - \delta_r)}{l_r} = \frac{sin(\frac{\pi}{2}+ \delta_r)}{R} \tag{2} lrsin(β−δr)=Rsin(2π+δr)(2)
• 化简：
  s i n ( δ f ) c o s ( β ) − s i n ( β ) c o s ( δ f ) l f = c o s ( δ f ) R (3) \frac{sin(\delta_f)cos(\beta) - sin(\beta)cos(\delta_f)}{l_f} = \frac{cos(\delta_f)}{R} \tag{3} lfsin(δf)cos(β)−sin(β)cos(δf)=Rcos(δf)(3)
  s i n ( β ) c o s ( δ r ) − s i n ( δ r ) c o s ( β ) l r = c o s ( δ r ) R (4) \frac{sin(\beta)cos(\delta_r) - sin(\delta_r)cos(\beta)}{l_r} = \frac{cos(\delta_r)}{R} \tag{4} lrsin(β)cos(δr)−sin(δr)cos(β)=Rcos(δr)(4)
  等式（3）两边同乘以 l f c o s ( δ f ) \frac{l_f}{cos(\delta_f)} cos(δf)lf:
  s i n ( δ f ) c o s ( β ) − s i n ( β ) c o s ( δ f ) = l f c o s ( δ f ) R (5) sin(\delta_f)cos(\beta) - sin(\beta)cos(\delta_f) = \frac{l_fcos(\delta_f)}{R} \tag{5} sin(δf)cos(β)−sin(β)cos(δf)=Rlfcos(δf)(5)
  同理等式（4）两边同乘以 l r l_r lr:
  s i n ( β ) c o s ( δ r ) − s i n ( δ r ) c o s ( β ) = l r c o s ( δ r ) R (6) sin(\beta)cos(\delta_r) - sin(\delta_r)cos(\beta) = \frac{l_rcos(\delta_r)}{R} \tag{6} sin(β)cos(δr)−sin(δr)cos(β)=Rlrcos(δr)(6)
  （5）（6）两式相加：
  c o s ( β ) ( s i n ( δ f ) − s i n ( δ r ) ) = l r + l f R (7) cos(\beta)(sin(\delta_f)-sin(\delta_r)) = \frac{l_r +l_f}{R} \tag{7} cos(β)(sin(δf)−sin(δr))=Rlr+lf(7)
  根据条件假设，低速环境下，车辆行驶路径的转弯半径变化缓慢，此时我们可以假设车辆偏航角的变化率 ψ ˙ \dot{\psi} ψ˙
  可近似等于车辆的角速度 ω \omega ω。根据车辆角速度 ω = V R \omega = \frac{V}{R} ω=RV得：
  ψ ˙ = ω = V R (8) \dot{\psi} = \omega = \frac{V}{R} \tag{8} ψ˙=ω=RV(8)
  将（8）带入（7）中，消除 R R R得：
  ψ ˙ = V c o s ( β ) l r + l f ( t a n ( δ f ) − t a n ( δ r ) ) (9) \dot{\psi}= \frac{Vcos(\beta)}{l_r +l_f}(tan(\delta_f)-tan(\delta_r)) \tag{9} ψ˙=lr+lfVcos(β)(tan(δf)−tan(δr))(9)

2.3 滑移角 β \beta β

• 滑移角 β \beta β 定义为车辆速度矢量与车辆纵向轴之间的夹角。
• 当车辆转弯时，前轮和后轮的速度矢量会有所不同，导致车辆重心的速度矢量与车辆纵向轴之间有一个夹角。
  等式（5）两边同乘以 l r l_r lr:
  l r ( t a n ( δ f ) c o s ( β ) − s i n ( β ) − l f l r R (10) l_r(tan(\delta_f)cos(\beta)-sin(\beta) - \frac{l_f l_r}{R} \tag{10} lr(tan(δf)cos(β)−sin(β)−Rlflr(10)
  同理等式（6）两边同乘以 l f l_f lf:
  l f ( s i n ( β ) − t a n ( δ r ) c o s ( β ) ) = l f l r R (11) l_f(sin(\beta) - tan(\delta_r)cos(\beta)) = \frac{l_f l_r}{R} \tag{11} lf(sin(β)−tan(δr)cos(β))=Rlflr(11)
  （10）（11）两式相等：
  c o s ( β ) ( l r t a n ( δ f ) + l f t a n ( δ r ) ) = s i n ( β ) ( l r + l f ) (12) cos(\beta)(l_r tan(\delta_f) + l_f tan(\delta_r)) =sin(\beta)(l_r +l_f) \tag{12} cos(β)(lrtan(δf)+lftan(δr))=sin(β)(lr+lf)(12)
  合并 t a n ( β ) tan(\beta) tan(β):
  t a n ( β ) = l r t a n ( δ f ) + l f t a n ( δ r ) l r + l f (13) tan(\beta) = \frac{l_r tan(\delta_f) + l_f tan(\delta_r)}{l_r +l_f} \tag{13} tan(β)=lr+lflrtan(δf)+lftan(δr)(13)

车辆运动学模型
{ x ˙ = V cos ⁡ ( ψ + β ) y ˙ = V sin ⁡ ( ψ + β ) ψ ˙ = V c o s ( β ) l r + l f ( t a n ( δ f ) − t a n ( δ r ) ) (14) \left\{ \begin{array}{l} \dot{x} = V \cos (\psi + \beta) \\ \dot{y} = V \sin (\psi + \beta) \\ \dot{\psi} = \frac{Vcos(\beta)}{l_r +l_f}(tan(\delta_f)-tan(\delta_r)) \end{array} \right. \tag{14} ⎩ ⎨ ⎧x˙=Vcos(ψ+β)y˙=Vsin(ψ+β)ψ˙=lr+lfVcos(β)(tan(δf)−tan(δr))(14)

其中
β = a r c t a n ( l r t a n ( δ f ) + l f t a n ( δ r ) l r + l f ) (15) \beta = arctan(\frac{l_r tan(\delta_f) + l_f tan(\delta_r)}{l_r +l_f} \tag{15}) β=arctan(lr+lflrtan(δf)+lftan(δr))(15)

2.3 Python代码实现

import math

class KinematicModel_1:
    def __init__(self, x, y, psi, v, l_r, l_f, dt):
        self.x = x
        self.y = y
        self.psi = psi
        self.v = v
        self.l_f = l_f
        self.l_r = l_r
        self.dt = dt
    
    def update_state(self, a, delta_f, delta_r=0):
        # 计算滑移角 β
        beta = math.atan2(self.l_r * math.sin(delta_f) + self.l_f * math.sin(delta_r),
                          self.l_r * math.cos(delta_f) + self.l_f * math.cos(delta_r))
        
        # 更新状态
        self.x += self.v * math.cos(self.psi + beta) * self.dt
        self.y += self.v * math.sin(self.psi + beta) * self.dt
        self.psi += (self.v / (self.l_f + self.l_r)) * (math.tan(delta_f) - math.tan(delta_r)) * math.cos(beta) * self.dt
        self.v += a * self.dt
    
    def get_state(self):
        return self.x, self.y, self.psi, self.v

2.4 C++代码实现

#include <cmath>
#include <iostream>

class KinematicModel_1 {
public:
    KinematicModel_1(double x, double y, double psi, double v, double l_r, double l_f, double dt) :
        x(x), y(y), psi(psi), v(v), l_f(l_f), l_r(l_r), dt(dt) {}

    void update_state(double a, double delta_f, double delta_r = 0) {
        // 计算滑移角 β
        double beta = atan2(l_r * sin(delta_f) + l_f * sin(delta_r),
                            l_r  + l_f );
        
        // 更新状态
        x += v * cos(psi + beta) * dt;
        y += v * sin(psi + beta) * dt;
        psi += (v / (l_f + l_r)) * (tan(delta_f) - tan(delta_r)) * cos(beta) * dt;
        v += a * dt;
    }

    void get_state() const {
        std::cout << "x: " << x << ", y: " << y << ", psi: " << psi << ", v: " << v << std::endl;
    }

private:
    double x, y, psi, v, l_f, l_r, dt;
};

3. 前轮驱动的单车模型

3.1 模型介绍

当仅考虑前轮转向时，模型可以简化为：

车辆运动学模型
{ x ˙ = V cos ⁡ ( ψ + β ) y ˙ = V sin ⁡ ( ψ + β ) ψ ˙ = V sin ⁡ β l r (16) \left\{ \begin{array}{l} \dot{x} = V \cos (\psi + \beta) \\ \dot{y} = V \sin (\psi + \beta) \\ \dot{\psi} = \frac{V \sin \beta}{l_r} \end{array} \right. \tag{16} ⎩ ⎨ ⎧x˙=Vcos(ψ+β)y˙=Vsin(ψ+β)ψ˙=lrVsinβ(16)

其中 δ r = 0 \delta_r = 0 δr=0，滑移角 β \beta β 可以表示为：
β = arctan ⁡ ( l r l f + l r tan ⁡ ( δ f ) ) (17) \beta = \arctan \left( \frac{l_r}{l_f + l_r} \tan(\delta_f) \right) \tag{17} β=arctan(lf+lrlrtan(δf))(17)

3.3 Python代码实现

class KinematicModel_2:
    def __init__(self, x, y, psi, v, l_r, l_f, dt):
        self.x = x
        self.y = y
        self.psi = psi
        self.v = v
        self.l_f = l_f
        self.l_r = l_r
        self.dt = dt
    
    def update_state(self, a, delta_f):
        # 计算滑移角 β
        beta = math.atan((self.l_r / (self.l_f + self.l_r)) * math.tan(delta_f))
        
        # 更新状态
        self.x += self.v * math.cos(self.psi + beta) * self.dt
        self.y += self.v * math.sin(self.psi + beta) * self.dt
        self.psi += (self.v * math.sin(beta) / self.l_r) * self.dt
        self.v += a * self.dt
    
    def get_state(self):
        return self.x, self.y, self.psi, self.v

3.4 C++代码实现

class KinematicModel_2 {
public:
    KinematicModel_2(double x, double y, double psi, double v, double l_r, double l_f, double dt) :
        x(x), y(y), psi(psi), v(v), l_f(l_f), l_r(l_r), dt(dt) {}

    void update_state(double a, double delta_f) {
        // 计算滑移角 β
        double beta = atan((l_r / (l_f + l_r)) * tan(delta_f));
        
        // 更新状态
        x += v * cos(psi + beta) * dt;
        y += v * sin(psi + beta) * dt;
        psi += (v * sin(beta) / l_r) * dt;
        v += a * dt;
    }

    void get_state() const {
        std::cout << "x: " << x << ", y: " << y << ", psi: " << psi << ", v: " << v << std::endl;
    }

private:
    double x, y, psi, v, l_f, l_r, dt;
};

4. 以后轴中心为车辆中心的单车模型

4.1 模型介绍

当以车辆后轴中心为参考点时，模型可以进一步简化。假设后轮没有转向角（ δ r = 0 \delta_r = 0 δr=0），模型可以表示为：

{ x ˙ = V cos ⁡ ( ψ ) y ˙ = V sin ⁡ ( ψ ) ψ ˙ = V L tan ⁡ δ f (18) \left\{ \begin{array}{l} \dot{x} = V \cos (\psi) \\ \dot{y} = V \sin (\psi) \\ \dot{\psi} = \frac{V}{L} \tan \delta_f \end{array} \right. \tag{18} ⎩ ⎨ ⎧x˙=Vcos(ψ)y˙=Vsin(ψ)ψ˙=LVtanδf(18)

其中， L L L 是轴距， δ f \delta_f δf 是前轮转向角。

• 在这种模型中，滑移角 β \beta β 可以近似为0，因为重心位于后轴中心，且后轮没有转向角。
• 因此，滑移角 β \beta β 可以表示为：
  β ≈ 0 \beta \approx 0 β≈0

4.3 Python代码实现

class KinematicModel_3:
    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.v * math.cos(self.psi) * self.dt
        self.y += self.v * math.sin(self.psi) * self.dt
        self.psi += (self.v / self.L) * math.tan(delta_f) * self.dt
        self.v += a * self.dt
    
    def get_state(self):
        return self.x, self.y, self.psi, self.v

4.4 C++代码实现

class KinematicModel_3 {
public:
    KinematicModel_3(double x, double y, double psi, double v, double L, double dt) :
        x(x), y(y), psi(psi), v(v), L(L), dt(dt) {}

    void update_state(double a, double delta_f) {
        // 更新状态
        x += v * cos(psi) * dt;
        y += v * sin(psi) * dt;
        psi += (v / L) * tan(delta_f) * dt;
        v += a * dt;
    }

    void get_state() const {
        std::cout << "x: " << x << ", y: " << y << ", psi: " << psi << ", v: " << v << std::endl;
    }

private:
    double x, y, psi, v, L, dt;
};

5. 阿克曼转向几何

5.1 模型介绍

阿克曼转向几何（Ackerman Turning Geometry）是一种为了解决交通工具转弯时，内外转向轮路径指向的圆心不同的几何学。在单车模型中，将转向时左、右前轮偏角假设为同一角度 δ f \delta_f δf，虽然通常两个角度大致相等，但实际并不是，通常情况下，内侧轮胎转角更大。

• 外轮转角 ： δ o = L R + l w 2 \delta_o = \frac{L}{R + \frac{l_w}{2}} δo=R+2lwL
• 内轮转角 ： δ i = L R − l w 2 \delta_i = \frac{L}{R - \frac{l_w}{2}} δi=R−2lwL

其中， l w l_w lw 为轮距， R R R 为轨迹半径。

5.2 滑移角 β \beta β 的推导

1. 几何关系：

   • 车辆转弯时，内外轮的轨迹半径不同。
   • 外轮的轨迹半径为 R + l w 2 R + \frac{l_w}{2} R+2lw。
   • 内轮的轨迹半径为 R − l w 2 R - \frac{l_w}{2} R−2lw。

2. 转向角计算：

   • 外轮转角 δ o \delta_o δo 可以表示为：
     δ o = L R + l w 2 (19) \delta_o = \frac{L}{R + \frac{l_w}{2}} \tag{19} δo=R+2lwL(19)
   • 内轮转角 δ i \delta_i δi 可以表示为：
     δ i = L R − l w 2 (20) \delta_i = \frac{L}{R - \frac{l_w}{2}} \tag{20} δi=R−2lwL(20)

3. 滑移角 β \beta β 的计算：

   • 在阿克曼转向几何中，滑移角 β \beta β 主要取决于车辆的几何参数和转向角。
   • 通常情况下，滑移角 β \beta β 可以近似为0，因为阿克曼转向几何设计使得内外轮的转向角能够很好地匹配车辆的转弯半径。

5.3 Python代码实现

class AckermannSteering:
    def __init__(self, L, l_w, R):
        self.L = L
        self.l_w = l_w
        self.R = R
    
    def calculate_steering_angles(self, delta_f):
        delta_o = self.L / (self.R + self.l_w / 2)
        delta_i = self.L / (self.R - self.l_w / 2)
        return delta_o, delta_i

5.4 C++代码实现

#include <cmath>
#include <iostream>

class AckermannSteering {
public:
    AckermannSteering(double L, double l_w, double R) : L(L), l_w(l_w), R(R) {}

    std::pair<double, double> calculate_steering_angles(double delta_f) {
        double delta_o = L / (R + l_w / 2);
        double delta_i = L / (R - l_w / 2);
        return {delta_o, delta_i};
    }

private:
    double L, l_w, R;
};

6. 总结

本文详细介绍了车辆运动学模型的基本概念，并提供了Python和C++的实现代码。通过这些模型，我们可以更好地理解车辆在不同控制输入下的运动行为，这对于开发自动驾驶系统的控制策略至关重要。希望本文能够为自动驾驶领域的研究者和开发者提供有价值的参考。

参考资料

• 【自动驾驶】车辆运动学模型 - CSDN博客

---

以上就是《【自动驾驶】车辆运动学模型》的全部内容，希望能够帮助到你。如果你有任何疑问或者建议，欢迎留言交流！