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

主程序为：

clc; clear all;close all
warning off
global T_P_E T_M_B P1
P0=[-11 -1 286;%定点S1
    -42 -84 255;%定点S2
    -80 -71 188;%定点S3
    11 -103 186;%定点S4
    -85 -22 244;%定点S5
     -79 7 155];%定点S6
    %x  y  z 
v1=[0.01 0.01 1;%过定点S1的直线向量
    0.01 0.01 1;%过定点S2的直线向量
    0.01 1 0.01;%过定点S3的直线向量
    0.01 1 0.01;%过定点S4的直线向量
    1 0.01 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，确定动系在定系中位姿的程序
warning off
global T_P_E T_M_B P1
%% 计算固定点对应的动点
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对应的动点
%% 计算六动点确定的三个正交平面
x0=[0,0,1,0,1,0,1,0,0];
x=fsolve(@liudiansanmian,x0);
C1=[x(1) x(2) x(3)];%平面xoy的法向量
C2=[x(4) x(5) x(6)];%平面xoz的法向量
C3=[x(7) x(8) x(9)];%平面yoz的法向量
%% 计算三个正交平面的交点，即被测基准块的原点
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];%程序输出

子程序的子程序liudiansanmian.m为 

function f=liudiansanmian(x)
%六点求解三正交面法向量的程序，已知量为六点坐标，其中两点位于同一平面上，两点位于另一平面上，
%两点位于第三平面上，待求量为三个平面的法向量
global T_P_E T_M_B P1
f1=x(1)*P1(1,1)+x(2)*P1(1,2)+x(3)*P1(1,3)+1;
f2=x(1)*P1(2,1)+x(2)*P1(2,2)+x(3)*P1(2,3)+1;
f3=x(4)*P1(3,1)+x(5)*P1(3,2)+x(6)*P1(3,3)+1;
f4=x(4)*P1(4,1)+x(5)*P1(4,2)+x(6)*P1(4,3)+1;
f5=x(7)*P1(5,1)+x(8)*P1(5,2)+x(9)*P1(5,3)+1;
f6=x(7)*P1(6,1)+x(8)*P1(6,2)+x(9)*P1(6,3)+1;
f7=x(1)*x(4)+x(2)*x(5)+x(3)*x(6);
f8=x(1)*x(7)+x(2)*x(8)+x(3)*x(9);
f9=x(4)*x(7)+x(5)*x(8)+x(6)*x(9);
f=[f1;f2;f3;f4;f5;f6;f7;f8;f9];

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