基于带约束S型加减速曲线的空间直线插补与空间圆弧插补算法（Matlab）

写在前面

学习代码都记录在个人github上，欢迎关注~

Matlab机器人工具箱版本9.10

在前面的博文中：

基于抛物线过渡（梯形加减速）的空间直线插补算法与空间圆弧插补算法（Matlab）

基于单位四元数的姿态插补（Matlab）

我使用了抛物线过渡（也就是梯形加减速）作为空间插补算法的速度规划方法，但是梯形曲线的缺点也很明显，即加速度不连续，速度过渡不平滑，容易造成机械系统冲击。下面我尝试了S型加减速曲线的规划方法并结合到空间插补算法中，仿真效果还可以，加速度曲线连续，更柔和，适用于精度速度要求更高的场合。

空间直线插补：

空间圆弧插补：

直观插补：

带约束S型速度规划

预备知识：

机器人学回炉重造（6）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划

参照基于抛物线过渡（梯形加减速）的空间直线插补算法与空间圆弧插补算法（Matlab）中归一化参数思想，将归一化参数计算函数中的梯形加减速方法换成S型加减速法。此时，定义归一化时间算子：
λ(t)=s(t)−s(0)L\lambda( t ) = \frac { s ( t ) - s ( 0 ) } { L } λ(t)=Ls(t)−s(0)
其中，t∈[0,T]t \in [ 0 , T ]t∈[0,T],s(t)s(t)s(t)是S型曲线中位移在全局时间坐标下的函数，LLL是机器人执行器末端经过的距离。

空间直线插补与空间圆弧插补

位置插补与姿态插补需要分开讨论。

位置插补

假设机器人末端由P1点沿直线运动到P2点，(x1,y1,z1)\left(x_{1}, y_{1}, z_{1}\right)(x1,y1,z1)分别为起点P1的位置，(x2,y2,z2)\left(x_{2}, y_{2}, z_{2}\right)(x2,y2,z2)为终点P2的位置，(xi,yi,zi)\left(x_{i}, y_{i}, z_{i}\right)(xi,yi,zi)为中间插补点Pi的位置。

