基于带约束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型曲线中位移在全局时间坐标下的函数, L L L是机器人执行器末端经过的距离。

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

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

位置插补

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

各个插值点位置坐标向量为:
{ x = x 1 + λ Δ x y = y 1 + λ Δ y z = z 1 + λ Δ 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 = x 2 − x 1 Δ y = y 2 − y 1 Δ z = z 2 − z 1 \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=x2x1Δy=y2y1Δz=z2z1

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

预备知识:

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

已知起点S、终点D的齐次变换矩阵(位姿)分别为 T S T_S TS T D T_D TD,则姿态插补步骤如下:

  1. 从齐次变换矩阵中提取出各自的旋转矩阵 R S R_S RS R D R_D RD,并将其转换为四元数 Q s Q_s Qs Q d Q_d Qd;

  2. 姿态四元数插补公式为:
    Q k ( t ) = Slerp ⁡ ( Q s , Q d , t ) = sin ⁡ [ ( 1 − λ ( t ) ) θ ] sin ⁡ θ Q s + sin ⁡ [ λ ( t ) θ ] sin ⁡ θ Q d Q_{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
    式中, θ = a r c c o s ( Q s ⋅ Q d ) \theta=arccos(Q_{s} \cdot Q_{d}) θ=arccos(QsQd)

  3. 将四元数 Q k ( t ) Q_{k}(t) Qk(t)转换为旋转矩阵 R k R_{k} Rk,将 R k R_{k} Rk和位置插补得到的 P k P_{k} Pk组合得到各插值点对应的齐次变换矩阵 T k T_{k} Tk

  4. 带入逆运动学计算,得到各关节角度变量,进行插补运动。

规划插补流程

基于带约束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: T
   q(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;
end
 
for 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);
    else
        sin_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);
    end
    qk(i) = Qs * k0 + Qd * k1;
end

end
% 空间单一圆弧位置插补与单位四元数姿态插补 + 梯形加减速归一化处理/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;
else
    angle_SOM = atan2(y2_, x2_);
end
if (atan2(y3_, x3_) < 0)
    angle_SOD = atan2(y3_, x3_) + 2*pi;
else
    angle_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;
end

for 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);
    else
        sin_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);
    end
    qk(i) = Qs_ * k0 + Qd_ * k1;
end

end

不足之处

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

参考文献

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

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

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

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

打赏
文章很值,打赏犒劳作者一下
相关推荐
©️2020 CSDN 皮肤主题: 技术黑板 设计师:CSDN官方博客 返回首页

打赏

xuuyann

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者