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

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

在簡要了解了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）