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

主程序如下：

%利用不共线三点求解并联机构动系在定系中的位姿
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)>0x=x;
elsex=-x;
end
if y(2)>0y=y;
elsey=-y;
end
if z(3)>0z=z;
elsez=-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

结果有待验证！！！

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

不共线三点确定特征平面并求解平面系数
1.基本原理已知三点p1,p2,p3,求其平面方程,p1=[x1,y1,z1];p2=[x2,y2,z2];p3=[x3,y3,z3];那么求解下面矩阵q行列式就是了:q=[ones(4,1),[x ...
七参数坐标转换 matlab,利用三点法求解空间坐标系转换所需的七参数
同一个物体的点云在不同坐标系中的坐标表示不同,有时候需要将其进行转换到同一坐标系中进行处理,经典的方法为七参数法(7-parameters),用matlab实现如下: %PS.好久不用matlab了, ...
在Simulink中利用simmechanics对三自由度的串联机械臂进行仿真
在写本科的毕业论文的时候,需要对三自由度的机械臂进行一个仿真实验.在网上查阅相关资料,得知可以利用simmechanics 来进行仿真实验.关于simmechanics 这个的介绍就百度一下就可以,这 ...
利用线性筛算法框架求解因数个数以及因数和问题
利用线性筛算法框架求解因数个数以及因数和问题一前言关于线性筛算法,在前一篇文章利用线性筛以及素数筛求某一范围内的所有素数中已经介绍过,若读者对线性筛算法不太了解或有所遗忘,可以点击链接查看.此 ...
ZZULIOJ 1076: 三位数求解
三位数求解题目描述已知xyz+yzz=n,其中n是一个正整数,x.y.z都是数字(0-9),编写一个程序求出x.y.z分别代表什么数字.如果无解,则输出"No Answer" ...
html让圆点闪烁,利用jQ CSS3实现圆点闪烁背景动效
特效描述:利用jQ CSS3实现圆点闪烁背景动效.利用jQ CSS3实现圆点闪烁背景动效代码结构 1. 引入CSS 2. 引入JS 3. HTML代码 $(document).ready(fun ...
制作gif动图python_利用Python如何制作好玩的GIF动图详解
前言之前我们分享过用Python进行可视化的9种常见方式.其实我们还能让可视化图形逼格更高一些,今天就分享一下如何让可视化秀起来:用Python和matplotlib制作GIF图表. 假如电脑上没有 ...
短视频如何吸引用户？利用好“黄金三秒”很关键，教你三招
短视频如何吸引用户?利用好"黄金三秒"很关键,教你三招对于做短视频的小伙伴们来说,想要吸粉引流是肯定的,但是做短视频的人这么多,真正成功的人却占比不多.那么,如果我们想要成功,想 ...
利用CVX和gurobi这个求解器复现一篇混合整数线性规划问题时遇到的问题
利用CVX和gurobi这个求解器复现一篇混合整数线性规划问题时遇到的问题 1.当出现从cvx 转换为double 时出现错误: ![](https://img-blog.csdnimg.cn/202 ...
利用反函数求导定理求解四个反函数的导数_20160409
利用反函数求导定理求解四个反函数的导数

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

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

最新文章

热门文章