UVW平台运动控制算法以及matlab仿真

UVW平台运动控制算法以及matlab仿真

最近公司同事因为对某视觉对位平台的运动控制算法有疑问，所以来请教我。
由于我也是第一次接触到UVW自动对位平台（也可以叫XXY自动对位平台），于是找了一些资料学习一下，大概了解了运动模式后，使用matlab模拟了此平台，并验证了UVW平台资料提供的运动控制算法的正确性。

一、UVW平台介绍

1、这是一种可以实现以平面上任意一点为中心，进行旋转运动的装置，并可沿着任意的方向平移。
2、此平台和视觉CCD纠偏系统对接在一起，可以很快完成高精度的纠偏工作，重复定位精度一般可达±1μm；

UVW平台和以前的xyθ平台相比，有以下几点不同：
1、控制精度高于xyθ平台；
2、UVW平台可以平面上任意一点为中心做旋转运动（包括无限远）；而xyθ平台由于仅仅依靠一个电机的转动控制，所以旋转中心必须是固定在平台上某处（θ电机连接处），且必须随平台一同运动。
3、基于第二点的区别，显然UVW平台是需要一个绝对坐标系作为参考系，其旋转中心才有意义；而xyθ平台则必须是一个随平台动的坐标系作为参考系，这样控制计算方法便完全不一样了。

UVW平台工作模式如下图：

二、计算方法

计算方法由平台供应商提供，截图如下：

仔细研究一下上述的公式，很容易发现，这只是简单的几何运算以及对二维坐标的求解问题。

简单说明一下视觉对位和运动控制思路：
1、通过UVW平台供应商提供的说明书，找到机械参数，得到UVW三个轴的初始坐标（基于UVW平台原点坐标系）；
2、通过视觉标定方法，确定相机坐标系到UVW平台坐标系的转换矩阵；确定标志物模板基于UVW平台原点坐标系的坐标值（x_m, y_m）；
3、通过相机得到标志物模板位置和待纠偏标志物之间的x、y、θ偏移量（基于UVW平台原点坐标系）；
4、按照上图公式，输入三个轴初始坐标，设置旋转中心为(0,0)，输入θ偏移量，可得到UVW三轴新的坐标值，以及待纠偏物体的新的坐标，以及三个电机对应的给进量A1、A2、A3；
5、输入上一步求得的UVW三轴新的坐标值，另外通过上一步求得的待纠偏物体的新的坐标，计算得此时待纠偏物体到模板点位置的x2、y2偏移量；输入x2、y2偏移量，则可以得到三个电机对应的给进量B1、B2、B3；
6、将5和6步获取的三个电机的给进量对应相加，分别得到对应电机给进量C1、C2、C3，并用此给进量驱动对应电机即可。即是，将运动过程拆解，变成平移和旋转部分，分别计算电机给进量。

接下来使用了一个matlab仿真实例，来验证自己的思路是正确的。

三、MATLAB仿真

按照第二步的程序思路，写了以下matlab代码，以下代码省略了电机给进量的计算。基本都有注释，不过多解释。
模拟效果如图所示：

