省流說明：本文提供球鉸的非對稱布置情況下其正逆運(yùn)動學(xué)的求解，但需要有額外的輸入?yún)?shù)

3-SPR并聯(lián)機(jī)器人的運(yùn)動學(xué)模型

S：球鉸 P：移動副 R：轉(zhuǎn)動副
3-SPR并聯(lián)機(jī)器人即包含三條獨(dú)立的SPR運(yùn)動支鏈
輸入：三個移動副（P）的伸長量
輸出：系統(tǒng)末端參考點(diǎn)（一般是動平臺的中心點(diǎn)）

運(yùn)動學(xué)模型如下：
（這里的圖使用了四個移動副，即在并聯(lián)機(jī)構(gòu)上串聯(lián)了一個移動副，使其具有更大的運(yùn)動范圍）
對于3-SPR并聯(lián)機(jī)器人來說，其末端參考點(diǎn)為$O_P$（動平臺中心點(diǎn)）
對于串聯(lián)移動副后的機(jī)構(gòu)而言，其末端參考點(diǎn)為$O_T$（動平臺上串聯(lián)的移動副的末端）

逆運(yùn)動學(xué)求解

3-SPR并聯(lián)機(jī)器人的位置逆解可以描述為：當(dāng)已知系統(tǒng)末端參考點(diǎn)$O_P$在世界坐標(biāo)系{W}下的坐標(biāo)（即靜平臺所處的坐標(biāo)系），求3-SPR并聯(lián)機(jī)器人移動副$P_1$、$P_2$、$P_3$的伸長量。

求解公式:
公式的求解過程主要是利用約束條件，并聯(lián)機(jī)器人在運(yùn)動的過程中，其移動副$P$的方向向量總是與轉(zhuǎn)動副的旋轉(zhuǎn)軸垂直的（$P$的方向向量由球鉸$B$指向移動副中心點(diǎn)$A$），建立三個約束方程，聯(lián)立求解即可得到位置逆解（證明過程略，有人需要的話再放出）
由于球鉸是非對稱布置的，所以在求解時要將三個球鉸的位置輸入到公式中（$r_B$${}_1$、$r_B$${}_2$、$r_B$${}_3$）

方程注：變量說明見Matlab程序的注釋
Matlab程序：

