车辆运动学方程推导和代码实现
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 L=L_f+L_r L=Lf?+Lr?
我们对质心速度
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??β)Lf??=sin(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?+β)Lr??=sin(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}
RLf??=cosδf?sinδf?cosβ?cosδf?sinβ?=tanδf?cosβ?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\beta+sin\beta \tag{7} RLr??=cosδr?sinδr?cosβ+cosδr?sinβ?=tanδr?cosβ+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
L=lf?+Lr?是不变的,因此由式子
(
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}
RLf?+Lr??=RL?=(tanδf?+tanδ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}
φ˙?=Rv?=Lv(tanδf?+tanδ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 Camera
class Vehicle:
def __init__(self,
x=0.0,
y=0.0,
yaw=0.0,
v=0.0,
dt=0.1,
l=3.0):
self.steer = 0
self.x = x
self.y = y
self.yaw = yaw
self.v = v
self.dt = dt
self.L = l # 轴距
self.x_front = x + l * math.cos(yaw)
self.y_front = y + l * math.sin(yaw)
def update(self, a, delta, max_steer=np.pi):
delta = np.clip(delta, -max_steer, max_steer)
self.steer = delta
self.x = self.x + self.v * math.cos(self.yaw) * self.dt
self.y = self.y + self.v * math.sin(self.yaw) * self.dt
self.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dt
self.v = self.v + a * self.dt
self.x_front = self.x + self.L * math.cos(self.yaw)
self.y_front = self.y + self.L * math.sin(self.yaw)
class VehicleInfo:
# Vehicle parameter
L = 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_info=VehicleInfo, color='black'):
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/2
rl_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, :] += x
fr_wheel[1, :] += y
fl_wheel = np.dot(rot2, fl_wheel)
fl_wheel[0, :] += x
fl_wheel[1, :] += y
rr_wheel = np.dot(rot2, rr_wheel)
rr_wheel[0, :] += x
rr_wheel[1, :] += y
rl_wheel = np.dot(rot2, rl_wheel)
rl_wheel[0, :] += x
rl_wheel[1, :] += y
vehicle_outline = np.dot(rot2, vehicle_outline)
vehicle_outline[0, :] += x
vehicle_outline[1, :] += y
ax.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(x=0.0,
y=0.0,
yaw=0,
v=0.0,
dt=0.1,
l=VehicleInfo.L)
vehicle.v = 1
trajectory_x = []
trajectory_y = []
fig = plt.figure()
# 保存动图用
# camera = Camera(fig)
for i in range(600):
plt.cla()
plt.gca().set_aspect('equal', adjustable='box')
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(interval=5)
# animation.save('trajectory.gif')
运行结果如下:
文章首发公众号:iDoitnow如果喜欢话,可以关注一下
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符,请联系我的编程经验分享网邮箱:veading@qq.com进行投诉反馈,一经查实,立即删除!