下面程序定义模板和待纠偏物体时，Template 数组和Rectify_deviation 数组的值都是可以任意改变的，这两个数组即是通过传感器输入的模板坐标及位姿和待纠偏物体坐标及位姿。

  1 % 蓝色的o符号是待纠偏物体，初始位置；
  2 % 黑丝的o符号是待纠偏物体，在第一次旋转角度后的位置；
  3 % 红色的o符号是待纠偏物体，在第二次平移之后的位置
  4 % 红色的*符号是模板位置
  5
  6 close all; clear all; clc
  7
  8 %% 设置模板位置参数
  9 figure(1)
 10 grid on;axis([-150,150,-150,150]);hold on;
 11
 12 % 定义生成模板物体mode:x,y,theta（基于UVW平台绝对坐标系）
 13 Template = [25, 35 , pi*0.2];
 14 % 定义待纠偏物体re:x,y,theta（基于UVW平台绝对坐标系）
 15 Rectify_deviation = [15, 22, pi*0.4];
 16
 17  %求出第一次变换之前，模板与待纠偏物体的角度偏移（基于UVW平台绝对坐标系）
 18 THETA_M = Template(3) - Rectify_deviation(3);
 19
 20 %% 绘制模板和待纠偏物体
 21 plot(Rectify_deviation(1), Rectify_deviation(2),‘ob‘); % 绘制待纠偏物体re，蓝色o符号
 22 hold on;
 23
 24 plot(Template(1), Template(2),‘*r‘); hold on;% 绘制模板位置，红色的*符号
 25 draw_triangle(Template(1), Template(2), Template(3));hold on;
 26
 27
 28 %% UWV平台初始坐标参数
 29 R = 72.837; % 四轴到原点的半径
 30 m = 51.504; % 四个轴的坐标绝对值
 31
 32 % 轴初始坐标
 33 Ux0 = -m;
 34 Uy0 = m;
 35
 36 Vx0 = m;
 37 Vy0 = m;
 38
 39 Wx0 = m;
 40 Wy0 = -m;
 41
 42 Ox0 = -m;
 43 Oy0 = -m;
 44
 45 %% 绘制UVW平台以及待纠偏物体
 46 draw_frame(Ux0,Uy0,Vx0,Vy0,Wx0,Wy0,Ox0,Oy0);
 47 draw_circle(Ox0, Oy0, 5);
 48 draw_circle(Ux0, Uy0, 5);
 49 draw_circle(Vx0, Vy0, 5);
 50 draw_circle(Wx0, Wy0, 5);
 51 draw_triangle(Rectify_deviation(1), Rectify_deviation(2), Rectify_deviation(3))
 52 %% 1--先旋转
 53 figure(2)
 54 grid on;axis([-150,150,-150,150]);hold on;
 55 plot(Template(1), Template(2),‘*r‘);hold on; % 绘制模板位置，红色的*符号
 56 draw_triangle(Template(1), Template(2), Template(3));hold on;
 57
 58 X = 0;
 59 Y = 0;
 60 THETA = THETA_M;
 61 % 旋转中心
 62 at = 0;
 63 bt = 0;
 64
 65 % U轴执行机构-目标坐标
 66 ux = (Ux0 - at)*cos(THETA) - (Uy0 - bt)*sin(THETA) + at + X;
 67 uy = (Ux0 - at)*sin(THETA) + (Uy0 - bt)*cos(THETA) + bt + Y;
 68 % V轴执行机构-目标坐标
 69 vx = (Vx0 - at)*cos(THETA) - (Vy0 - bt)*sin(THETA) + at + X;
 70 vy = (Vx0 - at)*sin(THETA) + (Vy0 - bt)*cos(THETA) + bt + Y;
 71 % W轴执行机构-目标坐标
 72 wx = (Wx0 - at)*cos(THETA) - (Wy0 - bt)*sin(THETA) + at + X;
 73 wy = (Wx0 - at)*sin(THETA) + (Wy0 - bt)*cos(THETA) + bt + Y;
 74 % O坐标
 75 ox = (Ox0 - at)*cos(THETA) - (Oy0 - bt)*sin(THETA) + at + X;
 76 oy = (Ox0 - at)*sin(THETA) + (Oy0 - bt)*cos(THETA) + bt + Y;
 77
 78 % ！！！！求出第一次旋转后，待纠偏物体新的位姿
 79 rx1 = Rectify_deviation(1);
 80 rx2 = Rectify_deviation(2);
 81 Rectify_deviation(1) = (rx1 - at)*cos(THETA) - (rx2 - bt)*sin(THETA) + at + X;
 82 Rectify_deviation(2) = (rx1 - at)*sin(THETA) + (rx2 - bt)*cos(THETA) + bt + Y;
 83 Rectify_deviation(3) = Rectify_deviation(3) + THETA_M;
 84
 85 plot(Rectify_deviation(1), Rectify_deviation(2),‘ok‘); % 绘制待纠偏物体re，黑色o符号
 86 hold on;
 87
 88 % UVW平台新矩形位置
 89 draw_frame(ux,uy,vx,vy,wx,wy,ox,oy);
 90 draw_circle(ux, uy, 5);
 91 draw_circle(vx, vy, 5);
 92 draw_circle(wx, wy, 5);
 93 draw_circle(ox, oy, 5);
 94 draw_triangle(Rectify_deviation(1), Rectify_deviation(2), Rectify_deviation(3))
 95
 96 %% 2--再平移xy
 97 figure(3)
 98 grid on;axis([-150,150,-150,150]);hold on;
 99 plot(Template(1), Template(2),‘*r‘); % 绘制模板位置，红色的*符号
