圆弧插值算法（有代码）

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正逆运动学分析与轨迹规划]
完整代码见下载链接

或者联系扣扣2386317960