手把手教你做python无人驾驶车辆控制之车辆运动学模型(二)
车辆运动学模型
以车辆质心为研究点的模型,更符合车辆运动特性。
其中,O为车辆瞬时转动中心,β为滑移角,指车辆行进方向和轮圈所指方向两者间所成的角度;ψ为航向角,指车身与X轴的夹角。
模型假设:
- 车辆无垂向运动,仅在XY平面运动
- 车辆左右两侧轮胎偏角相同,可简化为自行车模型
- 车辆为前驱,可假设后轮偏角δr=0
python代码实现
class vehicle:
def __init__(self,x=0.0,y=0.0,yaw=0.0,v=0.0):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
self.delta = 0
self.dt = 0.1
def update(self,a,steering):
self.x = self.x + self.v*math.cos(self.yaw + self.delta)*dt
self.y = self.y + self.v*math.sin(self.yaw + self.delta)*dt
self.yaw = self.yaw + self.v/l_r*math.tan(self.delta)*dt
self.v = self.v + a*dt
self.delta = math.atan(l_r/(l_f+l_r)*math.tan(steering))