%***************************************************************    
% ** Name:   非對稱布置3-SPR并聯(lián)機(jī)構(gòu)的逆運(yùn)動學(xué)求解                  **
% ** Author: __Lnx                                             **
% ** Date:   2023-03-26                                        **
% ** Input:  r1、r2、r3：球鉸B1、B2、B3距離原點(diǎn)O_W的距離           **
%            X_a、Y_a、Z_a：代表末端參考點(diǎn)O_T要達(dá)到的位置           **
%            p4：移動副P4的伸長量，如是 3-SPR并聯(lián)機(jī)構(gòu)則將該值設(shè)定為0  **
%****************************************************************
function output = SPR_InverseKinematics(r1, r2, r3, X_a, Y_a, Z_a,p4)
    
    R_A = 78.603; % 動平臺旋轉(zhuǎn)副中心點(diǎn)外接圓半徑（A1、A2、A3距離O_P的距離）
    syms Delt Theta Phi % 已知六個自由度中的三個，將剩下三個自由度設(shè)為要求的值
    
	% 旋轉(zhuǎn)矩陣定義
    ux = cosd(Theta)*cosd(Delt);
    uy = cosd(Theta)*sind(Delt);
    uz = -sind(Theta);
    vx = sind(Phi)*sind(Theta)*cosd(Delt)-cosd(Phi)*sind(Delt);
    vy = sind(Phi)*sind(Theta)*sind(Delt)+cosd(Phi)*cosd(Delt);
    vz = sind(Phi)*cosd(Theta);
    wx = cosd(Phi)*sind(Theta)*cosd(Delt)+sind(Phi)*sind(Delt);
    wy = cosd(Phi)*sind(Theta)*sind(Delt)-sind(Phi)*cosd(Delt);
    wz = cosd(Phi)*cosd(Theta);
    R_l(1,1) = ux; R_l(1,2)=vx; R_l(1,3)=wx;
    R_l(2,1) = uy; R_l(2,2)=vy; R_l(2,3)=wy;
    R_l(3,1) = uz; R_l(3,2)=vz; R_l(3,3)=wz;

	% 由O_T的位置求出O_P的位置
    X_o = X_a-R_l(1,:)*[0 0 p4]';
    Y_o = Y_a-R_l(2,:)*[0 0 p4]';
    Z_o = Z_a-R_l(3,:)*[0 0 p4]';

	% 根據(jù)公式求解
    f1 = atand((3*(r2-r1)*(cosd(Theta)-cosd(Phi))+3*sqrt(3)*(r1+r2)*sind(Phi)*sind(Theta))/(4*sqrt(3)*r3*cosd(Theta)+3*(r2-r1)*sind(Phi)*sind(Theta)+sqrt(3)*(r1+r2)*(3*cosd(Phi)+cosd(Theta)))) == Delt;
    f2 = (6*(uz*vy-vz*uy)*Z_o-(3*(ux-vy)+sqrt(3)*(3*vx-uy))*uy*r2+2*(sqrt(3)*uy+3*vy)*uy*r3)/(6*(vx*uy-ux*vy)) == X_o;
    f3 = (6*(uz*vx-vz*ux)*Z_o-(3*(ux-vy)+sqrt(3)*(3*vx-uy))*ux*r2+2*(sqrt(3)*ux+3*vx)*uy*r3)/(6*(ux*vy-vx*uy)) == Y_o;
    
    [DELTA,THETA,PHI] = solve([f1, f2, f3],[Delt Theta Phi]);
    r = eval([DELTA,THETA,PHI]);

    T = r(2);D = r(1);P = r(3);
    rux = cosd(T)*cosd(D);
    ruy = cosd(T)*sind(D);
    ruz = -sind(T);
    rvx = sind(P)*sind(T)*cosd(D)-cosd(P)*sind(D);
    rvy = sind(P)*sind(T)*sind(D)+cosd(P)*cosd(D);
    rvz = sind(P)*cosd(T);
    rwx = cosd(P)*sind(T)*cosd(D)+sind(P)*sind(D);
    rwy = cosd(P)*sind(T)*sind(D)-sind(P)*cosd(D);
    rwz = cosd(P)*cosd(T);

    R(1,1) = rux; R(1,2)=rvx; R(1,3)=rwx;
    R(2,1) = ruy; R(2,2)=rvy; R(2,3)=rwy;
    R(3,1) = ruz; R(3,2)=rvz; R(3,3)=rwz;

    % 根據(jù)輸入，計(jì)算球鉸點(diǎn)坐標(biāo)
    B1 = [r1*cosd(30) r1*sind(30) 0]';
    B2 = [r2*cosd(150)  r2*sind(150)  0]';
    B3 = [r3*cosd(270) r3*sind(270) 0]';
  
  	%根據(jù)求得的旋轉(zhuǎn)矩陣，計(jì)算動平臺中心點(diǎn)O_P的坐標(biāo)
    O_A=[ X_a, Y_a, Z_a]';
    O_A = O_A-R*[0 0 p4]';
    
    % 根據(jù)求得的旋轉(zhuǎn)矩陣及O_P點(diǎn)的坐標(biāo)，計(jì)算各旋轉(zhuǎn)副坐標(biāo)
    A1 = R * [R_A*cosd(30) R_A*sind(30) 0]'+ O_A;
    A2 = R * [R_A*cosd(150)  R_A*sind(150)  0]'+ O_A;
    A3 = R * [R_A*cosd(270) R_A*sind(270) 0]'+ O_A;
    
    % 計(jì)算各桿伸長量
    P1 = norm(A1 - B1);
    P2 = norm(A2 - B2);
    P3 = norm(A3 - B3);
    
    % 輸出結(jié)果
    r_output = [P1 P2 P3];
    output = r_output;
end

正運(yùn)動學(xué)求解（待更新補(bǔ)充說明）

正運(yùn)動學(xué)的求解和以往的方法不一樣，利用了點(diǎn)云配準(zhǔn)的思想。
主要思路是：

Matlab正運(yùn)動學(xué)計(jì)算函數(shù)：SPR_ForwardKinematics

%**********************************************************    
% ** Name:   非對稱布置3-SPR并聯(lián)機(jī)構(gòu)的正運(yùn)動學(xué)求解            **
% ** Author: __Lnx                                       **
% ** Date:   2023-03-26                                  **
%**********************************************************
function [R_output,T_output] = SPR_ForwardKinematics(lidar1, lidar2, lidar3, p4)

