圆弧插值算法详解与代码实现（附完整代码）

1、圆弧插值算法理论

弧形运动是机械臂呈直线运动轨迹的一种补充轨迹方式，在空间坐标系中，直角插补算法采用的是两点之间取任意个插值点构成一条直线， 圆弧插补算法则是通过在空间中选择三个不共面点，确定一个空间圆弧，之后转化为平面圆弧问题进行求解。

如图4-3所示，OoXoYqZo坐标系中存在 A(x,Jy,2小B(x2, 2.z2), C(2,Yg, z)构成的空间圆弧，A、B、C三点以A点建立新的坐标系0,XYZ.其中Z轴坐标都为0,此时空间圆弧就转换到了平面圆弧进行插值规划轨迹。

空间圆弧转化为平面圆弧

为确定角度利于插补，设定圆弧的圆心 在 坐标系中的原点位置，如图所示，分别构造辅助线 ，同时满足可  

平面圆弧插补图

依据渊足条件求出圆心坐标，圆弧半径为  ,求出A、C两点间距离  根据R、d得到圖心角a = 2arcsin[d/2R].确定出相关参数后，对圆心角a进行N次插补，角位移插补增量为  各插补点计算方式如下所示:

根据第三章机械臂相对坐标系变换知识，在坐标系 进行插值后需要将所有插值点转换原坐标系 中，变化矩阵F如下所示。设某插补点在坐标系 中坐标为[Rxo, Ryo, R2o]，通过位姿变换得到坐标系 下的坐标[Rx1, Ry1, R23]

矩阵F是由圆弧上各点在坐标系 的坐标和矢量X1,Y1,z1的方向余弦向量构成，如下所示:

将X,Y Z分别依次化为单位向量{nx, ny, nZ},{Ox 0y, 0Z},{ax, ay, az}带入到位姿矩阵N中，求出机械臂位姿矩阵各参数后通过求逆得到各插补点的关节变量，机械臂进行动作执行呈圆弧形轨迹运动。

设定起始点为(4,3,3) 终点为(3,1,-1) ,中间点为(2,2,0)， 插补次数N=21,圆弧运动轨迹共插入20个中间点。得到机械臂轨迹仿真图如下所示:

2、代码

clear;clc,close all;

%% 给定三个末端的空间位置 ，三组参考值

m=0.001;

p1=[-100,150,180]*m;p2=[-150,0,150]*m;p3=[0,-139,131]*m;StepSize=30;    %定义空间三个点，用于圆弧插值

%% 圆弧插补

%输入参数 p1, p2, p3为1x3的矩阵，StepSize为插补点的个数，

% 输出参数Q为(step+1)x6的矩阵，ChaBuType为插补的类型

% ChaBuType>0:   圆弧

% ChaBuType = -1:输入数据有问题

% ChaBuType = -2:三点共线

[T,ChaBuType] = ArcLinearInterpolation(p1, p2, p3,StepSize);    %圆弧插值，pos为3x(step+1)的矩阵为插值点

%% 

plot3(T(1:end,1),T(1:end,2),T(1:end,3),'b*');

% axis(W);   %设置坐标轴范围

hold on;grid on;

% axis([-0.3 0.1 -0.3 0.34 0.2 0.35])

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

xlabel('x'),ylabel('y');zlabel('z');

[下载咨询链接：matlab正逆运动学分析与轨迹规划\]](https://link.zhihu.com/?target=https%3A//item.taobao.com/item.htm%3Fspm%3Da1z10.1-c-s.w4004-22529865413.4.75e351a02MUWu0%26id%3D640920926591)

完整代码见付费内容

或者联系扣扣2386317960