运动规划——空间圆弧和直线插补及姿态平滑

一、实现流程

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

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

是

否

输入空间三点

是否为直线

直线插补

计算半径和圆心

姿态插补

圆弧插补

二、轨迹插补

2.1、圆弧插补

2.1.1 三点圆弧

原理说明

设给定三点分别为$P_1(x_1,y_1,z_1),P_2(x_2,y_2,z_2),P_3(x_3,y_3,z_3)$，圆心坐标为$P_0(x_0,y_0,z_0)$，圆半径为$R$。

• 算法一
  根据空间圆的标准方程可根据给定的三点获得前三个方程，同时计算所得圆心与已知的给定三点在同一平面，因此根据共面方程可得到第四个方程，计算时直接联立求解方程即可，具体如下：
  $$\{\begin{array}{c}(x_0-x_1{)}^2+(y_0-y_1{)}^2+(z_0-z_1{)}^2=R^2\\ (x_0-x_2{)}^2+(y_0-y_2{)}^2+(z_0-z_2{)}^2=R^2\\ (x_0-x_3{)}^2+(y_0-y_3{)}^2+(z_0-z_3{)}^2=R^2\\ \left[\begin{array}{cccc}{x}_0& y_0& z_0& 1\\ x_1& y_1& z_1& 1\\ x_2& y_2& z_2& 1\\ x_3& y_3& z_3& 1\end{array}\right]=0\end{array}$$
  注意：该算法存在缺陷，当$x_1=x_2$&&$z_1=z_2$时结果的分母中会出现0，此时无法得出结果。

• 算法二
  通过将空间点转换到二维平面，可将三维空间问题变为二维平面问题。

1. 构造新坐标系$P_1-UVW$，以$P_1$为坐标原点，以$P_1{P}_2$为$U$轴，以空间三点平面法向量为为$W$轴，再通过$U$和$W$轴叉积获得$V$轴。
   $$\begin{array}{c}{\mathbf{u}}_1=P_2-P_1\\ {\mathbf{w}}_1=(P_3-P_1)\times {\mathbf{u}}_1\\ \mathbf{u}={\mathbf{u}}_1/|{\mathbf{u}}_1|\\ \mathbf{w}={\mathbf{w}}_1/|{\mathbf{w}}_1|\\ \mathbf{v}=\mathbf{w}\times \mathbf{u}\end{array}$$

2. 将空间三点转换到新坐标系$P_1-UVW$($P_1,P_2,P_3$在新坐标系上表示为$A,B,C$)，并计算圆心。
   在坐标系$P_1-UVW$中，点$A$为圆心，坐标为$(0,0)$，点$B$在$U$轴上，坐标为$(b_x,0)$，点$C$坐标为$(c_x,c_y)$。通过向量点积可以求得点$P_2$和$P_3$在$U,V$轴上得投影。
   $$\begin{array}{c}B=(b_x,0)=((P_2-P_1)\cdot \mathbf{u},0)\\ C=(c_x,c_y)=((P_3-P_1)\cdot \mathbf{u},(P_3-P_1)\cdot \mathbf{v})\end{array}$$
   根据$A,B,C$三点位置可知，圆心必定落在$x=\frac{b_x}{2}$的直线上，因此设圆心坐标为$(\frac{b_x}{2},h)$，根据平面圆的标准方程可得：
   $$(c_x-b_x/2{)}^2+(c_y-h{)}^2=(b_x/2{)}^2+h^2$$
   上式中只有$h$一个未知数，因此可解得：
   $$h=\frac{(c_x-b_x/2{)}^2+c_y^2-(b_x/2{)}^2}{2c_y}$$

3. 将圆心转换到空间坐标系$O-XYZ$中，上面算得圆心为在$U,V$上得值，通过与$U,V$相乘并加上点$P_1$的偏置可求得圆心在空间坐标系中的坐标，半径通过其中一个点求取与圆心距离即可。
   $$\begin{array}{c}{P}_c=P_1+(b_x/2)\mathbf{u}+h\mathbf{v}\\ R=\sqrt{(x_1-x_c{)}^2+(y_1-y_c{)}^2+(z_1-z_c{)}^2}\end{array}$$

程序实现

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
    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 插补

原理说明

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

• 坐标变换
  空间圆弧本质上是三个空间点形成平面(平面M)上的圆弧,我们只要求出坐标系在坐标系$O-XYZ$下的变换矩阵$T$ ,就可经过坐标变换将三维空间($O-XYZ$)的圆弧变换到二维平面($P_0-UVW$)的圆弧。
  由平面方程可算得平面M的法矢量的方向数为
  $$\begin{array}{c}A=(y_2-y_1)(z_3-z_2)-(z_2-z_1)(y_3-y_2)\\ B=(z_2-z_1)(x_3-x_2)-(x_2-x_1)(z_3-z_2)\\ C=(x_2-x_1)(y_3-y_2)-(y_2-x_1)(x_3-x_2)\end{array}$$

以平面M的法矢量方向作为新坐标系$P_0-UVW$的W轴方向，W轴的方向余弦为：
$$\begin{array}{c}k=\sqrt{A^2+B^2+C^2}\\ ax=\frac{l}{k}\\ ay=\frac{m}{k}\\ az=\frac{n}{k}\end{array}$$

以$\underset{P_0{P}_1}{\to }$方向作为新坐标系$P_0-UVW$的U轴方向，U轴的方向余弦为：
$$\begin{array}{c}r=\sqrt{(x_1-x_0{)}^2+(y_1-y_0{)}^2+(z_1-z_0{)}^2}\\ nx=\frac{x_1-x_0}{r}\\ ny=\frac{y_1-y_0}{r}\\ nz=\frac{z_1-z_0}{r}\end{array}$$

以U轴的方向余弦与W轴的方向余弦的叉乘作为新坐标系$P_0-UVW$的V轴的方向余弦，V轴的方向余弦为：
$$\begin{array}{c}ox=ay\times nz-az\times ny\\ oy=az\times nx-ax\times nz\\ oz=ax\times ny-ay\times nx\end{array}$$

从而可求出新坐标系$P_0-UVW$在坐标系$O-XYZ$下的变换矩阵
$$\begin{array}{c}T=\left[\begin{array}{cc}R& P_0\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}n& o& a& p\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}nx& ox& ax& x_0\\ ny& oy& ay& y_0\\ nz& oz& az& z_0\\ 0& 0& 0& 1\end{array}\right]\\ A^{-1}=\left[\begin{array}{cc}{R}^T& -R^T{P}_0\\ 0& 1\end{array}\right]\end{array}$$
进而可求出$P_1,P_2,P_3$点在新坐标系下对应的坐标值，其中$i=1,2,3$
$$\left[\begin{array}{c}Q\\ 1\end{array}\right]=T^{-1}\left[\begin{array}{c}{p}_i\\ 1\end{array}\right]$$

