机器人轨迹规划:三次样条曲线
文章目录
- 机器人轨迹规划:三次样条曲线
-
- 写在前面
-
- 学习代码都记录在[个人github](https://github.com/xuuyann/RobotLearningCode)上,欢迎关注~
- 三次样条曲线性质
- 当指定初始速度v0和最终速度vn时的参数计算(也就是v0和vn已知)
-
- 例子
- 周期三次样条:没有指定初始速度v0和最终速度vn(也就是v0和vn未知)
-
- 例子
- 具有指定初始速度v0和最终速度vn的三次样条(v0和vn已知):基于加速度的计算
- 具有指定初始、最终速度以及加速度的三次样条曲线
-
- 例子
- 参考文献(实际上我就是翻译了一下下。。。)
写在前面
学习代码都记录在个人github上,欢迎关注~
在一些避障的应用场景下,一般都是先在任务空间中对多轴机械臂的末端进行路径规划,得到的是末端的运动路径点数据。这条轨迹只包含位置关系,并没有告诉机器人应该以怎样的速度、加速度运动,这就需要进行带时间参数的轨迹规划处理,也就是对这条空间轨迹进行速度、加速度约束,并且计算运动到每个路点的时间,高级的算法有TOPP等,一般的呢就是贝塞尔、三次准/非/均匀B、五次及三次样条等。下面从最简单的三次样条开始讨论。
三次样条曲线性质
当给出n+1个点时,可以使用n个p次多项式(通常较低)代替唯一的n次插值多项式,每个多项式定义一段轨迹。以这种方式定义的总函数s(t)称为p阶的样条曲线。p的值是根据所需的样条连续度来选择的。例如,为了在两个连续段之间发生过渡的时刻tk获得速度和加速度的连续性,可以假定多项式的阶数p=3(三次多项式)。
定义三次样条曲线的函数形式为:
这段轨迹由n个三次多项式构成,并且每个多项式需要计算四个参数。由于n个多项式是定义一条通过n+1点的轨迹所必需的,因此需要确定的系数总数为4n。为了解决这个问题,必须考虑以下条件:
- 给定点插值的2n条件,因为每一个三次函数必须在其极值处穿过点。
- n-1个条件,过渡点的速度要连续;
- n-1个条件,过渡点的加速度要连续;
这样的话,就已经限制了2n+2(n-1)个条件,还剩下2个自由度还未限制。通过前面分析,还需要两个限制条件才行,这里讨论的就是初始点和终点的速度以及加速度。下面是几种可能的选择,可以任意选择:
通常情况,样条曲线具有如下几个特性:
- 对于由给定点(tk,qk),k=0,…n得到的p阶样条曲线s(t),[n(p+1)]个参数可以确定
- 给定n+1个点,并且给定边界条件,则p阶插值样条曲线s(t)能被唯一确定
- 用于构造样条曲线的多项式的阶数p不取决于数据点的数目
- 函数s(t)p-1阶连续可导
- 自然样条曲线是指初始加速度和最终加速度均为0的样条曲线
当指定初始速度v0和最终速度vn时的参数计算(也就是v0和vn已知)
在定义自动机械的轨迹时,速度剖面的连续性条件至关重要。因此,计算样条曲线的典型选择是指定初始和最终速度v0和vn。因此,给定点(tk,qk),k=0,…n以及速度的边界条件(初始速度和最终速度)v0,vn,就有如下几个条件成立:
可以最终确定样条曲线的函数s(t)为
系数ak,i可以由以下算法进行确定: 第一种情况,如果中间点(插补点)的速度我们已知,也就是vk,k=1,…,n-1,对于每段三次样条曲线,有
这就可以把每一段曲线的系数都求出来,从而得到样条曲线。这是最简单的情况! 子函数如下:
% 三次样条:指定初始速度v0和终止速度vn,并且中间插补点的速度已知,这是最简单的情况
% Input:
% q:给定点的位置
% t:给定点位置对应的时间
% v:包括给定起始、中间及终止速度的速度向量
% tt:插补周期
% Output:
% yy dyy ddyy:样条曲线函数值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_1(q, t, v, tt)
if length(q) ~= length(t)
error('输入的数据应成对')
end
n = length(q);
T = t(n) - t(1); % 运行总时长
nn = T / tt; % 总点数
yy = zeros(1, nn);
dyy = zeros(1, nn);
ddyy = zeros(1, nn);
j = 1;
for i = 1: n-1
Tk = t(i+1) - t(i);
a0 = q(i);
a1 = v(i);
a2 = (1/Tk) * ((3*(q(i+1)-q(i)))/Tk - 2*v(i) - v(i+1));
a3 = (1/(Tk*Tk)) * ((2*(q(i)-q(i+1)))/Tk + v(i) + v(i+1));
for tk = t(i): tt: t(i+1)
if i > 1 && tk == t(i)
continue
end
yy(j) = a0 + a1*(tk-t(i)) + a2*power(tk-t(i), 2) + a3*power(tk-t(i), 3);
dyy(j) = a1 + 2*a2*(tk-t(i)) + 3*a3*power(tk-t(i), 2);
ddyy(j) = 2*a2 + 6*a3*(tk-t(i));
j = j + 1;
end
end
end
第二种情况,如果中间点的速度我们未知,也就是vk,k=1,…,n-1均未知,然而这些值是必须被计算的。为此,考虑了中间加速度的连续条件:
其中常数项ck仅取决于中间位置和已知的样条曲线段的持续时间Tk。由于速度v0和vn也是已知的,所以可以消除矩阵A‘的相应列并获得
另外v0=2,v6=-3。
% 三次样条:指定初始速度v0和终止速度vn,但是中间点速度未知
% Input:
% q:给定点的位置
% t:给定点位置对应的时间
% v0:初始速度
% vn:终止速度
% tt:插补周期
% Output:
% yy dyy ddyy:样条曲线函数值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_2(q, t, v0, vn, tt);
if length(q) ~= length(t)
error('输入的数据应成对');
end
n = length(q);
c = zeros(n-2, 1);
% 矩阵A是个(n-2)*(n-2)的对角占优矩阵
A = zeros(n-2);
for i = 1: n-2
Tk_1 = t(i+2) - t(i+1);
Tk = t(i+1) - t(i);
if i == 1
A(i, i) = 2*(Tk + Tk_1);
A(i, i+1) = Tk;
c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i))) - Tk_1*v0;
elseif i == n-2
A(i, i-1) = Tk_1;
A(i, i) = 2*(Tk + Tk_1);
c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i))) - Tk*vn;
else
A(i, i-1) = Tk_1;
A(i, i) = 2*(Tk + Tk_1);
A(i, i+1) = Tk;
c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i)));
end
end
% 经过上述步骤得到对角占优矩阵A和c
% vk = A \ c; % 这一步matlab计算很慢,应换成追赶法求vk
for i = 1: n-2
a(i) = A(i, i); % 对角线
if i == n-3
b(i) = A(i, i+1); % 上边
d(i) = A(i+1, i); % 下边
continue;
elseif i < n-2
b(i) = A(i, i+1); % 上边
d(i) = A(i+1, i); % 下边
end
end
[~, ~, vk] = crout(a, b, d, c); % 追赶法
% 得到中间插补点的速度vk,然后调用cubicSpline_1即可
v_ = [v0, vk', vn];
[yy dyy ddyy] = cubicSpline_1(q, t, v_, tt);
end
%追赶法求解三对角线性方程组,Ax=b,A用一维数组a,c,d存储。
function [L,U,x]=crout(a,c,d,b)%数组a存储三角矩阵A的主对角线元素,c、d存储主对角线上边下边带宽为1的元素
n=length(a);
n1=length(c);
n2=length(d);
%错误检查
if n1~=n2%存储矩阵的数组维数错误
error('MATLAB:Crout:不是三对角矩阵,参数数组中元素个数错误.');
elseif n~=n1+1
error('MATLAB:Crout:不是三对角矩阵,参数数组中元素个数错误.');
end
%初始化
L=zeros(n);%生成n*n的全零矩阵
U=zeros(n);
p=1:n;
q=1:n-1;
x=1:n;
y=1:n;
%追赶法程序主体
p(1)=a(1);
for i=1:n-1
q(i)=c(i)/p(i);
p(i+1)=a(i+1)-d(i)*q(i);%d的下标改为1到n-1
end
%正解y
y(1)=b(1)/p(1);%用x存储y
for i=2:n
y(i)=(b(i)-d(i-1)*y(i-1))/p(i);
end
%倒解x
x(n)=y(n);
for i=(n-1):-1:1
x(i)=y(i)-q(i)*x(i+1);
end
%L,U矩阵
for i=1:n
L(i,i)=p(i);
U(i,i)=1;
end
for i=1:n-1
L(i+1,i)=d(i);
U(i,i+1)=q(i);
end
end %end of function
测试:
%% 自写cubicSpline_2函数测试
q = [3, -2, -5, 0, 6, 12, 8];
t = [0, 5, 7, 8, 10, 15, 18];
n = length(t);
v0 = 2; vn = -3; tt = 0.1;
[yy dyy ddyy] = cubicSpline_2(q, t, v0, vn, tt);
subplot(3, 1, 1)
plot(t, q, 'o');
ylabel('位置')
grid on
hold on
plot([t(1):tt:t(n)], yy);
subplot(3, 1, 2)
plot([t(1), t(n)], [v0, vn], 'o');
grid on
hold on
plot([t(1):tt:t(n)], dyy);
ylabel('速度')
subplot(3, 1, 3)
grid on
hold on
plot([t(1):tt:t(n)], ddyy);
ylabel('加速度')
周期三次样条:没有指定初始速度v0和最终速度vn(也就是v0和vn未知)
在许多应用中,要执行的运动是周期性的,即初始位置和最终位置相同。在这种情况下,利用为计算样条曲线而指定的最后两个自由度,以使曲线具有初始和最终速度和加速度的连续性。因此,计算系数的方法与先前报告的略有不同。事实上,在这种情况下,必须考虑代替任意选择的初始和最终速度v0和vn的条件。
系统的矩阵不再是三对角的。这种情况被称为循环三对角系统,也存在有效的求解计算方法。一旦获得速度v0,…,vn−1,就可以通过(4.8)计算样条曲线的系数。
例子
% 三次样条:周期三次样条,没有指定初始速度v0和终止速度vn,也就是v0和vn未知
% Input:
% q:给定点的位置
% t:给定点位置对应的时间
% tt:插补周期
% Output:
% yy dyy ddyy:样条曲线函数值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_3(q, t, tt)
if length(q) ~= length(t)
error('输入的数据应成对');
end
n = length(q);
c = zeros(n-1, 1);
% 矩阵A是个(n-1)*(n-1)的循环三对角矩阵
A = zeros(n-1);
for i = 1: n-1
if i == 1
Tn_1 = t(n) - t(n-1);
T0 = t(i+1) - t(i);
A(i, i) = 2*(Tn_1 + T0);
A(i, i+1) = Tn_1;
A(i, n-1) = T0;
c(i, 1) = (3/(Tn_1*T0))*((Tn_1^2)*(q(i+1)-q(i))+(T0^2)*(q(n)-q(n-1)));
else
Tk_1 = t(i+1) - t(i);
Tk = t(i) - t(i-1);
c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+1)-q(i))+Tk_1^2*(q(i)-q(i-1)));
if i == n-1
A(i, 1) = Tk_1;
A(i, i-1) = Tk_1;
A(i, i) = 2*(Tk + Tk_1);
else
A(i, i-1) = Tk_1;
A(i, i) = 2*(Tk + Tk_1);
A(i, i+1) = Tk;
end
end
end
% 经过上述步骤得到矩阵A和c
vk = A \ c; % 这一步matlab计算很慢,应换成追赶法求vk
% 这个vk的第一个值为v0,然后v0和vn相等
% 得到中间插补点的速度vk,然后调用cubicSpline_1即可
v_ = [vk', vk(1)];
% v_ = [-2.28 -2.78 2.99 5.14 2.15 -1.8281 -2.28]
[yy dyy ddyy] = cubicSpline_1(q, t, v_, tt);
end
测试:
%% 自写cubicSpline_3函数测试
q = [3, -2, -5, 0, 6, 12, 3];
t = [0, 5, 7, 8, 10, 15, 18];
t1 = [18, 23, 25, 26, 28, 33, 36];
n = length(t);
tt = 0.1;
[yy dyy ddyy] = cubicSpline_3(q, t, tt);
[yy_, dyy_, ddyy_] = cubicSpline_3(q, t1, tt);
subplot(3, 1, 1)
plot(t, q, 'o');
ylabel('位置')
grid on
hold on
plot([t(1):tt:t(n)], yy);
plot([t1(1):tt:t1(n)], yy_);
subplot(3, 1, 2)
% plot([t(1), t(n)], [v0, vn], 'o');
grid on
hold on
plot([t(1):tt:t(n)], dyy);
plot([t1(1):tt:t1(n)], dyy_);
ylabel('速度')
subplot(3, 1, 3)
grid on
hold on
plot([t(1):tt:t(n)], ddyy);
plot([t1(1):tt:t1(n)], ddyy_);
ylabel('加速度')
这个方法主要是为下面的方法做准备,因此不写例子。
具有指定初始、最终速度以及加速度的三次样条曲线
样条曲线是一个二阶连续导数的函数,但通常不可能同时指定初始速度和最终速度以及加速度。因此,样条曲线在其末端的特征是速度或加速度的不连续性,一般情况下我们会指定初始和最终速度,则此时初始和最终加速度难以保证连续,会出现突变。下面需要做的就是需要保证在指定初始速度和最终速度的前提下,还要保证初始、最终加速度从0开始连续变化。如果这些不连续代表一个问题,可以采用不同的方法:
- 一个5次多项式函数可以用于第一个和最后一个域,其缺点是允许在这些段中有更大的超调,并且稍微增加了计算负担;
- 在第一段和最后一段中添加两个自由额外点(从这个意义上说,这些点不能先验地固定),并通过施加所需的速度和加速度的初始值和最终值来计算它们的值。
后一种方法现在详细说明。让我们考虑一个要插值的n-1点向量,这两个向量中都没有第二个值以及倒数第二个值,也就是q1、t1和qn-1、tn-1(目前我还不知道为啥要这么做。。。)
增加的这两个点,可以通过已知的变量去表达这两个点,即初始/最终的位置、速度以及加速度(q0/qn,v0/vn,a0/an)同时包括在这些点上的加速度w1和wn-1(其中边界点的加速度是用a0和an来表示,而中间点的加速度是用w来表示)。这样,就可以考虑初始加速度和最终加速度的约束。
将(4.26)和(4.27)替换掉(4.24)中的有关项,通过重新排列n-1方程,我们得到一个线性系统
其中
例子
其中v0=2,vn=-3,a0=0,an=0。额外增加的两个时间点t1=2.5,t7=16.5,当然也可以任意选择。
% 三次样条:指定初始、终止速度以及加速度,也就是v0,vn,a0,an已知
% 这个方法需要增加两个额外的点q1_和qn-1_
% q1_在原有q1和q2之间,qn-1_在原有的qn-1和qn之间
% 这两个额外点对应的时间t1_和tn-1_需要计算,可以任意选择,本程序选择取平均值
% Input:
% q:给定点的位置
% t:给定点位置对应的时间
% v0:初始速度
% vn:终止速度
% a0:初始加速度
% an:终止加速度
% tt:插补周期
% Output:
% yy dyy ddyy:样条曲线函数值、速度、加速度值
function [yy dyy ddyy q1 qn_1] = cubicSpline_4(q, t, v0, vn, a0, an, tt)
if length(q) ~= length(t)
error('输入的数据应成对');
end
n = length(q); % 原来的点数量
% 增加两个额外点q1_和qn-1_,计算两点对应的时间点
t1_ = (t(1)+t(2)) / 2;
tn_1_ = (t(n-1)+t(n)) / 2;
% 更新时间向量
t = [t(1), t1_, t(2: n-1), tn_1_, t(n)];
% 更新点的数量
n = n + 2;
% 矩阵A是个(n-2)阶对角占优矩阵
A = zeros(n-2);
c = zeros(n-2, 1);
for i = 1: n-2
Tk_1 = t(i+2) - t(i+1);
Tk = t(i+1) - t(i);
if i == 1
A(i, i) = 2*Tk_1 + Tk*(3+Tk/Tk_1);
A(i, i+1) = Tk_1;
c(i, 1) = 6*((q(2)-q(1))/Tk_1-v0*(1+Tk/Tk_1)-a0*(0.5+Tk/(3*Tk_1))*Tk);
elseif i == 2
T0 = t(2)-t(1);
A(i, i-1) = Tk - (T0^2)/Tk;
A(i, i) = 2*(Tk + Tk_1);
A(i, i+1) = Tk_1;
c(i, 1) = 6*((q(3)-q(2))/Tk_1-(q(2)-q(1))/Tk+v0*(T0/Tk)+a0*(T0^2)/(3*Tk));
elseif i == n-2-1
Tn_1 = t(n) - t(n-1);
A(i, i-1) = Tk;
A(i, i) = 2*(Tk + Tk_1);
A(i, i+1) = Tk_1 - (Tn_1^2)/Tk_1;
c(i, 1) = 6*((q(n-2)-q(n-3))/Tk_1-(q(n-3)-q(n-4))/Tk-vn*(Tn_1/Tk_1)+an*(Tn_1^2)/(3*Tk_1));
elseif i == n-2
A(i, i) = 2*Tk + Tk_1*(3+Tk_1/Tk);
A(i, i-1) = Tk;
c(i, 1) = 6*((q(n-3)-q(n-2))/Tk+vn*(1+Tk_1/Tk)-an*(0.5+Tk_1/(3*Tk))*Tk_1);
else
A(i, i-1) = Tk;
A(i, i) = 2*(Tk + Tk_1);
A(i, i+1) = Tk_1;
c(i, 1) = 6*((q(i+1)-q(i))/Tk_1-(q(i)-q(i-1))/Tk);
end
end
% 经过上述步骤得到对角占优矩阵A和c
wk = A \ c; % 这一步matlab计算很慢,应换成追赶法求vk
% for i = 1: n-2
% a(i) = A(i, i); % 对角线
% if i == n-3
% b(i) = A(i, i+1); % 上边
% d(i) = A(i+1, i); % 下边
% continue;
% elseif i < n-2
% b(i) = A(i, i+1); % 上边
% d(i) = A(i+1, i); % 下边
% end
% end
% [~, ~, wk_] = crout(a, b, d, c); % 追赶法
n_ = length(wk);
q1 = q(1) + T0*v0 + ((T0^2)/3)*a0 + ((T0^2)/6)*wk(1);
Tn_1 = t(n) - t(n-1);
qn_1 = q(n-2) - Tn_1*vn + ((Tn_1^2)/3)*an + ((Tn_1^2)/6)*wk(n_);
% 更新位置q向量
q = [q(1), q1, q(2: n-3), qn_1, q(n-2)];
% 更新加速度w向量
w = [a0, wk', an];
% 规划样条轨迹
T = t(n) - t(1); % 运行总时长
nn = T / tt; % 总点数
yy = zeros(1, nn);
dyy = zeros(1, nn);
ddyy = zeros(1, nn);
j = 1;
for i = 1: n-1
Tk = t(i+1) - t(i);
a0 = q(i);
a1 = (q(i+1)-q(i))/Tk-(Tk/6)*(w(i+1)+2*w(i));
a2 = w(i) / 2;
a3 = (w(i+1)-w(i))/(6*Tk);
for tk = t(i): tt: t(i+1)
if i > 1 && tk == t(i)
continue
end
yy(j) = a0 + a1*(tk-t(i)) + a2*power(tk-t(i), 2) + a3*power(tk-t(i), 3);
dyy(j) = a1 + 2*a2*(tk-t(i)) + 3*a3*power(tk-t(i), 2);
ddyy(j) = 2*a2 + 6*a3*(tk-t(i));
j = j + 1;
end
end
end
测试:
%% 自写cubicSpline_4函数测试
q = [3, -2, -5, 0, 6, 12, 8];
t = [0, 5, 7, 8, 10, 15, 18];
v0 = 2; vn = -3; a0 = 0; an = 0;
tt = 0.1;
n = length(t);
[yy dyy ddyy q1 qn_1] = cubicSpline_4(q, t, v0, vn, a0, an, tt);
% 增加两个额外点q1_和qn-1_,计算两点对应的时间点
t1_ = (t(1)+t(2)) / 2;
tn_1_ = (t(n-1)+t(n)) / 2;
% 更新时间向量
t = [t(1), t1_, t(2: n-1), tn_1_, t(n)];
n = length(t);% 更新n
% 更新q
q = [q(1), q1, q(2: n-3), qn_1, q(n-2)];
subplot(3, 1, 1)
plot(t, q, 'o');
ylabel('位置')
grid on
hold on
plot([t(1):tt:t(n)], yy);
hold on
plot(t(2), q(2), 'r*');
plot(t(n-1), q(n-1), 'r*');
subplot(3, 1, 2)
% plot([t(1), t(n)], [v0, vn], 'o');
grid on
hold on
plot([t(1):tt:t(n)], dyy);
ylabel('速度')
subplot(3, 1, 3)
grid on
hold on
plot([t(1):tt:t(n)], ddyy);
ylabel('加速度')
三次样条部分还有平滑三次样条,后面再写,今天就到这里~
参考文献(实际上我就是翻译了一下下。。。)
Trajectory Planning for Automatic Machines and Robots