步骤

（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');

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

侵权必删