UVW platform motion control algorithms and simulation matlab
Recently my co-workers because of the movement of a visual bit platform control algorithms have questions, so I have to ask.
Since I was first exposed to the UVW automatic alignment platform (also known as XXY automatic bit platforms), so look for some information to learn about, understand about the sport mode, the use of this platform matlab simulation, and verify the UVW motion control platform information provided correctness of the algorithm.
A, UVW Platform Introduction
1, which may be implemented in an arbitrary point on the plane as the center, rotational movement is performed, and translation along an arbitrary direction.
2, this platform CCD vision correction system, and are butted together, the correction can be done quickly with high precision work, repeated positioning accuracy is generally up to ± 1μm;
previous xyθ compared UVW internet platform, there are the following differences:
1, xyθ control accuracy is higher than the platform;
2, the UVW point on the plane can be arbitrarily internet rotational movement (including infinity) as the center; the internet due alone xyθ a motor rotation control, the rotation must be fixed somewhere in the center of the platform ([theta] at the motor connection), and must move together with the internet.
3, based on the difference between the second point, a need clearly UVW platform absolute coordinate system as a reference system, which makes sense rotational center; xyθ the internet must be movable platform with a coordinate system as a reference system, and control method of calculation It would be completely different.
UVW platform operating mode as shown below:
Second, the method of calculation
Calculation methods provided by the platform vendor, shots are as follows:
仔细研究一下上述的公式，很容易发现，这只是简单的几何运算以及对二维坐标的求解问题。
简单说明一下视觉对位和运动控制思路：
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