已知六个固定点P0（3+2+1），过固定点向量v1，动点到固定点距离L，被测基准块坐标系在并联机构动系中的位姿，并联机构定系在测量系中的位姿，求解动系在测量系中位姿Q的程序

主程序为：

clc; clear all;close all
warning off
global T_P_E T_M_B
P0=[-124  51  265;%定点S1
    -39  24  295;%定点S2
    -93 -56 260;%定点S3
    -110 -68 224;%定点S4
    -72 -55 136;%定点S5
    -135 10 186];%定点S6
    %x  y  z 
v1=[0.01 0.01 1;%过定点S1的直线向量
    0.01 0.01 1;%过定点S2的直线向量
    0.01 0.01 1;%过定点S3的直线向量
    0.01 1 0.01;%过定点S4的直线向量
    0.01 1 0.01;%过定点S5的直线向量
    1 0.01 0.01];%过定点S6的直线向量
    % x y z
L=-10*(rand(15,6)*2-1);%产生15*3的随机位移矩阵，定义不同动点到定点P0的距离，负号表示方向
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];%并联机构定系在测量系中的位姿
Q=cell(15,1);%预设元胞数组给Q
for q=1:length(L)
    Q{q}=weizijiesuan(P0,v1,L(q,:));%调用位姿解算程序，求解位姿
 end

子程序weizijiesuan.m为：

function Q=weizijiesuan(P0,v1,L)
%已知六个固定点P0，过固定点向量v1，动点到固定点距离L，确定动系在定系中位姿的程序
global T_P_E T_M_B
%% 计算固定点对应的动点
P1(1,:)=P0(1,:)+L(1)*v1(1,:)/(sum(v1(1,:).^2).^(1/2));%计算固定点1对应的动点
P1(2,:)=P0(2,:)+L(2)*v1(2,:)/(sum(v1(2,:).^2).^(1/2));%计算固定点2对应的动点
P1(3,:)=P0(3,:)+L(3)*v1(3,:)/(sum(v1(3,:).^2).^(1/2));%计算固定点3对应的动点
P1(4,:)=P0(4,:)+L(4)*v1(4,:)/(sum(v1(4,:).^2).^(1/2));%计算固定点4对应的动点
P1(5,:)=P0(5,:)+L(5)*v1(5,:)/(sum(v1(5,:).^2).^(1/2));%计算固定点5对应的动点
P1(6,:)=P0(6,:)+L(6)*v1(6,:)/(sum(v1(6,:).^2).^(1/2));%计算固定点6对应的动点
%% 计算六动点确定的三个正交平面
A1=[P1(1,1) P1(1,2) P1(1,3);
  P1(2,1) P1(2,2) P1(2,3);
  P1(3,1) P1(3,2) P1(3,3)];%求被测基准块XOY面方程的系数矩阵
 b1=[-1;-1;-1];%求被测基准块XOY面方程的b矩阵
 C1=A1\b1;%XOY面方程的法向量
A2=[P1(4,1) P1(4,2) P1(4,3);
P1(5,1) P1(5,2) P1(5,3);
C1(1),C1(2),C1(3)];%求被测基准块XOZ面方程的系数矩阵
b2=[-1;-1;0];%求被测基准块XOZ面方程的b矩阵
C2=A2\b2;
A3=[P1(6,1) P1(6,2) P1(6,3);
C1(1),C1(2),C1(3);
C2(1),C2(2),C2(3)];%求被测基准块YOZ面方程的系数矩阵
b3=[-1;0;0];%求被测基准块YOZ面方程的b矩阵
C3=A3\b3;
%% 计算三个正交平面的交点，即被测基准块的原点
A4=[C1(1),C1(2),C1(3);
C2(1),C2(2),C2(3);
C3(1),C3(2),C3(3)];%求三平面交点的方程的A矩阵
b4=[-1;-1;-1];%求三平面交点的方程的b矩阵
%% 计算被测基准块在测量系中的坐标变换矩阵
P=A4\b4;%三平面交点
NML_C1=[C1(1)/norm(C1) C1(2)/norm(C1) C1(3)/norm(C1)];%平面法向量归一化，下同
NML_C2=[C2(1)/norm(C2) C2(2)/norm(C2) C2(3)/norm(C2)];
NML_C3=[C3(1)/norm(C3) C3(2)/norm(C3) C3(3)/norm(C3)];
R=[NML_C1(1) NML_C2(1) NML_C3(1);
NML_C1(2) NML_C2(2) NML_C3(2);
NML_C1(3) NML_C2(3) NML_C3(3)];%被测基准块在测量系中的坐标变换矩阵
%% 计算动系在定系中的齐次坐标变换矩阵
T_M_E=[R P
   0 0 0 1];%被测基准块在测量系中的齐次坐标变换矩阵
T_B_P=(T_M_B)^-1*T_M_E*(T_P_E)^-1;%动系在定系中的齐次坐标变换矩阵
%% 计算动系在定系的中位姿
alfa=atan(NML_C2(3)/NML_C3(3));%等价旋转角alfa
beta=atan(-NML_C1(3)/sqrt(NML_C1(1)^2+NML_C1(2)^2));%等价旋转角beta
gama=atan(NML_C1(2)/NML_C1(1));%等价旋转角gama
Q=[T_B_P(1,4) T_B_P(2,4) T_B_P(3,4) alfa beta gama];%程序输出

这里面T_P_E和 T_M_B必须设置为全局变量，设为局部变量就不行，不知何故。