仔细研究一下上述的公式，很容易发现，这只是简单的几何运算以及对二维坐标的求解问题。简单说明一下视觉对位和运动控制思路：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
|