非対称配置における球面関節を持つ3-SPRパラレルロボットの順逆運動学解析
本稿では、球面関節が非対称に配置された3-SPRパラレルロボットの順運動学および逆運動学の解法について詳細に解説します。
3-SPRパラレルロボットの運動学モデル
S：球面関節 P：直動関節 R：回転関節
3-SPRパラレルロボットは、3つの独立したSPR運動チェーンから構成されます。
入力：3つの直動関節（P）の伸長量
出力：システム末端の基準点（通常は動プラットフォームの中心点）
***運動学モデル***：
（図では4つの直動関節を使用しており、パラレル機構に直動関節を直列に接続し、より広い運動範囲を実現しています）
3-SPRパラレルロボットの場合、末端基準点は OP（動プラットフォーム中心点）です。
直列直動関節を追加した機構の場合、末端基準点は OT（動プラットフォーム上に直列接続された直動関節の末端）です。
| 記号 | 説明 |
|---|---|
| B1、B2、B3 | **球面関節**、これら3点の連結線が固定プラットフォームを構成 |
| A1、A2、A3 | **回転関節**、これら3点の連結線が動プラットフォームを構成 |
| P1、P2、P3、P4 | **直動関節**、パラレル機構の入力 |
逆運動学の解法
3-SPRパラレルロボットの位置逆解は、システム末端基準点 OP の世界座標系{W}における座標（固定プラットフォームが存在する座標系）が既知の場合に、3-SPRパラレルロボットの直動関節 P1、P2、P3 の伸長量を求める問題として記述できます。
***解法公式***：
公式の解法プロセスは主に制約条件を利用します。パラレルロボットの運動中、直動関節 P の方向ベクトルは常に回転関節の回転軸と垂直です（P の方向ベクトルは球面関節 B から直動関節中心点 A へ向かいます）。3つの制約方程式を確立し、連立方程式を解くことで位置逆解を得ることができます（証明プロセスは省略、必要であれば別途公開します）。
球面関節が非対称に配置されているため、解法時には3つの球面関節の位置を公式に入力する必要があります（rB1、rB2、rB3）。
***MATLABプログラム***：
%***************************************************************
% ** 名称: 非対称配置3-SPRパラレル機構の逆運動学解法 **
% ** 入力: joint1、joint2、joint3：球面関節B1、B2、B3の原点O_Wからの距離 **
% X_pos、Y_pos、Z_pos：到達目標位置O_Tの座標 **
% ext4：直動関節P4の伸長量、3-SPRパラレル機構の場合は0に設定 **
%****************************************************************
function result = SPR_ReverseKinematics(joint1, joint2, joint3, X_pos, Y_pos, Z_pos, ext4)
platformRadius = 78.603; % 動プラットフォーム回転関節中心点の外接円半径（A1、A2、A3からO_Pまでの距離）
syms Delta Theta Phi % 6自由度のうち3つが既知、残り3つの自由度を求める
% 回転行列の定義
ux = cosd(Theta)*cosd(Delta);
uy = cosd(Theta)*sind(Delta);
uz = -sind(Theta);
vx = sind(Phi)*sind(Theta)*cosd(Delta)-cosd(Phi)*sind(Delta);
vy = sind(Phi)*sind(Theta)*sind(Delta)+cosd(Phi)*cosd(Delta);
vz = sind(Phi)*cosd(Theta);
wx = cosd(Phi)*sind(Theta)*cosd(Delta)+sind(Phi)*sind(Delta);
wy = cosd(Phi)*sind(Theta)*sind(Delta)-sind(Phi)*cosd(Delta);
wz = cosd(Phi)*cosd(Theta);
rotMatrix(1,1) = ux; rotMatrix(1,2)=vx; rotMatrix(1,3)=wx;
rotMatrix(2,1) = uy; rotMatrix(2,2)=vy; rotMatrix(2,3)=wy;
rotMatrix(3,1) = uz; rotMatrix(3,2)=vz; rotMatrix(3,3)=wz;
% O_Tの位置からO_Pの位置を計算
X_op = X_pos-rotMatrix(1,:)*[0 0 ext4]';
Y_op = Y_pos-rotMatrix(2,:)*[0 0 ext4]';
Z_op = Z_pos-rotMatrix(3,:)*[0 0 ext4]';
% 公式に基づいて解く
f1 = atand((3*(joint2-joint1)*(cosd(Theta)-cosd(Phi))+3*sqrt(3)*(joint1+joint2)*sind(Phi)*sind(Theta))/(4*sqrt(3)*joint3*cosd(Theta)+3*(joint2-joint1)*sind(Phi)*sind(Theta)+sqrt(3)*(joint1+joint2)*(3*cosd(Phi)+cosd(Theta)))) == Delta;
f2 = (6*(uz*vy-vz*uy)*Z_op-(3*(ux-vy)+sqrt(3)*(3*vx-uy))*uy*joint2+2*(sqrt(3)*uy+3*vy)*uy*joint3)/(6*(vx*uy-ux*vy)) == X_op;
f3 = (6*(uz*vx-vz*ux)*Z_op-(3*(ux-vy)+sqrt(3)*(3*vx-uy))*ux*joint2+2*(sqrt(3)*ux+3*vx)*uy*joint3)/(6*(ux*vy-vx*uy)) == Y_op;
[DELTA_VAL,THETA_VAL,PHI_VAL] = solve([f1, f2, f3],[Delta, Theta, Phi]);
solution = eval([DELTA_VAL,THETA_VAL,PHI_VAL]);
T = solution(2);D = solution(1);P = solution(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);
finalRot(1,1) = rux; finalRot(1,2)=rvx; finalRot(1,3)=rwx;
finalRot(2,1) = ruy; finalRot(2,2)=rvy; finalRot(2,3)=rwy;
finalRot(3,1) = ruz; finalRot(3,2)=rvz; finalRot(3,3)=rwz;
% 入力に基づいて球面関節点座標を計算
B1 = [joint1*cosd(30) joint1*sind(30) 0]';
B2 = [joint2*cosd(150) joint2*sind(150) 0]';
B3 = [joint3*cosd(270) joint3*sind(270) 0]';
% 求めた回転行列に基づいて、動プラットフォーム中心点O_Pの座標を計算
O_Target=[X_pos, Y_pos, Z_pos]';
O_Target = O_Target-finalRot*[0 0 ext4]';
% 求めた回転行列とO_P点の座標に基づいて、各回転関節座標を計算
A1 = finalRot * [platformRadius*cosd(30) platformRadius*sind(30) 0]'+ O_Target;
A2 = finalRot * [platformRadius*cosd(150) platformRadius*sind(150) 0]'+ O_Target;
A3 = finalRot * [platformRadius*cosd(270) platformRadius*sind(270) 0]'+ O_Target;
% 各ロッドの伸長量を計算
extension1 = norm(A1 - B1);
extension2 = norm(A2 - B2);
extension3 = norm(A3 - B3);
% 結果を出力
result = [extension1, extension2, extension3];
end
順運動学の解法
順運動学の解法は従来の方法とは異なり、点群レジストレーションの考え方を利用しています。
***主な考え方***：
***MATLAB順運動学計算関数：SPR_ForwardKinematics***
%**********************************************************
% ** 名称: 非対称配置3-SPRパラレル機構の順運動学解法 **
%**********************************************************
function [rotResult,transResult] = SPR_ForwardKinematics(sensor1, sensor2, sensor3, ext4)
% サンプルデータ
% sensor1 = [304.455 1054.25 21.7591];
% sensor2 = [302.484 1355.49 21.9924];
% sensor3 = [297.842 1198 22.56111];
% sensor1 = [304.455 711.704 29.55];
% sensor2 = [302.484 1030.71 24.915];
% sensor3 = [297.842 1030.71 24.915];
% ext4 = 500;
% radiusは球面関節の世界座標系原点からの距離
% LocalSpareは動プラットフォーム座標系における球面関節の座標
[radius,LocalSpare] = SPR_SphericalJointCalculation(sensor1,sensor2,sensor3);
WorldSpare = [[radius(1)*cosd(30) radius(1)*sind(30) 0]' [radius(2)*cosd(150) radius(2)*sind(150) 0]' [radius(3)*cosd(270) radius(3)*sind(270) 0]'];
% ICPアルゴリズム
rotation = eye(3); % 回転行列の初期化
translation = zeros(3, 1); % 並進ベクトルの初期化
targetCloud = WorldSpare; % ターゲット点群
sourceCloud = LocalSpare; % ソース点群
% ソース点群の各点からターゲット点群への最近点を計算
indices = [1 2 3]';
error = norm(targetCloud(1)-sourceCloud(1))+norm(targetCloud(2)-sourceCloud(2))+norm(targetCloud(3)-sourceCloud(3));
% 中心化
sourceMean = mean(sourceCloud, 2);
targetMean = mean(targetCloud(:, indices), 2);
H = (sourceCloud - sourceMean) * (targetCloud(:, indices) - targetMean)';
[U, ~, V] = svd(H);
currentRot = V * U';
currentTrans = targetMean - currentRot * sourceMean;
% 変換行列を更新
rotation = currentRot;
translation = currentRot * translation + currentTrans;
% 点群を更新
sourceCloud = currentRot * sourceCloud + currentTrans;
rotResult = rotation;
transResult = translation + rotation*[0,0,ext4]';
end
***以下の補助関数を含む：SPR_SphericalJointCalculation(sensor1,sensor2,sensor3)***
%**********************************************************
% ** 名称: 3つの球面関節の位置計算 **
%**********************************************************
function [outputRadius,outputJoints] = SPR_SphericalJointCalculation(sensor1,sensor2,sensor3)
% 入力
angles =[sensor1(3) sensor2(3) sensor3(3)];
distances =[sensor1(2) sensor2(2) sensor3(2)];
% 定数
positions = [30 150 270];
radius = 78.603; % 動プラットフォーム回転関節の外接円半径
offset = 42.5;
link1 = 139.135;
link2 = 159.966;
distances = distances + link1 + link2; % 直動関節の長さ
positions = deg2rad(positions);
angles = deg2rad(angles);
% 動プラットフォーム座標系におけるB1\B2\B3の位置を計算
B1 = [(radius+distances(1)*sin(angles(1))-offset*cos(angles(1)))*cos(positions(1))
(radius+distances(1)*sin(angles(1))-offset*cos(angles(1)))*sin(positions(1))
-(distances(1)*cos(angles(1))+offset*sin(angles(1)))];
B2 = [(radius+distances(2)*sin(angles(2))-offset*cos(angles(2)))*cos(positions(2))
(radius+distances(2)*sin(angles(2))-offset*cos(angles(2)))*sin(positions(2))
-(distances(2)*cos(angles(2))+offset*sin(angles(2)))];
B3 = [(radius+distances(3)*sin(angles(3))-offset*cos(angles(3)))*cos(positions(3))
(radius+distances(3)*sin(angles(3))-offset*cos(angles(3)))*sin(positions(3))
-(distances(3)*cos(angles(3))+offset*sin(angles(3)))];
outputJoints = [B1 B2 B3];
lengths(1) = norm(B1-B2);
lengths(2) = norm(B1-B3);
lengths(3) = norm(B2-B3);
syms r1 r2 r3;
eq1 = r2^2+r3^2+r2*r3 == lengths(1)^2;
eq2 = r1^2+r3^2+r1*r3 == lengths(2)^2;
eq3 = r1^2+r2^2+r1*r2 == lengths(3)^2;
[r1,r2,r3] = solve([eq1, eq2, eq3],[r1, r2, r3]);
solutions = eval([r1, r2, r3]);
for i=1:1:size(solutions,1) % 正の解を探索
if(solutions(i,1)>0 && solutions(i,2)>0 && solutions(i,3)>0)
outputRadius = solutions(i,:);
break;
end
end
end