苏州个人网站制作,合肥关键词排名技巧,联合外发加工网,建设公司企业简介文章目录 1. 运动学方程2. 模型实现 1. 运动学方程
自行车模型#xff08;Bicycle Model#xff09;是车辆数字化模型中最常见的一种运动学模型。其除了可以反映车辆的一些基础特性外#xff0c;更重要的是简单易用。通常情况下我们会把车辆模型简化为二自由度的自行车模型… 文章目录 1. 运动学方程2. 模型实现 1. 运动学方程
自行车模型Bicycle Model是车辆数字化模型中最常见的一种运动学模型。其除了可以反映车辆的一些基础特性外更重要的是简单易用。通常情况下我们会把车辆模型简化为二自由度的自行车模型。
自行车模型主要基于以下假设
车辆是在一个二维平面上运动不考虑车辆垂直平面的Z轴方向上的移动。车辆左右两个轮胎的运动可以合并为一个轮胎来描述即假设车辆左右轮胎在任意时刻都拥有相同或者近乎相同的转向角度和速度。车辆处于低速运动可以忽略前后轴载荷的偏移。车辆整体是刚体结构。
一般情况下我们可以将车辆运动学模型简化为如下形式 δ f \delta_f δf前轮转角 δ r \delta_r δr后轮转角 β \beta β质心侧偏角即质心速度与车身坐标系 X X X轴的夹角 φ \varphi φ横摆角即车的轴线与 X X X轴的夹角 β φ \beta\varphi βφ航向角 v v v质心速度 O O O速度瞬心 C C C质心 L f L_f Lf质心 C C C到前轴距离 L r L_r Lr质心 C C C到后轴距离 L L L轴距 L L f L r LL_fL_r LLfLr
我们对质心速度 v v v进行矢量分解如上图中的 X ˙ \dot{X} X˙和 Y ˙ \dot{Y} Y˙所示可以得到下式子 ( 1 ) (1) (1)和 ( 2 ) (2) (2)根据理论力学刚体角速度公式可得公式 ( 3 ) (3) (3)。由此得到单车模型下的车辆运动学微分模型为 X ˙ v c o s ( β φ ) (1) \dot{X} vcos(\beta\varphi) \tag{1} X˙vcos(βφ)(1) Y ˙ v s i n ( β φ ) (2) \dot{Y} vsin(\beta\varphi) \tag{2} Y˙vsin(βφ)(2) φ ˙ v R (3) \dot{\varphi} \frac{v}{R} \tag{3} φ˙Rv(3) 注一个刚体的角速度 线速度/线速度到速度瞬心的距离 根据图中几何关系和正弦定理可知 L f s i n ( δ f − β ) R s i n ( π 2 − δ f ) (4) \frac{L_f}{sin(\delta_f - \beta)} \frac{R}{sin(\frac{\pi}{2} - \delta_f)} \tag{4} sin(δf−β)Lfsin(2π−δf)R(4) L r s i n ( δ r β ) R s i n ( π 2 − δ r ) (5) \frac{L_r}{sin(\delta_r \beta)} \frac{R}{sin(\frac{\pi}{2} - \delta_r)} \tag{5} sin(δrβ)Lrsin(2π−δr)R(5)
由上式展开可得 L f R s i n δ f c o s β − c o s δ f s i n β c o s δ f t a n δ f c o s β − s i n β (6) \frac{L_f}{R} \frac{sin\delta_f cos\beta - cos\delta_f sin\beta}{cos\delta_f} tan\delta_fcos\beta - sin\beta\tag{6} RLfcosδfsinδfcosβ−cosδfsinβtanδfcosβ−sinβ(6) L r R s i n δ r c o s β c o s δ r s i n β c o s δ r t a n δ r c o s β s i n β (7) \frac{L_r}{R} \frac{sin\delta_r cos\beta cos\delta_r sin\beta}{cos\delta_r} tan\delta_rcos\betasin\beta \tag{7} RLrcosδrsinδrcosβcosδrsinβtanδrcosβsinβ(7)
由载荷的影响质心 C C C位置会发生变化导致 L f L_f Lf和 L r L_r Lr的长度发生变化但是由于 L l f L r L l_f L_r LlfLr是不变的因此由式子 ( 6 ) ( 7 ) (6)(7) (6)(7)可得 L f L r R L R ( t a n δ f t a n δ r ) c o s β (8) \frac{L_f L_r}{R} \frac{L}{R} (tan\delta_f tan\delta_r)cos\beta \tag{8} RLfLrRL(tanδftanδr)cosβ(8) 由 ( 3 ) (3) (3)和 ( 8 ) (8) (8)可得 φ ˙ v R v ( t a n δ f t a n δ r ) c o s β L (9) \dot{\varphi} \frac{v}{R} \frac{v(tan\delta_f tan\delta_r)cos\beta}{L} \tag{9} φ˙RvLv(tanδftanδr)cosβ(9) 由于低速条件下我们可以假设车辆不会发生侧向滑动漂移此时我们可以将 v y ≈ 0 v_y \approx 0 vy≈0因此 β ≈ 0 \beta \approx 0 β≈0则横摆角 φ \varphi φ约等于航向角 φ β \varphi \beta φβ 。又由于大部分车辆不具备后轮转向的功能因此我们可以假设后轮转角 δ r ≈ 0 \delta_r\approx0 δr≈0因此基于我们假设的前提下的运动学微分方程化简为 X ˙ v c o s φ Y ˙ v s i n φ φ ˙ v t a n δ f L (10) \dot{X} vcos\varphi \\ \dot{Y} vsin\varphi \tag{10} \\ \dot{\varphi} \frac{vtan\delta_f}{L} X˙vcosφY˙vsinφφ˙Lvtanδf(10)
2. 模型实现
python代码如下
#!/usr/bin/python
# -*- coding: UTF-8 -*-import math
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Cameraclass Vehicle:def __init__(self,x0.0,y0.0,yaw0.0,v0.0,dt0.1,l3.0):self.steer 0self.x xself.y yself.yaw yawself.v vself.dt dtself.L l # 轴距self.x_front x l * math.cos(yaw)self.y_front y l * math.sin(yaw)def update(self, a, delta, max_steernp.pi):delta np.clip(delta, -max_steer, max_steer)self.steer deltaself.x self.x self.v * math.cos(self.yaw) * self.dtself.y self.y self.v * math.sin(self.yaw) * self.dtself.yaw self.yaw self.v / self.L * math.tan(delta) * self.dtself.v self.v a * self.dtself.x_front self.x self.L * math.cos(self.yaw)self.y_front self.y self.L * math.sin(self.yaw)class VehicleInfo:# Vehicle parameterL 3.0 #轴距W 2.0 #宽度LF 3.8 #后轴中心到车头距离LB 0.8 #后轴中心到车尾距离MAX_STEER 0.6 # 最大前轮转角TR 0.5 # 轮子半径TW 0.5 # 轮子宽度WD W #轮距LENGTH LB LF # 车辆长度def draw_trailer(x, y, yaw, steer, ax, vehicle_infoVehicleInfo, colorblack):vehicle_outline np.array([[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],[vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])wheel np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],[vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2, vehicle_info.TW / 2]])rr_wheel wheel.copy() #右后轮rl_wheel wheel.copy() #左后轮fr_wheel wheel.copy() #右前轮fl_wheel wheel.copy() #左前轮rr_wheel[1,:] vehicle_info.WD/2rl_wheel[1,:] - vehicle_info.WD/2#方向盘旋转rot1 np.array([[np.cos(steer), -np.sin(steer)],[np.sin(steer), np.cos(steer)]])#yaw旋转矩阵rot2 np.array([[np.cos(yaw), -np.sin(yaw)],[np.sin(yaw), np.cos(yaw)]])fr_wheel np.dot(rot1, fr_wheel)fl_wheel np.dot(rot1, fl_wheel)fr_wheel np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])fl_wheel np.array([[vehicle_info.L], [vehicle_info.WD / 2]])fr_wheel np.dot(rot2, fr_wheel)fr_wheel[0, :] xfr_wheel[1, :] yfl_wheel np.dot(rot2, fl_wheel)fl_wheel[0, :] xfl_wheel[1, :] yrr_wheel np.dot(rot2, rr_wheel)rr_wheel[0, :] xrr_wheel[1, :] yrl_wheel np.dot(rot2, rl_wheel)rl_wheel[0, :] xrl_wheel[1, :] yvehicle_outline np.dot(rot2, vehicle_outline)vehicle_outline[0, :] xvehicle_outline[1, :] yax.plot(fr_wheel[0, :], fr_wheel[1, :], color)ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)# ax.axis(equal)if __name__ __main__:vehicle Vehicle(x0.0,y0.0,yaw0,v0.0,dt0.1,lVehicleInfo.L)vehicle.v 1trajectory_x []trajectory_y []fig plt.figure()# 保存动图用# camera Camera(fig)for i in range(600):plt.cla()plt.gca().set_aspect(equal, adjustablebox)vehicle.update(0, np.pi / 10)draw_trailer(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)trajectory_x.append(vehicle.x)trajectory_y.append(vehicle.y)plt.plot(trajectory_x, trajectory_y, blue)plt.xlim(-12, 12)plt.ylim(-2.5, 21)plt.pause(0.001)# camera.snap()# animation camera.animate(interval5)# animation.save(trajectory.gif)运行结果如下 文章首发公众号iDoitnow如果喜欢话可以关注一下