• 插补角

  • 方向
    在实际机械手的空间圆弧的任务操作中，圆弧一般具有确定的插补方向，此处约定在$P_0-UVW$坐标系中$UV$平面内逆时针方向为其插补方向，即由$P_3$到$P_2$再到$P_1$的圆弧始终为逆时针圆弧．

  • 大小
    $${\theta }_{12}=\{\begin{array}{cc}& arctan2(y_2,x_2)+2\pi ,y_2<0\\ & arctan2(y_2,x_2),y_2\ge 0\end{array}$$

    $${\theta }_{13}=\{\begin{array}{cc}& arctan2(y_3,x_3)+2\pi ,y_3<0\\ & arctan2(y_3,x_3),y_3\ge 0\end{array}$$

程序实现

% 建立圆弧坐标系
% 计算转换矩阵
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)，即沿着一条直线（也就是圆上的一个弦）进行插值，此种插值方式所得结果并非单位四元数(只有单位四元数才能表示旋转)。
$$q_t=Lerp(q_0,q_1,t)=(1-t)q_0+t{q}_1$$

3.1.2 正规化线性插值

正规化线性插值（Normalized LinearInterpolation)，是对线性插值的改进，即将线性插值除以其模⻓，将其转化为一个单位四元数。这种插补算法适用于插补角度接近0度的情况。
$$q_t=Lerp(q_0,q_1,t)=\frac{(1-t)q_0+t{q}_1}{\Vert (1-t)q_0+t{q}_1\Vert }$$

在单位时间内，$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度的情况。

