机器人在笛卡尔空间下两直线间的平滑过渡算法详解（附Matlab代码）

热心市民R先生

已于 2022-07-08 08:53:39 修改

阅读量2.1k

点赞数 16

分类专栏： Matlab机器人轨迹规划文章标签： matlab

于 2022-07-07 22:50:13 首次发布

本文链接：https://blog.csdn.net/h542812556sdu/article/details/125659171

版权

Matlab机器人轨迹规划专栏收录该内容

3 篇文章 5 订阅

订阅专栏

本文要有空间向量的思维，讲述了机器人在笛卡尔空间下两条直线轨迹之间的平滑过渡算法，条理清晰，希望各位同学点赞加关注，谢谢！！！

直奔主题。如图一所示，在笛卡尔空间中直线AB和BC的平滑过渡部分由蓝色虚线所示（过渡从A’开始到C‘结束），已知笛卡尔空间中的三个点A（a_x,a_y,a_z）、B(b_x,b_y,b_z)、C(c_x,c_y,c_z)，再给定直线AB和BC的速度v1和v2（注意，这里AA’段的速度为v1，CC’段的速度为v2，速度由v1变到v2发生在过渡阶段，这里的速度变化不只是大小，还有方向，要把速度当成一个矢量）。

图一

Step1：笛卡尔空间中过渡段（图一中蓝色虚线）的位移函数为p(t)，过渡从0时刻开始到△T结束，其形式如图二所示：

图二

Step2：过渡段的运动方程p(t),p’(t),p’’(t)如下：

加速度方程为：

（1-1）

上式中的KAB，KBC表示直线段AB和BC的单位向量。v1,v2是给定的常数。那么v1KAB、v2KBC、就表示速度矢量，其大小是v1、v2，方向是KAB、KBC的方向。

对上式（1-1）求解定积分可得：

（1-2）

式（1-2）中v1KAB物理意义是t=0时刻过渡段的起始速度（大小和方向），v2KBC的物理意义是t=△T时刻过渡段的最终速度（大小和方向），（v2KBC-v1KAB）的物理意义是过渡段速度的变化量（大小和方向）。

对上式（1-2）继续求解定积分可得：

（1-3）

Step3：通过分析上诉三个方程，已知量是v1，v2，我们要求未知量KAB，KBC，△T、A’;

首先求直线AB和BC的单位向量KAB，KBC，如图三所示：

图三

然后求未知数△T、A’，如下：

因为过渡总时间为△T，过渡起点是A‘，终点是C‘，所以△T时刻到达C‘。

将△T带入式（1-3）并化简，那么式（1-4）成立。

（1-4）

由式（1-4）可得式（1-5）：

（1-5）

式（1-5）两边同时减去B可得下式（1-6）：

（1-6）

又因为下式（1-7）成立：

（1-7）

所以将上式（1-7）带入上式（1-6）并化简后可得下式（1-8）成立：

（1-8）

将上面的式子右侧展开，由KAB和KBC前面对应的数是相等的，所以下面两个式子（1-9）成立：

（1-9）

所以下两式（1-10）成立：

求出d1后，则A’=B-d1*KAB。至此所有未知数求解完毕。

Step4：

根绝给定条件计算过渡阶段的p(t),p’(t),p’’(t)。

条件一：给定v1和v2和△T

给定v1和v2、△T，需要求出d1.进而求得A‘，进而得到p(t),p’(t),p’’(t)。其matlab代码如下：

%已知v1和v2，给定平滑过渡时间△T(代码用dt表示)。
v1=1;v2=2;dt=0.2;
%给定笛卡尔空间中的三个点
A=[3 ;5 ;8];
B=[2 ;6 ;9];
C=[11 ;4 ;87];
%求出向量AB.BA以及单位向量Kab，Kbc。
AB=B-A;
BC=C-B;
Kab=(B-A)/(sqrt(AB(1)^2+AB(2)^2+AB(3)^2));
Kbc=(C-B)/(sqrt(BC(1)^2+BC(2)^2+BC(3)^2));
%求出d1、d2、A'(代码用A1表示)。
d1=v1*dt/2;
d2=v2*dt/2;
A1=B-d1*Kab;
%创建时间t的数组，从0到△T。
T=linspace(0,dt,300);
%创建存放位移、速度、加速度的数组。
PPOS=[];
PVEL=[];
PACC=[];
%依据公式计算位移PPOS、速度PVEL、加速度PACC。
for t=T
    ppos=A1+v1*Kab*t+((v2*Kbc-v1*Kab)*t^2)/(2*dt);
    PPOS=[PPOS,ppos];
    pvel=v1*Kab+(v2*Kbc-v1*Kab)*t/dt;
    PVEL=[PVEL,pvel];
    pacc=(v2*Kbc-v1*Kab)/dt;
    PACC=[PACC,pacc];
end
%转置
PPOS=PPOS';
PVEL=PVEL';
PACC=PACC';
%绘图
f1=figure('Name',"xyz三个方向位移");
plot(T,PPOS(:,1),T,PPOS(:,2),T,PPOS(:,3));
grid on;
f2=figure('Name',"xyz三个方向速度");
plot(T,PVEL(:,1),T,PVEL(:,2),T,PVEL(:,3));
grid on;
f3=figure('Name',"xyz三个方向的加速度");
plot(T,PACC(:,1),T,PACC(:,2),T,PACC(:,3));
grid on;
f4=figure('Name',"轨迹");
plot3(PPOS(:,1),PPOS(:,2),PPOS(:,3));
grid on;
f5=figure('Name',"总速度");
plot(T,sqrt(PVEL(:,1).^2+PVEL(:,2).^2+PVEL(:,3).^2));
grid on;
f6=figure('Name',"总加速度");
plot(T,sqrt(PACC(:,1).^2+PACC(:,2).^2+PACC(:,3).^2));
grid on;

