一、实现流程

插补（Interpolation），即机床数控系统依照一定方法确定刀具运动轨迹的过程。也可以说，已知曲线上的某些数据，按照某种算法计算已知点之间的中间点的方法，也称为“数据点的密化”；数控装置根据输入的零件程序的信息，将程序段所描述的曲线的起点、终点之间的空间进行数据密化，从而形成要求的轮廓轨迹，这种“数据密化”机能就称为“插补”。

通常意义上的插补为二维平面上的轨迹插补，但机械臂末端运动空间为三维，因此此处插补指三维空间下的插补。

程序实现

function [center,rad] = CircleCenter(p1, p2, p3)
% 根据三个空间点，计算出其圆心及半径
% rad>0:   圆弧
% rad = -1:输入数据有问题
% rad = -2:三点共线

center = 0;rad =0;
% 数据检查
% 检查数据输入格式是否正确
if size(p1,2)~=3 || size(p2,2)~=3 || size(p3,2)~=3
    fprintf('输入点维度不一致\n');rad = -1;return;
end
n = size(p1,1);
if size(p2,1)~=n || size(p3,1)~=n
    fprintf('输入点维度不一致\n');rad = -1;return;
end

% 计算p1到p2的单位向量和p1到p3的单位向量
% 检查点是否相同
v1 = p2 - p1;
v2 = p3 - p1;
if find(norm(v1)==0) | find(norm(v2)==0) %#ok<OR2>
    fprintf('输入点不能一样\n');rad = -1;return;
end
v1n = v1/norm(v1);
v2n = v2/norm(v2);

% 计算圆平面上的单位法向量
% 检查三点是否共线
nv = cross(v1n,v2n);
 if all(nv==0)
    fprintf('三个点共线\n');rad = -2;return;
 end
if find(sum(abs(nv),2)<1e-5)
    fprintf('三点过于趋近直线\n');rad = -1;return;
end

% 计算新坐标系UVW轴
u = v1n;
w = cross(v2,v1)/norm(cross(v2,v1));
v = cross(w,u);

% 计算投影
bx = dot(v1,u);
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);
end

2.1.2 插补

原理说明

圆弧插补的实现流程为，将空间点转换到三个点形成的平面，将三维问题转换为二维。然后计算圆弧角，并在该平面上进行插补。最后通过变换矩阵，将插补点从二维坐标转换为三维坐标。

• 坐标变换
• 
• 
• 
• 
• 程序实现

• % 建立圆弧坐标系
  % 计算转换矩阵
  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;
  n = (p1 -pc)/r;
  o = cross(a,n);
  T = [n' o' a' pc'; 0 0 0 1];
  
  % 求转换后的点
  q1 = inv(T)*[p1 1]';
  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 step = 0:theta13/sumStep: theta13
      p_i(:,count) = T*[r*cos(step) r*sin(step) 0 1]';
      count = count+1;
  end
  

• 2.2、直线插补

  直线插补采用简单线性插补即可，根据插补次数分别计算各轴步矩然后累加。

  程序实现

• % 计算插补点数
  stepNum = round(sqrt((p3(1)-p1(1))^2+(p3(2)-p1(2))^2+(p3(3)-p1(3))^2)/step);
  p_i = zeros(4,stepNum+1);
      
  fprintf("line\n");
  dx=(p3(1)-p1(1))/stepNum;
  dy=(p3(2)-p1(2))/stepNum;
  dz=(p3(3)-p1(3))/stepNum;
  
  for t=0:1:stepNum
  	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
  

• 三、姿态插补

• 
  此处插补对象为两个姿态，多姿态插补参考运动规划——多姿态插值

  3.1、线性插值
  3.1.1 普通线性插值
  线性插值(Lerp/Linear Interpolation)，即沿着一条直线（也就是圆上的一个弦）进行插值，此种插值方式所得结果并非单位四元数(只有单位四元数才能表示旋转)。
  

• 3.1.2 正规化线性插值
  正规化线性插值（Normalized LinearInterpolation)，是对线性插值的改进，即将线性插值除以其模⻓，将其转化为一个单位四元数。这种插补算法适用于插补角度接近0度的情况。
  

