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

主程序如下：

%利用不共线三点求解并联机构动系在定系中的位姿
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 ZA XB YB ZB XC YC ZC T_P_E T_M_B%定义全局变量
%% 利用fsolve函数求解座标原点
fun=@myfun_2;
x0=[0;0;192];% 设定初值
opt=optimset('Display','off');%不显示输出
x=fsolve(fun,x0,opt);%调用函数求解座标原点
%% 利用cross函数求三正交向量
X=[XB-XC YB-YC ZB-ZC];%X轴向量,由C点指向B点
b=[XA-XC YA-YC ZA-ZC];%其中一个边的向量，由C点指向A点
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];%%被测基准块在测量系中的座标变换矩阵
T_M_E=[R x;
       0 0 0 1];%被测基准块在测量系中的齐次座标变换矩阵
T_B_P=(T_M_B)^-1*T_M_E*(T_P_E)^-1;%动系在定系中的齐次座标变换矩阵 
%% 计算动系在定系的中位姿
alfa=T_B_P(2,3)/T_B_P(3,3);%等价旋转角alfa
beta=atan(-T_B_P(1,3)/sqrt(T_B_P(1,1)^2+T_B_P(1,2)^2));%等价旋转角beta
gama=atan(T_B_P(1,2)/T_B_P(1,1));%等价旋转角gama
Q=[T_B_P(1,4) T_B_P(2,4) T_B_P(3,4) alfa beta gama];%程序输出

子程序的子程序如下：

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

运行结果如下：

Q =

  -92.5236   -8.4133  225.9525    1.6589   -0.2395   -0.8589
   29.1866  -44.8791  240.0577   -5.1141   -0.6164    1.1533
   35.1826  -80.0305  325.4927    0.7855    0.5133    1.3096
   41.0961   29.0553  174.4461    0.2997   -0.5033    0.6695
  -88.3679   21.6326  265.1052    0.9142   -0.1099   -1.4699
  -67.4241  -29.4202  137.0946    1.7923   -0.0448   -1.0142
  -17.0258 -110.2923  224.5796   -4.6028    0.2994   -1.3216
   85.5420   18.4292  220.4272    0.4911   -0.7235   -1.2104
  -41.8621  -94.1039  254.3527    0.5754    0.5964   -0.0079
 -145.5743    0.9559  131.2870    6.8798    0.4713    0.5793
    8.2815   15.0953  231.2123   -0.5058   -0.7559    0.2392
  -61.4213   53.5875  176.5768    1.0170    0.9116   -0.2903
 -119.2199 -131.6504  212.0024   -1.3271   -0.2203    1.1731
  -61.6812  -44.2491  285.7788    0.4959    1.1713    0.1633
  -48.9099  -54.8524  251.2883   15.6221    0.7180    1.2227

结果有待验证！！！