無人駕駛汽車系統入門（五）——運動學自行車模型和動力學自行車模型

在簡要了解了PID控制以後，我們就要接觸一些現代的控制算法。在了解進階的車輛控制算法之前，掌握車輛運動模型是非常有必要的。車輛運動模型就是一類能夠描述我們的車輛的運動規律的模型。顯然，越複雜的模型就越接近現實中的車輛運動規律，本節我們一起了解一下兩個廣泛使用的車輛模型——運動學自行車模型（Kinematic Bicycle Model） 和 動力學自行車模型（Dynamic Bicycle Model）

原創不易，轉載請注明出處：http://blog.csdn.net/adamshan/article/details/78696874

無人駕駛系統往往分成感覺，決策和控制三個子產品，其中無人車的路徑規劃和底層控制是工作在不同的層的，路徑規劃層往往會基于更加高層的（感覺層，定位層）的資訊和底層的（控制層）的實時資訊指定行駛的路徑，那麼從路徑規劃層傳來的就是車輛的參考路徑，控制系統需要做的就是嚴格按照這個參考路徑（以及速度等控制輸入量）去駕駛我們的車輛，一般來說，我們會用多項式的行駛來描述這個路徑曲線，如下所示的三次多項式就可以描述絕大多數的路徑了：

y=ax3+bx2+cx+d

無人車的控制依賴于一項稱為 模型預測控制(Model Predictive Control) 的簡單技術，這種控制的方法是産生一系列的可行的（也就是說在實際上車輛能夠做到的）控制輸入，基于一定的算法（往往是帶限制的非線性優化算法）來調整這一系列的控制輸入，使得一個 損失函數(cost function) 最小化,這個損失函數的求解就要依賴于車輛的運動學或者動力學模型的輸出和參考的路徑的內插補點求得，這些模型正是本節介紹的内容。

自行車模型（Bicycle Model）

首先我們要簡化汽車運動，其中自行車模型就是簡單且有效的簡化方式。自行車模型基于如下幾個假設：

• 車輛在垂直方向的運動被忽略掉了，也就是說我們描述的車輛是一個二維平面上的運動物體（可以等價與我們是站在天空中的俯視視角）
• 我們假設車輛的結構就像自行車一樣，也就是說車輛的前面兩個輪胎擁有一直的角度和轉速等，同樣後面的兩個輪胎也是如此，那麼前後的輪胎就可以各用一個輪胎來描述
• 我們假設車輛運動也和自行車一樣，這意味着是前面的輪胎控制這車輛的轉角

首先我們簡單的在一個二維平面上描述一個車輛：

其中 θ 是其在 Yaw 方向的偏轉角度，它是相對于 x 軸的逆時針方向的角度，v 是 θ 方向的速度， L 是車輛的軸距（前後輪胎的距離）, (x,y) 是車輛的坐标。

下圖是該車輛的自行車模型：

運動學自行車模型

作為一種自行車模型，運動學自行車模型也假定車輛形如一輛自行車，整個的控制量可以簡化為 (a, δf) ，其中 a 是車輛的加速度，踩油門踏闆意味着正的加速度，踩刹車踏闆意味着負的加速度。δf 是我們的方向盤轉角，我們假定這個方向盤轉角就是前輪胎目前的轉角。這樣，我們使用兩個量描述了車輛的 控制輸入(control input)。

然後我們定義我們模型中的狀态量，運動學自行車模型使用四個狀态量來描述車輛的目前狀态：

• x : 即車輛目前的 x 坐标
• y : 即車輛目前的 y 坐标
• ψ : 即車輛目前的偏航角（ Yaw 方向的偏角，往往用弧度來描述, 逆時針方向為正）
• v : 即車輛的速度

一個簡單的運動學自行車模型如圖所示：

其中 lf 和 lr 為前輪和後輪到車輛重心的距離。那麼更具運動學定理，運動學自行車模型中的各個狀态量的更新公式如下：

xt+1=xt+vtcos(ψt+β)×dt

yt+1=yt+vtsin(ψt+β)×dt

ψt+1=ψt+vtlrsin(β)×dt

vt+1=vt+a×dt

其中 β 可以由如下公式計算求得：

β=tan−1(lrlf+lrtan(δf))

由于絕大多數的汽車後輪都不能夠偏轉，是以我們的自行車模型就假定後輪的轉角控制輸入 δr=0 ，也就是說，方向盤上的控制輸入，都反映到了前輪的轉角上了。

那麼基于這個簡單的運動學自行車模型，在給定了一個時刻的控制輸入以後，我們可以計算求得 dt 時間以後我們車輛的狀态資訊（坐标，偏航角以及速度），那麼這個模型就可以作為我們模型預測控制下的基礎車輛模型了。

動力學自行車模型

前面的車輛運動學自行車模型其實還隐含着一個重要的假設，那就是： 車前輪的方向即是車輛目前的速度方向，在實際車輛運動過程中，當車輛在以相對高的速度行駛時，車輪的方向并不一定車輛目前的速度方向，這個時候，我們引入車輛的動力學自行車模型。

車輛動力學模型通過對輪胎和路面之間的複雜互相作用來描述車輛的運動。在一個動力模型中，我們需要考慮各種各樣的力的作用，他們可以大緻分為兩類： 縱向力(Longitudinal force) 和 側向力（Lateral force）, 縱向力就是使車輛前後移動的力量，而側向力則促使車輛在橫向移動，在力的互相作用過程中，輪胎起着決定性的作用（根據一定的實體常識，輪胎是車輛運動的一個重要的力的來源）。

我們仍然以上圖為例，考慮一個簡單的動力學模型，這個模型的各個狀态量為 (x˙,y˙,ψ˙,X,Y) , 其中 x˙ 和 y˙ 表示的是車身的縱向和側向速度， ψ˙ 表示的是偏航角速度， (X,Y) 表示車車身的目前坐标，這些狀态量在時間尺度上的微分方程如下：

其中， m 和 Iz 分别表示車輛的品質和偏航慣性(yaw inertia), Fc,f 和 Fc,r 分别表示的是前後輪胎受到的側向力，他們可以通過具體的輪胎模型求得，在簡單的線性輪胎模型中， Fc,i, (i=f or r) 表示為：

Fc,i=−Cαiαi

其中 αi 是輪胎的偏轉角，這個偏轉角是指輪胎目前的朝向和目前的速度的夾角， Cαi 被稱為輪胎偏滾剛度（tire cornering stiffness）。

運動學自行車模型的Python實作

我們使用Python代碼簡單實作一個運動學自行車模型類，作為後期我們使用模型預測控制的車輛模型。

from __future__ import print_function

import math


class KinematicModel(object):
    def __init__(self, x, y, psi, v, f_len, r_len):
        self.x = x
        self.y = y
        self.psi = psi
        self.v = v

        self.f_len = f_len
        self.r_len = r_len

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

    def update_state(self, a, delta, dt):
        beta = math.atan((self.r_len / (self.r_len + self.f_len)) * math.tan(delta))

        self.x = self.x + self.v * math.cos(self.psi * beta) * dt
        self.y = self.y + self.v * math.sin(self.psi * beta) * dt
        self.psi = self.psi + (self.v / self.f_len) * math.sin(beta) * dt
        self.v = self.v + a * dt
        return self.x, self.y, self.psi, self.v
           

這個簡易的車輛模型中，我們的控制量包含了一個前胎的轉角和一個加速度a，由于受車輛機械的限制，車輛本身有很多動作是實作不了的，比如說加速度a的值不可能過大，輪胎的轉角也會有極限，我們稱這種性質叫做模型非完整性（model nonholonomic）