版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/zghforever/article/details/82869519
一、基于地面坐标系建模
如下图所示,车辆运动轨迹的计算建立在地面坐标系上。这里定义
X1轴的指向与车辆功能开启时的车体坐标系
X2轴指向一致,即初始状态车体坐标系与地面坐标系重合。定义车辆的初始位置为
(x1,y1,ψ1),下一时刻车辆位置为
(x2,y2,ψ2),转弯半径为
R,旋转坐标点为
(x0,y0)。定义
R01在
X轴和
Y轴上的投影分别为
H1和
L1,
R02在
X轴和
Y轴上的投影分别为
H2和
L2。假设车辆在
Δt内的速度为
V,车辆在短时间内可以看作是匀速运动。
二、角度关系
ψ2=ψ1+Δψ
其中
Δψ计算如下:
Δψ=RV∗Δt
三、计算投影
计算
R01在
X轴和
Y轴的投影:
H1=R∗sin(ψ1)L1=R∗cos(ψ1)
计算
R02在
X轴和
Y轴的投影:
H2=R∗sin(ψ2)L2=R∗cos(ψ2)
四、计算车辆位置
x2=x1+H2−H1y2=y1+L1−L2
带入得:
x2=x1+R∗(sin(ψ2)−sin(ψ1))y2=y1+R∗(cos(ψ1)−cos(ψ2))
五、考虑方向盘转角正负
定义当车辆方向盘转角为负值时,车辆向右运动;当方向盘转角为正值时,车辆向左运动;当方向盘转角为0时,车辆直线行驶。定义方向盘转角为
θ,则
θ的符号可以通过
sign函数去获取,可以表示为
sign(θ)。
x2=x1+sign(θ)∗R∗(sin(ψ2)−sin(ψ1))y2=y1+sign(θ)∗R∗(cos(ψ1)−cos(ψ2))