(s,sdot)相图轨迹规划

看到知乎上一篇【轨迹规划】考虑动力学的轨迹规划问题

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