机器人笛卡尔空间轨迹规划-直线差补和圆弧差补

上一文，我们讨论了三次多项式和五次多项式的差补算法，那么这边文章具体讨论一下笛卡尔空间轨迹规划的直线差补和圆弧差补。

步骤

（1）知道起始点和终止点的位姿，和速度信息。

（2）根据两点能确定一条直线，求出直线的方程

（3）按时间或者按距离给出差补间隔-步长（d = vt），并求出插补次数

（4）利用差补次数和步长，求出在每一次步数时的坐标

（5）利用机器人逆解求出此刻各个关节的角度

平面的圆弧插补

步骤：

（1）三点确定一个圆，圆弧

（2）利用几何关系求出圆的半径，圆心点为（0，0）

（3）利用三角函数求出theta1和theta2的圆弧弧度

（4）求出在规定的时间ts，所走的角位移量detatheta = ts*v/r

（5）求出总的插补步数，并取整数

（6）利用每一步的弧度变化值和初始值（theta1）来进行求解圆弧上的p点坐标

（7）得到了圆弧上的p点坐标，利用机器人逆解就可以求解出机器人各个关节的角度

解释：知道p1，p2，p3的坐标，

U1的向量等于p2-p1

3和p1的向量和p2和p1的向量叉乘得到向量w

并对向量u和w进行除以他们的模长得到单位向量，之后求出向量v的长度。

解释：

（1）以P1（A）点作为圆心，坐标为（0，0），点乘是投影的关系，所以可以得到p1，p2，p3的对于平面坐标系的坐标x与y

（2）根据位置的，圆弧的圆心坐标一定在bx/2上，即坐标是（bx/2，H），利用圆的标准公式，可以求出H的公式，半径是圆心到（0，0）的距离，因此可以知道了圆心的坐标。

解释：

知道了圆弧圆心在平面的xy坐标，也是圆心在u'v上的值，通过相乘在加上P1的坐标就等于圆心在基坐标系（空间）下的xyz坐标

解释：

计算圆弧角。先求出变换矩阵T，

其中x0，y0，z0是圆弧圆心的坐标。知道了theta13，根据步数已知，求出每一步长的弧度，并且利用theta13从0到theta13的间隔，间隔为theta13/step，计算出圆弧上的点坐标。

并且根据需求知道了角位移的变化量，之后知道了总插步数，并进行取整

利用角度值进行来求解圆弧上的点对于空间坐标系的xyz坐标

利用机器人逆解进行求解各个关节的角度值

matlab直线差补：

clear,clc,close all;
p1=[-300,200,200];
p2=[0,200,300];
p3=[300,200,400];
step=30;  
p_i = zeros(4,step+1);
dx=(p3(1)-p1(1))/step;
dy=(p3(2)-p1(2))/step;
dz=(p3(3)-p1(3))/step;
for t=0:1:step
    p_i(1,t+1)=p1(1)+dx*t;
    p_i(2,t+1)=p1(2)+dy*t;
    p_i(3,t+1)=p1(3)+dz*t;
end
pos(1,:) = p_i(1,:);pos(2,:) = p_i(2,:);pos(3,:) = p_i(3,:);
T=pos';
plot3(T(1:end,1),T(1:end,2),T(1:end,3),'.','color','k');
grid on;hold on;
plot3(p1(1),p1(2),p1(3),'o','color','m');
plot3(p2(1),p2(2),p2(3),'o','color','c');
plot3(p3(1),p3(2),p3(3),'o','color','r');

matalb圆弧差补：

clear,clc,close all;
p1=[320,250,200];p2=[-230,-320,350];p3=[89,-439,181];step=100;  %空间小半径大圆弧
center = 0; rad =0;
% 计算p1到p2的单位向量和p1到p3的单位向量
% 检查点是否相同
v1 = p2 - p1;
v2 = p3 - p1;
if find(norm(v1)==0) | find(norm(v2)==0) %#ok<OR2>  norm矩阵范数
    fprintf('输入点不能一样\n');rad = -1;return;
end
v1n = v1/norm(v1);
v2n = v2/norm(v2);

% 计算圆平面上的单位法向量
% 检查三点是否共线

% if find(sum(abs(nv),2)<1e-14)
%     fprintf('三点过于趋近直线\n');rad = -1;return;
% end

% 计算新坐标系UVW轴
u = v1n;
w = cross(v2,v1)/norm(cross(v2,v1));  %cross两个向量的叉乘
v = cross(w,u);

% 计算投影
bx = dot(v1,u);   %内积函数，dot(A,B)= A'*B点乘
cx = dot(v2,u);
cy = dot(v2,v);

% 计算圆心
h = ((cx - bx/2)^2 + cy^2 -(bx/2)^2)/(2*cy);
center = zeros(1,3);
center(1,:) = p1(1,:) + bx/2.*u(1,:) + h.*v(1,:);

% 半径
rad = sqrt((center(1,1)-p1(1,1)).^2+(center(1,2)-p1(1,2)).^2+(center(1,3)-p1(1,3)).^2);

if rad>0
    pc = center;
    A = (p2(2)-p1(2))*(p3(3)-p2(3))-(p2(3)-p1(3))*(p3(2)-p2(2));
    B = (p2(3)-p1(3))*(p3(1)-p2(1))-(p2(1)-p1(1))*(p3(3)-p2(3));
    C = (p2(1)-p1(1))*(p3(2)-p2(2))-(p2(2)-p1(2))*(p3(1)-p2(1));
    K = sqrt(A^2+B^2+C^2);
    a = [A B C]/K;
    r = sqrt((p1(1)-pc(1))^2+(p1(2)-pc(2))^2+(p1(3)-pc(3))^2);
    n = (p1 -pc)/r;
    o = cross(a,n);
    T = [n' o' a' pc'; 0 0 0 1];

% 求转换后的点
    q1 = inv(T)*[p1 1]';   %inv矩阵的逆
    q2 = inv(T)*[p2 1]';
    q3 = inv(T)*[p3 1]';

% 计算角度
    if q3(2)<0
        theta13 = atan2(q3(2),q3(1)) + 2*pi;
    else
        theta13 = atan2(q3(2),q3(1));
    end

    if q2(2)<0
        theta12 = atan2(q2(2),q2(1)) + 2*pi;
    else
        theta12 = atan2(q2(2),q2(1));
    end

    % 轨迹插补
    count =1;
     for  ChaZhi= 0:theta13/step: theta13
         p_i(:,count) = T*[r*cos(ChaZhi) r*sin(ChaZhi) 0 1]';
         count = count+1;
     end
%      plot2(p_i,'b');
end
 pos(1,:) = p_i(1,:);pos(2,:) = p_i(2,:);pos(3,:) = p_i(3,:);
T=pos';
TQ=transl(T);
plot3(T(1:end,1),T(1:end,2),T(1:end,3),'.','color','k');  % plot2(T,'b');
grid on;
hold on;
plot3(p1(1),p1(2),p1(3),'o','color','m');
plot3(p2(1),p2(2),p2(3),'o','color','c');
plot3(p3(1),p3(2),p3(3),'o','color','r');

当然，写入到机器人算法中，还需要对机器人的逆解进行选择，一般来说选择是的角度最小的那组解作为最优解，同时还需要注意所带入的点是不是奇异点等一些问题。

侵权必删