%     這是一組示例
%     lidar1 = [304.455 1054.25 21.7591];
%     lidar2 = [302.484	1355.49	21.9924];
%     lidar3 = [297.842	1198	22.56111];
%     lidar1 = [304.455 711.704 29.55];
%     lidar2 = [302.484	1030.71	24.915];
%     lidar3 = [297.842	1030.71	24.915];
% 	  p4 = 500;

    % r 為球鉸副距離世界坐標(biāo)系原點(diǎn)距離；Spare_Local 為球鉸副在動平臺坐標(biāo)系中的坐標(biāo)
    [r,Spare_Local] = SPR_SpareJointCalculate(lidar1,lidar2,lidar3);
    Spare_World = [[r(1)*cosd(30) r(1)*sind(30) 0]' [r(2)*cosd(150)  r(2)*sind(150)  0]' [r(3)*cosd(270) r(3)*sind(270) 0]'];
    hold on;
%     scatter3(Spare_World(1,1),Spare_World(2,1),Spare_World(3,1));
%     scatter3(Spare_World(1,2),Spare_World(2,2),Spare_World(3,2));
%     scatter3(Spare_World(1,3),Spare_World(2,3),Spare_World(3,3));

    % ICP
    R = eye(3);      % 旋轉(zhuǎn)矩陣初始化
    t = zeros(3, 1); % 位移矩陣初始化

    X = Spare_World; % 目標(biāo)點(diǎn)云
    Y = Spare_Local; % 源點(diǎn)云

    % 計(jì)算Y中每個點(diǎn)到X中的最近點(diǎn)
    idx = [1 2 3]';
    err = norm(X(1)-Y(1))+norm(X(2)-Y(2))+norm(X(3)-Y(3));

    % 去中心
    Y_mean = mean(Y, 2);
    X_mean = mean(X(:, idx), 2);
    H = (Y - Y_mean) * (X(:, idx) - X_mean)';

    [U, ~, V] = svd(H);
    R_cur = V * U';
    t_cur = X_mean - R_cur * Y_mean;

    % 更新變換矩陣
    R = R_cur;
    t = R_cur * t + t_cur;


    % 更新點(diǎn)云
    Y = R_cur * Y + t_cur;
    hold on;
    scatter3(Y(1,:),Y(2,:),Y(3,:),'red');
    scatter3(X(1,:),X(2,:),X(3,:),'black');
axis equal;
    R_output = R;
    T_output = t + R*[0,0,p4]';
end

其中包含另一個函數(shù)：SPR_SpareJointCalculate(lidar1,lidar2,lidar3)文章來源地址http://www.zghlxwxcb.cn/news/detail-786727.html

%**********************************************************    
% ** Name:   三個球鉸的位置計(jì)算                             **
% ** Author: __Lnx                                       **
% ** Date:   2023-03-16                                  **
%**********************************************************


function [output_r,output_B] = SPR_SpareJointCalculate(lidar1,lidar2,lidar3)
    % 輸入
    theta =[lidar1(3) lidar2(3) lidar3(3)];
    P =[lidar1(2) lidar2(2) lidar3(2)];
    % 常量
    alpha = [30 150 270];
    r = 78.603; % 動平臺轉(zhuǎn)動副外切圓半徑
    k = 42.5;

    l = 139.135;
    m = 159.966;%220;         
    P = P + l + m;  % 移動副長度

    alpha = deg2rad(alpha);
    theta = deg2rad(theta);

    % 計(jì)算B1\B2\B3在動平臺坐標(biāo)系中的位置
    B1 = [(r+P(1)*sin(theta(1))-k*cos(theta(1)))*cos(alpha(1))
          (r+P(1)*sin(theta(1))-k*cos(theta(1)))*sin(alpha(1))
           -(P(1)*cos(theta(1))+k*sin(theta(1)))];
    B2 = [(r+P(2)*sin(theta(2))-k*cos(theta(2)))*cos(alpha(2))
          (r+P(2)*sin(theta(2))-k*cos(theta(2)))*sin(alpha(2))
           -(P(2)*cos(theta(2))+k*sin(theta(2)))];
    B3 = [(r+P(3)*sin(theta(3))-k*cos(theta(3)))*cos(alpha(3))
          (r+P(3)*sin(theta(3))-k*cos(theta(3)))*sin(alpha(3))
           -(P(3)*cos(theta(3))+k*sin(theta(3)))];
    hold on;
%     scatter3(B1(1),B1(2),B1(3));
%     scatter3(B2(1),B2(2),B2(3));
%     scatter3(B3(1),B3(2),B3(3));
%     output_B = [B2 B3 B1];% 非對稱
      output_B = [B1 B2 B3];% 對稱

    L(1) = norm(B1-B2);
    L(2) = norm(B1-B3);
    L(3) = norm(B2-B3);

    syms r1 r2 r3;
    f1 = r2^2+r3^2+r2*r3 == L(1)^2;
    f2 = r1^2+r3^2+r1*r3 == L(2)^2;
    f3 = r1^2+r2^2+r1*r2 == L(3)^2;
    [r1,r2,r3] = solve([f1, f2, f3],[r1 r2 r3]);
%     r = eval([r2 r1 r3]);% 非對稱
    r = eval([r1 r2 r3]);% 對稱

    for i=1:1:size(r,1) % 正數(shù)解
        if(r(i,1)>0 && r(i,2)>0 && r(i,3)>0)
            output_r = r(i,:);
            break;
        end
    end
end

到了這里，關(guān)于非對稱3-SPR并聯(lián)機(jī)器人正逆運(yùn)動學(xué)（Matlab源碼）的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！