看到知乎上一篇【轨迹规划】考虑动力学的轨迹规划问题
https://zhuanlan.zhihu.com/p/89718348
感觉有点意思。
假设你驾驶一辆小车想要尽快回家看一场比赛直播。你家离你有1km 远,回家的路是笔直的。如上图所示,这条路在第 0.3km 到 0.7km 间限速 100km/h 。我们用 来表示小车在路上的位置,令
为关于位置参数
的函数,即
。显然,小车位置满足如下函数:
小车的动力学方程为:
其中u是汽车发送机(为正)或刹车(为负)所能提供的加速度。 是空气阻力等因素所形成的非线性阻尼。由于汽车发动机的动力和刹车阻力是有限的,因此加速度需要满足如下约束:
,且非线性阻尼常数
.
对于题目已知的加速度的限制,在 平面上可以表示为下图
假如有这样的轨迹规划任务:已知物体要通过一条确定的路径,要求规划该物体在每个位置处的速度和加速度。我们用位置参数 表示运动物体在整个路径中所处的位置。
表示物体处于路径起点,
表示物体处于路径终点。相应的
表示速度,
表示加速度。则我们可以利用所谓
相图来进行该轨迹规划任务。
下图为一个典型的 相图,其横轴为位置参数
,纵轴为速度参数
。其上有一条曲线,连接了位置起点
和终点
,并表示出了轨迹上各位置点处的速度。可以看到,曲线的起点和终点纵坐标均为0,说明该运动从静止状态出发,最终刚好停在终点处。
进一步考虑运动的加速度限制。我们任取一点 的切线,
, 由动力学方程可得
令 ,则有
,得
.
写了个matlab程序作了下图
clear
close all
u = 9.0*10^4;
b = 1.4;
x0 = 0:0.05:1;
x0 = x0(:);
y0 = zeros(length(x0),1);
c = abs(u - b*y0.^2)./exp(-2*b.*x0);
figure
axis([0 1 0 300])
xlabel('s(km)')
ylabel('sdot(km/h)')
hold on
for j = 1:length(x0)
x = [];
y = [];
k = 1;
for i = 1:100
if i*0.01 > x0(j)
x(k) = i*0.01;
y(k) = sqrt(abs(u - c(j)*exp(-2*b*x(k)))/b);
k = k+1;
end
end
x = [x0(j) x ];
y = [y0(j) y ];
plot(x, y)
end
u = -3.6*10^5;
c = (u - b*y0.^2)./exp(-2*b.*x0);
for j = 1:length(x0)
x=[]; y = [];k = 1;
for i = 1:100
if i*0.01 < x0(j)
x(k) = i*0.01;
y(k) = sqrt(abs(u - c(j)*exp(-2*b*x(k)))/b);
k = k+1;
end
end
x = [ x x0(j)];
y = [ y y0(j)];
plot(x, y)
end