100 hold on;
101 draw_triangle(Template(1), Template(2), Template(3));hold on;
102
103 % 求出此时，模板位置相对于待纠偏物体的位姿
104 X_M = Template(1) - Rectify_deviation(1);
105 Y_M = Template(2) - Rectify_deviation(2);
106 THETA_M = Template(3) - Rectify_deviation(3);
107
108 X = X_M;
109 Y = Y_M;
110 THETA = 0;
111 % 旋转中心
112 at = 0;
113 bt = 0;
114
115 % U轴执行机构-目标坐标
116 ux = (ux - at)*cos(THETA) - (uy - bt)*sin(THETA) + at + X;
117 uy = (ux - at)*sin(THETA) + (uy - bt)*cos(THETA) + bt + Y;
118 % V轴执行机构-目标坐标
119 vx = (vx - at)*cos(THETA) - (vy - bt)*sin(THETA) + at + X;
120 vy = (vx - at)*sin(THETA) + (vy - bt)*cos(THETA) + bt + Y;
121 % W轴执行机构-目标坐标
122 wx = (wx - at)*cos(THETA) - (wy - bt)*sin(THETA) + at + X;
123 wy = (wx - at)*sin(THETA) + (wy - bt)*cos(THETA) + bt + Y;
124 % O坐标
125 ox = (ox - at)*cos(THETA) - (oy - bt)*sin(THETA) + at + X;
126 oy = (ox - at)*sin(THETA) + (oy - bt)*cos(THETA) + bt + Y;
127
128 % ！！！！求出第二次平移后，待纠偏物体新的位姿
129 rx1 = Rectify_deviation(1);
130 rx2 = Rectify_deviation(2);
131 Rectify_deviation(1) = (rx1 - at)*cos(THETA) - (rx2 - bt)*sin(THETA) + at + X;
132 Rectify_deviation(2) = (rx1 - at)*sin(THETA) + (rx2 - bt)*cos(THETA) + bt + Y;
133 Rectify_deviation(3) = Rectify_deviation(3) + THETA_M;
134 plot(Rectify_deviation(1), Rectify_deviation(2),‘or‘); % 绘制待纠偏物体re，红色的o符号
135 hold on;
136
137 % UVW平台新矩形位置
138 draw_frame(ux,uy,vx,vy,wx,wy,ox,oy);
139 draw_circle(ux, uy, 5);
140 draw_circle(vx, vy, 5);
141 draw_circle(wx, wy, 5);
142 draw_circle(ox, oy, 5);
143 draw_triangle(Rectify_deviation(1), Rectify_deviation(2), Rectify_deviation(3))
144
145 %% 规定范围以及网格
146 grid on;axis([-150,150,-150,150]);hold on;
147
148 %% 各种函数定义
149 % 以inc_x和inc_y为中心，inc_r为半径画圆
150 function ret = draw_circle(inc_x, inc_y, inc_r)
151 r = inc_r;
152 theta=0:pi/100:2*pi;
153 x = r*cos(theta) + inc_x;
154 y = r*sin(theta) + inc_y;
155
156 plot(x,y,‘-b‘);hold on; axis equal  % 等圆
157 fill(x,y, ‘c‘) % 填充颜色
158 ret = 0;
159 end
160
161 % 输入三个顶点的坐标，画直角三角形，角度30，60，90
162 function ret = draw_triangle(inc_x, inc_y, inc_theta)
163 % 定义直角三角形abc三条边,设定斜边c为：
164
165 a = 8 * 1;
166 b = 8 * sqrt(3);
167 c = 8 * 2;
168
169 AX = inc_x;
170 AY = inc_y;
171
172 BX = c * cos(inc_theta) + inc_x;
173 BY = c * sin(inc_theta) + inc_y;
174
175 CX = b * cos(inc_theta + pi/6) + inc_x;
176 CY = b * sin(inc_theta + pi/6) + inc_y;
177
178 plot([AX,BX],[AY, BY],‘-r‘);hold on;
179 plot([AX,CX],[AY, CY],‘-r‘);hold on;
180 plot([BX,CX],[BY, CY],‘-r‘);hold on;
181
182 ret = 0;
183 end
184
185 % 输入四个顶点的坐标,画矩形
186 function [] = draw_frame(Ux0,Uy0,Vx0,Vy0,Wx0,Wy0,Ox0,Oy0)
187
188 plot([Ux0,Vx0],[Uy0, Vy0],‘-r‘);hold on;
189 plot([Vx0,Wx0],[Vy0, Wy0],‘-r‘);hold on;
190 plot([Ox0,Wx0],[Oy0, Wy0],‘-r‘);hold on;
191 plot([Ox0,Ux0],[Oy0, Uy0],‘-r‘);hold on;
192 end
193

原文地址：https://www.cnblogs.com/futurelei/p/12117896.html

时间： 2024-08-09 11:33:02

UVW平台运动控制算法以及matlab仿真的相关文章

经典功率谱估计及Matlab仿真

原文出自:http://www.cnblogs.com/jacklu/p/5140913.html 功率谱估计在分析平稳各态遍历随机信号频率成分领域被广泛使用,并且已被成功应用到雷达信号处理.故障诊断等实际工程中.本文给出了经典功率谱估计的几类方法,并通过Matlab的实验仿真对经典功率谱估计方法性能进行了分析,最后说明了经典功率谱估计法的局限性和造成这种局限性的原因. 1.引言 给定一个标准的正弦信号,我们可以通过傅里叶变换来分析它的频率成分.然而,实际工程应用中,由于存在着各种干扰.噪声,我

用MATLAB仿真BPSK调制