各个插值点位置坐标向量为：
{x=x1+λΔxy=y1+λΔyz=z1+λΔz\left\{\begin{array}{l}{x=x_{1}+\lambda \Delta x} \\ {y=y_{1}+\lambda \Delta y} \\ {z=z_{1}+\lambda \Delta z}\end{array}\right. ⎩⎨⎧x=x1+λΔxy=y1+λΔyz=z1+λΔz
其中，λ\lambdaλ是归一化时间因子，(Δx,Δy,Δz)(\Delta x, \Delta y, \Delta z)(Δx,Δy,Δz)表示首末位置间的位置增量，其解为：
{Δx=x2−x1Δy=y2−y1Δz=z2−z1\left\{\begin{array}{l}{\Delta x=x_{2}-x_{1}} \\ {\Delta y=y_{2}-y_{1}} \\ {\Delta z=z_{2}-z_{1}} \end{array}\right. ⎩⎨⎧Δx=x2−x1Δy=y2−y1Δz=z2−z1

基于单位四元数的姿态插补

预备知识：

基于单位四元数的姿态插补（Matlab）

已知起点S、终点D的齐次变换矩阵（位姿）分别为TST_STS和TDT_DTD，则姿态插补步骤如下：

从齐次变换矩阵中提取出各自的旋转矩阵RSR_SRS和RDR_DRD，并将其转换为四元数QsQ_sQs和QdQ_dQd;
姿态四元数插补公式为：
Qk(t)=Slerp⁡(Qs,Qd,t)=sin⁡[(1−λ(t))θ]sin⁡θQs+sin⁡[λ(t)θ]sin⁡θQdQ_{k}(t)=\operatorname{Slerp}\left(Q_{s}, Q_{d}, t\right)=\frac{\sin [(1-\lambda(t)) \theta]}{\sin \theta} Q_{s}+\frac{\sin [\lambda(t) \theta]}{\sin \theta} Q_{d} Qk(t)=Slerp(Qs,Qd,t)=sinθsin[(1−λ(t))θ]Qs+sinθsin[λ(t)θ]Qd
式中，θ=arccos(Qs⋅Qd)\theta=arccos(Q_{s} \cdot Q_{d})θ=arccos(Qs⋅Qd)；
将四元数Qk(t)Q_{k}(t)Qk(t)转换为旋转矩阵RkR_{k}Rk，将RkR_{k}Rk和位置插补得到的PkP_{k}Pk组合得到各插值点对应的齐次变换矩阵TkT_{k}Tk；
带入逆运动学计算，得到各关节角度变量，进行插补运动。

规划插补流程

基于带约束S型加减速曲线的插补算法流程图如下：

Matlab实现代码

和前面博文的联系非常紧密，部分函数并未展出。

% 归一化处理
% S型加减速曲线
% 输入参数：机械臂末端运动总位移（角度）pos
%          机械臂末端线速度（角速度）初始v0，终止v1，最大速度vmax
%          最大加减速度amax、最大加加速度jmax
%          插补周期t
% 返回值： 插值点数N
function [lambda, N] = Normalization_S(pos, v0, v1, vmax, amax, jmax, t)
% S曲线参数计算（S型速度规划，又称七段式轨迹）
% function para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
q0 = 0; q1 = pos;
para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax); % 获取S曲线参数
% 总的规划时间
T = para(1) + para(2) + para(3)
% 等时插补
N = ceil(T / t);
j = 1;
for i = 0 : t: Tq(j) = S_position(i, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
%    qd(j) = S_velocity(i, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));lambda(j) = q(j) / pos;j = j + 1;
end
end

% 空间单一直线位置插补与单元四元数姿态插补 + 梯形加减速归一化处理/S型加减速曲线归一化处理
% 参数：起点S位置， 终点D位置
%      起点S的单元四元数Qs，终点的单元四元数Qd
%      起始速度v0，终止速度v1，最大速度vmax，最大加速度amax，最大加加速度jmax
%      插补周期t
% 返回值：插值点位置、单元四元数、插值点数
function [x y z qk N] = SpaceLine_Q(S, D, Qs, Qd, v0, v1, vmax, amax, jmax, t)
x1 = S(1); y1 = S(2); z1 = S(3);
x2 = D(1); y2 = D(2); z2 = D(3);P = 1; % 插值参数，增加插值点数，避免过小
% 总位移S
s = sqrt(power(x2 - x1, 2) + power(y2 - y1, 2) + power(z2 - z1, 2))
% 插值点数N
% N = ceil(P*s / vs)
% 求归一化参数：梯形加减速曲线
% function lambda = Normalization(pos, vel, accl, N)
% lambda = Normalization(s, vs, a, N);
% 求归一化参数：S型加减速曲线
% function lambda = Normalization_S(pos, v0, v1, vmax, amax, jmax, N)
[lambda, N] = Normalization_S(s, v0, v1, vmax, amax, jmax, t);
delta_x = x2 - x1;
delta_y = y2 - y1;
delta_z = z2 - z1;
% 计算两个四元数之间的夹角
dot_q = Qs.s*Qd.s + Qs.v(1)*Qd.v(1) + Qs.v(2)*Qd.v(2) + Qs.v(3)*Qd.v(3);
if (dot_q < 0)dot_q = -dot_q;
endfor i = 1: N% 位置插补x(i) = x1 + delta_x*lambda(i);y(i) = y1 + delta_y*lambda(i);z(i) = z1 + delta_z*lambda(i);% 单位四元数球面线性姿态插补% 插值点四元数if (dot_q > 0.9995)k0 = 1-lambda(i);k1 = lambda(i);elsesin_t = sqrt(1 - power(dot_q, 2));omega = atan2(sin_t, dot_q);k0 = sin((1-lambda(i)*omega)) / sin(omega);k1 = sin(lambda(i)*omega) / sin(omega);endqk(i) = Qs * k0 + Qd * k1;
endend

% 空间单一圆弧位置插补与单位四元数姿态插补 + 梯形加减速归一化处理/S型加减速曲线归一化处理
% 参数： 起点S位置, 中间点M位置, 终点D位置
%       起点S和终点D的单位四元数Qs_,Qd_
%       起始速度v0，终止速度v1，最大速度vmax，最大加速度amax，最大加加速度jmax
%       插值周期t
% 返回值：插值点位置、单元四元数、插值点数
% 方便起见，角速度和角加速度均为角度制
function [x y z qk N] = SpaceCircle_Q(S, M, D, Qs_, Qd_, v0, v1, vmax, amax, jmax, t)
x1 = S(1); x2 = M(1); x3 = D(1);
y1 = S(2); y2 = M(2); y3 = D(2);
z1 = S(3); z2 = M(3); z3 = D(3);A1 = (y1 - y3)*(z2 - z3) - (y2 - y3)*(z1 - z3);
B1 = (x2 - x3)*(z1 - z3) - (x1 - x3)*(z2 - z3);
C1 = (x1 - x3)*(y2 - y3) - (x2 - x3)*(y1 - y3);
D1 = -(A1*x3 + B1*y3 + C1*z3);A2 = x2 - x1;
B2 = y2 - y1;
C2 = z2 - z1;
D2 = -((x2^2 - x1^2) + (y2^2 - y1^2) + (z2^2 - z1^2)) / 2;A3 = x3 - x2;
B3 = y3 - y2;
C3 = z3 - z2;
D3 = -((x3^2 - x2^2) + (y3^2 - y2^2) + (z3^2 - z2^2)) / 2;
A = [A1, B1, C1; A2, B2, C2; A3, B3, C3]
b = [-D1, -D2, -D3]'
% 圆心
C = Gauss_lie(3, A, b)
x0 = C(1); y0 = C(2); z0 = C(3);
plot3(x0, y0, z0, 'bo')
hold on
% 外接圆半径
r = sqrt(power(x1 - x0, 2) + power(y1 - y0, 2) + power(z1 - z0, 2));
% 新坐标系Z0的方向余弦
L = sqrt(A1^2 + B1^2 + C1^2);
ax = A1 / L; ay = B1 / L; az = C1 / L;
% 新坐标系X0的方向余弦
nx = (x1 - x0) / r;
ny = (y1 - y0) / r;
nz = (z1 - z0) / r;
% 新坐标系Y0的方向余弦
o = cross([ax, ay, az], [nx, ny, nz]);
ox = o(1);
oy = o(2);
oz = o(3);
% 相对于基座标系{O-XYZ}， 新坐标系{C-X0Y0Z0}的坐标变换矩阵
T = [nx ox ax x0;ny oy ay y0;nz oz az z0;0  0  0  1]
T_ni = T^-1
% 求在新坐标系{C-X0Y0Z0}下S、M和D的坐标
S_ = (T^-1)*[S'; 1]
M_ = (T^-1)*[M'; 1]
D_ = (T^-1)*[D'; 1]
x1_ = S_(1) , y1_ = S_(2), z1_ = S_(3)
x2_ = M_(1), y2_ = M_(2), z2_ = M_(3)
x3_ = D_(1), y3_ = D_(2), z3_ = D_(3)
% 判断圆弧是顺时针还是逆时针，并求解圆心角
if (atan2(y2_, x2_) < 0)angle_SOM = atan2(y2_, x2_) + 2*pi;
elseangle_SOM = atan2(y2_, x2_);
end
if (atan2(y3_, x3_) < 0)angle_SOD = atan2(y3_, x3_) + 2*pi;
elseangle_SOD = atan2(y3_, x3_);
end
% 逆时针
if (angle_SOM < angle_SOD)flag = 1;theta = angle_SOD % 圆心角
end
% 顺时针
if (angle_SOM >= angle_SOD)flag = -1;theta = 2*pi - angle_SOD % 圆心角
end
% 插补点数N
% P = 1; %插补参数，增加插值点数，避免过小
% ws = ws*pi / 180; % 角度换成弧度
% a = a*pi / 180;
% N = ceil(P*theta / ws);
% % 求归一化参数：梯形加减速曲线
% lambda = Normalization(theta, ws, a, N);
% 求归一化参数：S型加减速曲线
% function lambda = Normalization_S(pos, v0, v1, vmax, amax, jmax, N)
[lambda, N] = Normalization_S(theta, v0, v1, vmax, amax, jmax, t);% 插补原理: 在新平面上进行插补（简化）
% 在新坐标系下z1_,z2_,z3_均为0，即外接圆在新坐标系的XOY平面内
% 此时转化为平面圆弧插补问题
delta_ang = theta;
% 计算两个四元数之间的夹角
dot_q = Qs_.s*Qd_.s + Qs_.v(1)*Qd_.v(1) + Qs_.v(2)*Qd_.v(2) + Qs_.v(3)*Qd_.v(3);
if (dot_q < 0)dot_q = -dot_q;
endfor i = 1: N% 位置插补x_(i) = flag * r * cos(lambda(i)*delta_ang);y_(i) = flag * r * sin(lambda(i)*delta_ang);P = T*[x_(i); y_(i); 0; 1];x(i) = P(1);y(i) = P(2);z(i) = P(3);% 单位四元数球面线性姿态插补% 插值点四元数if (dot_q > 0.9995)k0 = 1-lambda(i);k1 = lambda(i);elsesin_t = sqrt(1 - power(dot_q, 2));omega = atan2(sin_t, dot_q);k0 = sin((1-lambda(i))*omega) / sin(omega);k1 = sin(lambda(i)*omega) / sin(omega);endqk(i) = Qs_ * k0 + Qd_ * k1;
endend

不足之处

圆弧的姿态插补只严格通过了起始姿态和终点姿态，没有经过中间姿态；
无法达到时间最优。

参考文献

[1]刘鹏飞,杨孟兴,宋科,段晓妮.‘S’型加减速曲线在机器人轨迹插补算法中的应用研究[J].制造业自动化,2012,34(20):4-8+11.

[2]李振娜,王涛,王斌锐,郭振武,陈迪剑.基于带约束S型速度曲线的机械手笛卡尔空间轨迹规划[J].智能系统学报,2019,14(04):655-661.

[3]王添. 基于单位四元数机器人多姿态轨迹平滑规划[D].天津工业大学,2018.

[4]季晨. 工业机器人姿态规划及轨迹优化研究[D].哈尔滨工业大学,2013.