三点法求解并联机构动系相对静系的旋转矩阵

主程序如下： 

%% 参数定义
A=[0.17452405 -0.26172963 9.9950504]; %动平台A点在测量系下的坐标
B=[9.9924393 5.0868735 9.9636774]; %动平台B点在测量系下的坐标
C=[-0.17441772 9.7285938 10.262749]; %动平台C点在测量系下的坐标
%% 求解动系x向量与y向量
fun1 = @diandaozhixian;
x0 = [1 0 0];
x = fsolve(fun1,x0);
x_vector=(B-x)/10; %动系x向量
y_vector=(C-x)/5; %动系y向量
%% 求解动系z向量
fun2 = @orthogonal_vector;
z0 = [0,0,1];
z_vector= fsolve(fun2,z0);% 动系z向量
%%  求解旋转矩阵
R=[x_vector' y_vector' z_vector'];% 动系相对静系的旋转矩阵

 子程序 diandaozhixian.m 如下：

function F=diandaozhixian(x)
A=[0.17452405 -0.26172963 9.9950504];
B=[9.9924393 5.0868735 9.9636774];
C=[-0.17441772 9.7285938 10.262749];
F(1)=(x(1)-A(1,1))^2+(x(2)-A(1,2))^2+(x(3)-A(1,3))^2+(x(1)-B(1,1))^2+(x(2)-B(1,2))^2+(x(3)-B(1,3))^2-11.18034^2;
F(2)=(x(1)-C(1,1))^2+(x(2)-C(1,2))^2+(x(3)-C(1,3))^2+(x(1)-B(1,1))^2+(x(2)-B(1,2))^2+(x(3)-B(1,3))^2-11.18034^2;
F(3)=sqrt((x(1)-A(1,1))^2+(x(2)-A(1,2))^2+(x(3)-A(1,3))^2)+sqrt((x(1)-C(1,1))^2+(x(2)-C(1,2))^2+(x(3)-C(1,3))^2)-10;

子程序 orthogonal_vector.m 如下：

function F=orthogonal_vector(x)
F(1)=x(1)*9.9923859/10+x(2)*0.35344172/10+x(3)*(-0.16522217/10);
F(2)=x(1)*(-0.1744709/5)+x(2)*4.995162/5+x(3)*0.13384914/5;
F(3)=x(1)^2+x(2)^2+x(3)^2-1;

求得旋转矩阵为：

R =

   0.999241973970996  -0.034887456058008   0.017452390365523
   0.035339093339779   0.999022246679559  -0.026172920256372
  -0.016329455880081   0.027155408239837   0.999505196282925