1、圓弧插值算法理論

弧形運動是機械臂呈直線運動軌跡的一種補充軌跡方式，在空間坐標系中，直角插補算法采用的是兩點之間取任意個插值點構(gòu)成一條直線， 圓弧插補算法則是通過在空間中選擇三個不共面點，確定一個空間圓弧，之后轉(zhuǎn)化為平面圓弧問題進行求解。

如圖4-3所示，OoXoYqZo坐標系中存在 A(x,Jy,2小B(x2, 2.z2), C(2,Yg, z)構(gòu)成的空間圓弧，A、B、C三點以A點建立新的坐標系0,XYZ.其中Z軸坐標都為0,此時空間圓弧就轉(zhuǎn)換到了平面圓弧進行插值規(guī)劃軌跡。

 

空間圓弧轉(zhuǎn)化為平面圓弧

為確定角度利于插補，設(shè)定圓弧的圓心 在 坐標系中的原點位置，如圖所示，分別構(gòu)造輔助線 ，同時滿足可  

 

平面圓弧插補圖

依據(jù)淵足條件求出圓心坐標，圓弧半徑為  ,求出A、C兩點間距離  根據(jù)R、d得到圖心角a = 2arcsin[d/2R].確定出相關(guān)參數(shù)后，對圓心角a進行N次插補，角位移插補增量為  各插補點計算方式如下所示:

 

根據(jù)第三章機械臂相對坐標系變換知識，在坐標系 進行插值后需要將所有插值點轉(zhuǎn)換原坐標系 中，變化矩陣F如下所示。設(shè)某插補點在坐標系 中坐標為[Rxo, Ryo, R2o]，通過位姿變換得到坐標系 下的坐標[Rx1, Ry1, R23]

 

  

矩陣F是由圓弧上各點在坐標系 的坐標和矢量X1,Y1,z1的方向余弦向量構(gòu)成，如下所示:

 

將X,Y Z分別依次化為單位向量{nx, ny, nZ},{Ox 0y, 0Z},{ax, ay, az}帶入到位姿矩陣N中，求出機械臂位姿矩陣各參數(shù)后通過求逆得到各插補點的關(guān)節(jié)變量，機械臂進行動作執(zhí)行呈圓弧形軌跡運動。

設(shè)定起始點為(4,3,3) 終點為(3,1,-1) ,中間點為(2,2,0)， 插補次數(shù)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;    %定義空間三個點，用于圓弧插值

%% 圓弧插補

%輸入?yún)?shù) p1, p2, p3為1x3的矩陣，StepSize為插補點的個數(shù)，

% 輸出參數(shù)Q為(step+1)x6的矩陣，ChaBuType為插補的類型

% ChaBuType>0:   圓弧

% ChaBuType = -1:輸入數(shù)據(jù)有問題

% 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);   %設(shè)置坐標軸范圍

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正逆運動學(xué)分析與軌跡規(guī)劃\]](https://link.zhihu.com/?target=https%3A//item.taobao.com/item.htm%3Fspm%3Da1z10.1-c-s.w4004-22529865413.4.75e351a02MUWu0%26id%3D640920926591)

完整代碼見付費內(nèi)容

或者聯(lián)系扣扣2386317960