车辆运动学模型（自行车模型）

大家好，欢迎来到IT知识分享网。

模型建立

$c$ – 是前后轮中心点连线上的一个参考点，一般取车辆的质心位置。v表示该点的速度，即车辆质心速度。
$\psi$ – 车辆当前位置与横坐标夹角，即车辆航向角。
$\omega$ – 角速度
$\delta$ – 车辆前轮转角
$l_r$ – 质心与后轮中心距离
$l_f$ – 质心与前轮中心距离

模型推导

python代码

import numpy as np class KinematicModel(object): def __init__(self, delta_t, l_f, l_r): self.delta_t = delta_t self.l_f = l_f self.l_r = l_r def next_state(self, x, y, v, head, delta): beta = np.arctan(self.l_r/(self.l_r+self.l_f)*np.tan(delta)) x = x + v*np.cos(head+beta)*self.delta_t y = y + v*np.sin(head+beta)*self.delta_t head = head + v/(self.l_r+self.l_f)*np.cos(beta)*np.tan(delta)*self.delta_t return [x, y, head]