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