数学解析
插值的一般公式可以写为：$r=a(t)p+b(t)qr=a(t)p+b(t)q$，现在要找到合适的$a(t)$和$b(t)$。注意单位向量$p$和$q$之间的夹角为$\theta$，$p$和$r$之间的夹角为$t\theta$，$q$和$r$之间的夹角为$(1-t)\theta$。

将上面的公式两边点乘$p$可得
$$\begin{array}{c}p\cdot r=a(t)p\cdot p+b(t)p\cdot q\\ cost\theta =a(t)+b(t)cos\theta \end{array}$$
同样地，对公式两边点乘$q$可得
$$cos[(1-t)\theta ]=a(t)cos\theta +b(t)$$
结合上面两个方程可求得$a(t)$和$b(t)$
$$\begin{array}{c}a(t)=\frac{cost\theta -cos[(1-t)\theta ]cos\theta }{1-co{s}^2\theta }\\ b(t)=\frac{cos[(1-t)\theta ]-cost\theta cos\theta }{1-co{s}^2\theta }\end{array}$$
使用三角函数公式可以将其化简为
$$\begin{array}{c}a(t)=\frac{sin[(1-t)\theta ]}{sin\theta }\\ b(t)=\frac{sint\theta }{sin\theta }\end{array}$$
因此四元数的球面线性插值公式为
$$Slerp(p,q,t)=\frac{\sin [(1-t)\theta ]p+\sin t\theta q}{\sin \theta }$$
注意：这个公式有2个问题，必须在实现过程中加以考虑

1. 如果四元数点积的结果是负值（夹角大于90°），那么后面的插值就会在4D球面上绕远路。为了解决这个问题，先测试点积的结果，当结果是负值时，将2个四元数的其中一个取反（并不会改变它代表的朝向）。而经过这一步操作，可以保证这个旋转走的是最短路径。
   
2. 当$p$和$q$的夹角$\theta$差非常小时会导致$sin\theta \to 0$，这时除法可能会出现问题。为了避免这样的问题，当$\theta$非常小时可以使用简单的线性插值代替（当$\theta \to 0$时，$sin\theta \approx \theta$，因此方程退化为线性方程：$slerp(p,q,t)=(1-t)p+tq$）

程序实现

% 计算插补点数
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. 向量乘法

   • 乘积：用于矩阵相乘，表示为$C=A\ast B$，A的列数与B的行数必须相同，C也是矩阵，C的行数等于A的行数，C的列数等于B的列数。$C_{i}j$为A的第i行与B的第j列的点积。

   • 点积：用于向量相乘，表示为$C=A.\ast B$，A与B均为向量，C为标量，也称标量积、内积、数量积等，Ａ在B上的投影
     两个向量$a=[a_1,a_2,...,a_n]$和$b=[b_1,b_2,...,b_n]$得点积为：
     $$a\cdot b=\sum _n^{i=1}{a}_i{b}_i=a_1{b}_1+a_2{b}_2+\cdot \cdot \cdot +a_n{b}_n$$

   • 叉积：用于向量相乘，表示为$C=A\times B$，A与B均为向量，C与A、B均正交，C也为向量，也称向量积。

3. 方向余弦

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

假设$v$是空间向量：
$$v=v_1\cdot \hat{x}+v_2\cdot \hat{y}+v_3\cdot \hat{z}$$

其中，$\hat{x},\hat{y},\hat{z}$是一组标准正交基的单位基底向量，$v_1,v_2,v_3$分别为对于x轴、y轴、z轴的分量。那么，$v$对于x轴、y轴、z轴的方向余弦$\alpha ,\beta ,\gamma$分别为
$$\begin{gathered} \alpha =cosa=\frac{v\cdot \hat{x}}{\Vert v\Vert }=\frac{v_1}{\sqrt{v_1^2+v_2^2+v_3^2}} \\ \beta =cosb=\frac{v\cdot \hat{y}}{\Vert v\Vert }=\frac{v_2}{\sqrt{v_1^2+v_2^2+v_3^2}} \\ \gamma =cosc=\frac{v\cdot \hat{z}}{\Vert v\Vert }=\frac{v_3}{\sqrt{v_1^2+v_2^2+v_3^2}} \end{gathered}$$

其中，$a,b,c$分别为v对于x-轴、y-轴、z-轴的角度。
注意方向余弦满足以下恒等式：
$${\alpha }^2+{\beta }^2+{\gamma }^2=1$$

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.