利用不共线三点确定被测基准块在测量系中的旋转矩阵

利用不共线三点确定被测基准块在测量系中的旋转矩阵的数学模型如图所示：

 主程序为：

P=100*(rand(1,9)*2-1)
R=xuanzhuanjuzhen(P)

子程序为：

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];

计算结果：

P =

   61.0979   15.3443  -63.4155  -52.0136   77.3024  -94.2652   -2.0197  -66.4146   95.7361

R =

    0.8530    0.4316    0.2935
   -0.4672    0.3807    0.7980
    0.2326   -0.8178    0.5264