这是本人的处女贴,以后还会写更多ICT方面的blog.今天为大家呈上MATLAB仿真BPSK调制的codes: clear all;clc; SNR_dB=1:1:9; SNR=10.^(SNR_dB./10); size=length(SNR_dB); A=ones(1,size);%固定A,定义变量sigma sigma=sqrt(1./(2*SNR));%A=1 N=10000; s=randi([0,1],1,N); s1=2*s-1; s2=zeros(1,N); N_errB=zer

极化码的matlab仿真(1)——参数设置

根据老师的安排,对于极化码的了解从仿真开始. 仿真的手段有很多种.可以利用C,C++,matlab等进行仿真的实现.其中matlab由于具有强大的函数库,和壮观的矩阵运算能力,被(我们老师课题组)看中了. 理由是,matlab的语法非常简单,接近自然语言.优秀的绘图能力,让其他软件自愧不如.还有众多的工具箱,功能强大到令人发指.当然非要用C来仿真也是可以的.但试想一个简单的函数,matlab只需要调用一下就好了,C语言怕是要自己动手写两行,何必自找麻烦呢. 话不多说,等下,我再说最后一句,本人研

MATLAB仿真总结

MATLAB仿真过程中,编写MATLAB代码的时候犯了很多错误,做了很多蠢事.记录下自己犯错的点点滴滴,并引以为戒.使用MATLAB版本为2014a,以下内容如有不当还请指正. 1. 仿真开始前清理工作区 工作区存在的变量可能会对脚本运行产生影响,故代码(脚本)开头需要添加如下命令 clc;clear all;close all; 2. 养成良好的变量.函数命名习惯 MATLAB中有很多内置的常量.函数等.写代码的时候不能够随意命名,以防造成不必要的麻烦.譬如在循环时不应该使用i,j变量,在MA

MATLAB仿真中连续和离散的控制器有何区别？

matlab系统同时提供连续和离散的控制器和对象的目的是:在降低用户使用复杂程度的同时提高仿真精度.仿真速度和应用的广泛性. 仿真步长和求解精度的概念对于理解这个问题至关重要. 首先是步长,步长和求解精度存在一对矛盾,步长的选择是仿真消耗的时间和求解精度要求的折中.计算机只能一步一步计算你的电路或者其他方程,例如你输入一个连续的信号,计算机在一个时刻仅仅会采集这个信号上的一个点,然后把这个点带入你的控制器数学方程中,求出电路方程的一个解,根据这个解得到系统的输出.因此,仿真波形其实是一个个的点聚

通信算法之五：五种编码方式增益比较及matlab仿真验证

1. 卷积码增益性能.误码率 信道环境:AWGN 信噪比SNR :0:0.1:6 MATALB仿真架构:源比特 +卷积码 +BPSK +AWGN +Viterbi +BER 说明:卷积编码,不同的R码率,不同的约束长度 2. Turbo增益性能.误码率 信道环境:AWGN 信噪比SNR :-1:0.1:2 MATALB仿真架构:源比特 +Turbo编码 +BPSK +AWGN +Turbo译码+BER 说明:turbo译码,不同的译码算法那,不同的交织长度,不同的迭代次数. LTE 标准的tur

Matlab 仿真实现TI Instaspin 的Foc 逆Clarke变换和SVPWM

一直没搞明白TI 的Instaspin的SVPWM实现原理,最后只能在Matlab里仿真看看输出波形是不是和普通的SVPWM实现输出的波形一样,用M文件实现,下面是代码: clear all; theta = 0:1:360; vd = 0.0; vq = 1.15; Valpha = zeros(size(theta)); Vbeta = zeros(size(theta)); Vx = zeros(size(theta)); Vy = zeros(size(theta)); Vz = zer

matlab仿真随机数的产生

概率论和数理统计实验(matlab中实现) 一.伯努利分布 R=binornd(N,P); //N,P为二次分布的俩个参数,返回服从参数为N,P的二项分布的随机数,且N,P,R的形式相同. R=binornd(N,P,m); //m是一个1*2向量,它为指定的随机数的个数,其中N,P分别代表返回值R中行与列的维数: R=binornd(N,P,m,n); //m,n分别表示R的行数与列数: 例:一个射击手进行射击比赛,假设每枪射击命中率为0.45,每枪射击10次,共进行10万轮,就可以用matl

对AM信号FFT的matlab仿真

普通调幅波AM的频谱,大信号包络检波频谱分析 u(t)=Ucm(1+macos ?t)cos ?ct ma称为调幅系数 它的频谱由载波,上下边频组成 , 包络检波中二极管截去负半周再用电容低通滤波,可以得到基带信号,那么,截去负半周后的AM信号必定包含基带信号的频谱.我们可以通过matlab来验证. %已知基带信号为1hz,载波为64hz,调制系数ma=0.3,采样频率1024hz,FFT变换区间N为2048 clear; fs=1024; f=1; %1hz基带信号 fc=64; %64hz载