条件二：给定v1和v2和d1

给定v1和v2和d1,需求出过渡时间△T，进而得到p(t),p’(t),p’’(t)。其matlab代码如下：

%已知v1和v2，给定平滑过渡距离d1。
v1=1;v2=3;d1=0.1;
%给定笛卡尔空间中的三个点
A=[3 ;5 ;8];
B=[2 ;6 ;9];
C=[11 ;4 ;87];
%求出向量AB.BA以及单位向量Kab，Kbc。
AB=B-A;
BC=C-B;
Kab=(B-A)/(sqrt(AB(1)^2+AB(2)^2+AB(3)^2));
Kbc=(C-B)/(sqrt(BC(1)^2+BC(2)^2+BC(3)^2));
%求出△T(代码用dt表示)、d2、A'(代码用A1表示)。
dt=2*d1/v1;
d2=d1*v2/v1;
A1=B-d1*Kab;
%创建时间t的数组，从0到△T。
T=linspace(0,dt,300);
%创建存放位移、速度、加速度的数组。
PPOS=[];
PVEL=[];
PACC=[];
%依据公式计算位移PPOS、速度PVEL、加速度PACC。
for t=T
    ppos=A1+v1*Kab*t+((v2*Kbc-v1*Kab)*t^2)/(2*dt);
    PPOS=[PPOS,ppos];
    pvel=v1*Kab+(v2*Kbc-v1*Kab)*t/dt;
    PVEL=[PVEL,pvel];
    pacc=(v2*Kbc-v1*Kab)/dt;
    PACC=[PACC,pacc];
end
%转置
PPOS=PPOS';
PVEL=PVEL';
PACC=PACC';
%绘图
f1=figure('Name',"xyz三个方向位移");
plot(T,PPOS(:,1),T,PPOS(:,2),T,PPOS(:,3));
grid on;
f2=figure('Name',"xyz三个方向速度");
plot(T,PVEL(:,1),T,PVEL(:,2),T,PVEL(:,3));
grid on;
f3=figure('Name',"xyz三个方向的加速度");
plot(T,PACC(:,1),T,PACC(:,2),T,PACC(:,3));
grid on;
f4=figure('Name',"轨迹");
plot3(PPOS(:,1),PPOS(:,2),PPOS(:,3));
grid on;
f5=figure('Name',"总速度");
plot(T,sqrt(PVEL(:,1).^2+PVEL(:,2).^2+PVEL(:,3).^2));
grid on;
f6=figure('Name',"总加速度");
plot(T,sqrt(PACC(:,1).^2+PACC(:,2).^2+PACC(:,3).^2));
grid on;

条件三：给定vmax（为简单起见，让v1=v2=vmax）和amax，需要求得△T和d1。

由给定条件，所以下两式（1-11）成立

（1-11）

由式（1-1）和上两式（1-11可求出△T，如下式（1-12）。

（1-12）

又由式（1-9）可求出d1，d2.如下式（1-13）所示

（1-13）

matlab如下所示：

%两个直线之间的平滑过度算法,此算法需给定平滑过渡的最大速度vmax和最大加速度amax，且v1=v2=vmax。
vmax=2;
v1=vmax;
v2=v1;
amax=5;
%给定笛卡尔空间中的三个点
A=[3 ;5 ;8];
B=[2 ;6 ;9];
C=[11 ;4 ;87];
%求出向量AB.BA以及单位向量Kab，Kbc。
AB=B-A;
BC=C-B;
Kab=(B-A)/(sqrt(AB(1)^2+AB(2)^2+AB(3)^2));
Kbc=(C-B)/(sqrt(BC(1)^2+BC(2)^2+BC(3)^2));
%求出△T(代码用dt表示)、d1、d2、A'(代码用A1表示)。
dt=(vmax/amax)*(sqrt(2*(1-Kbc(1)*Kab(1)-Kbc(2)*Kab(2)-Kbc(3)*Kab(3))));
d1=vmax*dt/2;
d2=d1;
A1=B-d1*Kab;
%创建时间t的数组，从0到△T。
T=linspace(0,dt,300);
%创建存放位移、速度、加速度的数组。
PPOS=[];
PVEL=[];
PACC=[];
%依据公式计算位移PPOS、速度PVEL、加速度PACC。
for t=T
    ppos=A1+v1*Kab*t+((v2*Kbc-v1*Kab)*t^2)/(2*dt);
    PPOS=[PPOS,ppos];
    pvel=v1*Kab+(v2*Kbc-v1*Kab)*t/dt;
    PVEL=[PVEL,pvel];
    pacc=(v2*Kbc-v1*Kab)/dt;
    PACC=[PACC,pacc];
end
%转置
PPOS=PPOS';
PVEL=PVEL';
PACC=PACC';
%绘图
f1=figure('Name',"xyz三个方向位移");
plot(T,PPOS(:,1),T,PPOS(:,2),T,PPOS(:,3));
grid on;
f2=figure('Name',"xyz三个方向速度");
plot(T,PVEL(:,1),T,PVEL(:,2),T,PVEL(:,3));
grid on;
f3=figure('Name',"xyz三个方向的加速度");
plot(T,PACC(:,1),T,PACC(:,2),T,PACC(:,3));
grid on;
f4=figure('Name',"轨迹");
plot3(PPOS(:,1),PPOS(:,2),PPOS(:,3));
grid on;
f5=figure('Name',"总速度");
plot(T,sqrt(PVEL(:,1).^2+PVEL(:,2).^2+PVEL(:,3).^2));
grid on;
f6=figure('Name',"总加速度");
plot(T,sqrt(PACC(:,1).^2+PACC(:,2).^2+PACC(:,3).^2));
grid on;