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