利用不共线三点求解并联机构动系在定系中的位姿的另一种解法

主程序如下：

%利用不共线三点求解并联机构动系在定系中的位姿
global XA YA ZA XB YB ZB XC YC ZC T_P_E T_M_B%定义全局变量
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];%被测基准块坐标系在动系中的位姿
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=cell(15,1);% 预设内存给各变量
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);%方程组参数赋值
Q{i}=xuanzhuanjuzhen(A(i,:));%调用子程序，计算被测基准块在测量系中的旋转矩阵R
end

子程序xuanzhuanjuzhen.m如下：

function Q=xuanzhuanjuzhen(P)
%利用不共线三点确定被测基准块在测量系中的旋转矩阵，输入A为1*9向量，即[x1 y1 z1 x2 y2 z2 x3 y3 z3]，输出R为3*3矩阵
global XA YA Z