欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2024-02-21 18:55:22
...

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

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

 主程序为:

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