• 在单位时间内， V t 扫过的⻆度是不同的， V t 扫过的速度（⻆速度）首先会不断地增加，当 t=0.50后会开始减速，所以Nlerp插值不能保证均匀的⻆速度。

  程序实现
  
  

• % 计算插补点数
  stepNum = round(r*theta13/step);
  p_i = zeros(4,stepNum+1);
  
  %判断路径,取路径短的，不影响方向
  cosa = p1_Q(1)*p3_Q(1)+p1_Q(2)*p3_Q(2)+p1_Q(3)*p3_Q(3)+p1_Q(4)*p3_Q(4);
  if cosa < 0
      p3_Q = -p3_Q;
  end
  
  for step = 0:1: stepNum
      k0 = 1-step/stepNum;
      k1 = step/stepNum;
      pt_Q(:,step+1) = (p1_Q*k0 + p3_Q*k1)/norm(p1_Q*k0 + p3_Q*k1);
  end
  

• 3.2、球面线性插值
  球面线性插值(SphericalLinearInterpolation)对每一对四元数使用Slerp插值虽然能够保证 每两个四元数之间的⻆速度是固定的，但是⻆速度会在切换插值的四元数时出现断点，或者说在切换点不可导．

  Slerp球面线性插值是对角度本身进行线性插值，适用于插补角度不接近0度的情况。

  数学解析
  

• 
• 

• 注意：这个公式有2个问题，必须在实现过程中加以考虑

  如果四元数点积的结果是负值（夹角大于90°），那么后面的插值就会在4D球面上绕远路。为了解决这个问题，先测试点积的结果，当结果是负值时，将2个四元数的其中一个取反（并不会改变它代表的朝向）。而经过这一步操作，可以保证这个旋转走的是最短路径。
  

• 
• 程序实现

• % 计算插补点数
  stepNum = round(r*theta13/step);
  p_i = zeros(4,stepNum+1);
          
  %判断路径,取路径短的，不影响方向
  cosa = p1_Q(1)*p3_Q(1)+p1_Q(2)*p3_Q(2)+p1_Q(3)*p3_Q(3)+p1_Q(4)*p3_Q(4);
  if cosa < 0
      p3_Q = -p3_Q;
  end
  
  sina = sqrt(1 - cosa*cosa);
  angle = atan2( sina, cosa );
  for step = 0:1: stepNum
      k0 = sin((1-step/stepNum)*angle)/sina;
      k1 = sin(step/stepNum*angle)/sina;
      pt_Q(:,step+1) = (p1_Q*k0 +p3_Q*k1)/norm(p1_Q*k0 + p3_Q*k1);
  end
  

• 四、实现效果直线插补
• 
• 圆弧插补
• 

• 五、补充

  5.1 基础知识

  1. 法向量
     法向量是空间解析几何的一个概念，垂直于平面的直线所表示的向量为该平面的法向量。由于空间内有无数个直线垂直于已知平面，因此一个平面都存在无数个法向量（包括两个单位法向量）。

  2. 向量乘法

  3. 

  4. 方向余弦

     方向余弦是指在解析几何里，一个向量的三个方向余弦分别是这向量与三个坐标轴之间的角度的余弦。两个向量之间的方向余弦指的是这两个向量之间的角度的余弦。

     假设 v 是空间向量：
     

  5. 5.2 参考
     网上资料
     三点圆弧原理
     三点圆弧原理-知乎
     三点圆弧函数
     姿态插补一
     姿态插值二
     轨迹插补

     文献

     Zhaodong L . 机械手空间圆弧位姿轨迹规划算法的实现[J]. Journal of Harbin Institute of Technology (New Series), 2012, 44:27-31.
     吴镇炜, 谈大龙, 吴镇炜, et al. 机械手空间圆弧运动的一种有效轨迹规划方法[J]. 机器人, 1999, 21(1):8-11.
     卓扬娃, 白晓灿, 陈永明. 机器人的三种规则曲线插补算法[J]. 装备制造技术, 2009(11):27-29.