主程序如下：
%利用不共線三點求解並聯機構動系在定系中的位姿
global XA YA ZA XB YB ZB XC YC ZC %定義全局變量
A=100*(rand(15,9)*2-1);% 設定15組能夠確定座標原點的三個點，每一行代表一個組，每組各元素含義如下：
% XA YA ZA XB YB ZB XC YC ZC
T_P_E=[1 0 0 10;
0 1 0 10;
0 0 1 -50;
0 0 0 1];%被測基準塊座標系在動系中的位姿
T_M_B=[1 0 0 20;
0 1 0 20;
0 0 1 -200;
0 0 0 1];%被測基準塊座標系在動系中的位姿
P=zeros(15,3);R=cell(15,1);T_M_E=cell(15,1);T_B_P=cell(15,1);alfa=zeros(15,1);beta=zeros(15,1);gama=zeros(15,1);
Q=zeros(15,6);% 預設內存給各變量
for i=1:length(A)
XA=A(i,1); YA=A(i,2);ZA=A(i,3);XB=A(i,4);YB=A(i,5);ZB=A(i,6);XC=A(i,7);YC=A(i,8);ZC=A(i,9);%方程組參數賦值
fun=@myfun_2;
x0=[0;0;192];% 設定初值
opt=optimset('Display','off');%不顯示輸出
x=fsolve(fun,x0,opt); %調用函數
P(i,1)=x(1);%原點X向座標賦值
P(i,2)=x(2);%原點Y向座標賦值
P(i,3)=x(3);%原點Z向座標賦值
R{i,1}=xuanzhuanjuzhen(A(i,:));%調用子程序，計算被測基準塊在測量系中的旋轉矩陣R
T_M_E{i,1}=[R{i,1} P(i,:)';
0 0 0 1];%被測基準塊在測量系中的齊次座標變換矩陣
T_B_P{i,1}=(T_M_B)^-1*T_M_E{i,1}*(T_P_E)^-1;%動系在定系中的齊次座標變換矩陣
alfa(i)=T_B_P{i,1}(2,3)/T_B_P{i,1}(3,3);;%等價旋轉角alfa
beta(i)=atan(-T_B_P{i,1}(1,3)/sqrt(T_B_P{i,1}(1,1)^2+T_B_P{i,1}(1,2)^2));%等價旋轉角beta
gama(i)=atan(T_B_P{i,1}(1,2)/T_B_P{i,1}(1,1));%等價旋轉角gama
Q(i,:)=[T_B_P{i,1}(1,4) T_B_P{i,1}(2,4) T_B_P{i,1}(3,4) alfa(i) beta(i) gama(i)]%輸出位姿
end
子程序myfun_2.m如下：
function f=myfun_2(x)
global XA YA ZA XB YB ZB XC YC ZC %定義全局變量
f1=(norm([XA-x(1);YA-x(2);ZA-x(3)]))^2+(norm([XB-x(1);YB-x(2);ZB-x(3)]))^2-(norm([XA-XB;YA-YB;ZA-ZB]))^2;
%勾股定理，原點與其中兩點構成直角三角形
f2=(norm([XB-x(1);YB-x(2);ZB-x(3)]))^2+(norm([XC-x(1);YC-x(2);ZC-x(3)]))^2-(norm([XB-XC;YB-YC;ZB-ZC]))^2;
%勾股定理，原點與其中兩點構成直角三角形
f3=norm([XA-x(1);YA-x(2);ZA-x(3)])+norm([XC-x(1);YC-x(2);ZC-x(3)])-norm([XA-XC;YA-YC;ZA-ZC]);
%三點一線時，中間一點與兩端點距離和等於兩端點之間距離
f=[f1;f2;f3];
end
子程序xuanzhuanjuzhen.m如下：
function R=xuanzhuanjuzhen(P)
%利用不共線三點確定被測基準塊在測量系中的旋轉矩陣，輸入A爲1*9向量，即[x1 y1 z1 x2 y2 z2 x3 y3 z3]，輸出R爲3*3矩陣
%% 利用cross函數求三正交向量
x=[P(4)-P(1) P(5)-P(2) P(6)-P(3)];%X軸向量,由第一點指向第二點
b=[P(7)-P(1) P(8)-P(2) P(9)-P(3)];%其中一個邊的向量，由第一點指向第三點
z=cross(b,x);%Z軸向量，同時垂直於x和b
y=cross(x,z);%Y軸向量，同時垂直於x和z
%% 修正向量主元正負,主元爲正就取正，主元爲負就取反，這樣可確保被測系與測量系座標軸方向大致相同
if x(1)>0
x=x;
else
x=-x;
end
if y(2)>0
y=y;
else
y=-y;
end
if z(3)>0
z=z;
else
z=-z;
end
%% 向量歸一化
norm_x = sqrt(x(1)^2+x(2)^2+x(3)^2);%X軸向量的模
ux=x(1)/norm_x;
uy=x(2)/norm_x;
uz=x(3)/norm_x;
norm_y = sqrt(y(1)^2+y(2)^2+y(3)^2);%Y軸向量的模
vx=y(1)/norm_y;
vy=y(2)/norm_y;
vz=y(3)/norm_y;
norm_z = sqrt(z(1)^2+z(2)^2+z(3)^2);%Z軸向量的模
wx=z(1)/norm_z;
wy=z(2)/norm_z;
wz=z(3)/norm_z;
%% 程序輸出
R=[ux vx wx;
uy vy wy;
uz vz wz];
計算結果如下：
Q =
-34.5679 -4.4202 261.9103 0.5992 -0.2799 -0.5671
79.5129 -11.7875 284.1264 0.8230 -0.2030 -0.9711
-2.6967 -53.5858 218.0119 0.5333 -0.9370 0.2876
-79.0990 -65.9837 335.3170 0.2237 0.0627 0.9961
-8.3867 20.9725 176.6826 0.2235 -0.2997 -0.7220
-157.2659 -68.1308 174.7557 -0.0933 0.7128 0.4830
74.6091 -41.4194 325.6684 0.7410 -0.5180 -1.1615
85.9828 -13.1062 133.2380 0.8691 -1.0387 -1.1595
10.3721 -59.2937 194.8132 -2.6244 -0.3709 -0.4577
29.6488 -44.8424 264.7108 -0.2685 -0.1591 1.5648
-23.0435 -32.4045 211.8318 -0.4754 0.1041 -0.7578
-1.5274 59.0978 200.3245 0.7501 -1.2967 0.1456
69.3299 -46.1658 333.3478 0.7161 -1.1002 0.6028
-13.9346 -62.7190 229.5088 0.2071 -0.5163 1.4465
-69.3607 -85.2744 230.5428 -4.2681 -0.5090 1.4784
結果有待驗證！！！