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

主程序如下：

%利用不共线三点求解并联机构动系在定系中的位姿
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

结果有待验证！！！