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

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

  1. 车辆运动控制算法——MPC

    MPC是模型预测控制算法,在车辆运动跟踪轨迹的控制中发挥很大的优势 基础的不多说,下面记录我对LQR/MPC/二次规划问题的理解 我们从LQR来引出MPC LQR的能量函数,目的是求函数J最小,即用最 ...

  2. 经典功率谱估计及Matlab仿真

    原文出自:http://www.cnblogs.com/jacklu/p/5140913.html 功率谱估计在分析平稳各态遍历随机信号频率成分领域被广泛使用,并且已被成功应用到雷达信号处理.故障诊断 ...

  3. MATLAB仿真总结

    MATLAB仿真过程中,编写MATLAB代码的时候犯了很多错误,做了很多蠢事.记录下自己犯错的点点滴滴,并引以为戒.使用MATLAB版本为2014a,以下内容如有不当还请指正. 1. 仿真开始前清理工 ...

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

    根据老师的安排,对于极化码的了解从仿真开始. 仿真的手段有很多种.可以利用C,C++,matlab等进行仿真的实现.其中matlab由于具有强大的函数库,和壮观的矩阵运算能力,被(我们老师课题组)看中 ...

  5. (转) 经典功率谱估计及Matlab仿真

    原文出自:http://www.cnblogs.com/jacklu/p/5140913.html 功率谱估计在分析平稳各态遍历随机信号频率成分领域被广泛使用,并且已被成功应用到雷达信号处理.故障诊断 ...

  6. OFDM通信系统的MATLAB仿真(1)

    由于是第一篇博客,想先说点废话,其实自己早就想把学到的一些东西总结成文章随笔之类的供自己复习时查看的了.但是一是觉得自己学的的不够深入,总结也写不出什么很深刻的东西:二是觉得网上也有海量的资料了,需要 ...

  7. OFDM通信系统的MATLAB仿真(2)

    关于OFDM系统的MATLAB仿真实现的第二篇随笔,在第一篇中,我们讨论的是信号经过AWGN信道的情况,只用添加固定噪声功率的高斯白噪声就好了.但在实际无线信道中,信道干扰常常是加性噪声.多径衰落的结 ...

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

    一直没搞明白TI 的Instaspin的SVPWM实现原理,最后只能在Matlab里仿真看看输出波形是不是和普通的SVPWM实现输出的波形一样,用M文件实现,下面是代码: clear all; the ...

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

    matlab系统同时提供连续和离散的控制器和对象的目的是:在降低用户使用复杂程度的同时提高仿真精度.仿真速度和应用的广泛性. 仿真步长和求解精度的概念对于理解这个问题至关重要. 首先是步长,步长和求解 ...

随机推荐

  1. EL表达式简单总结

    EL表达式 ## EL表达式的取值范围 JSP的四个作用域: pagecontext(生命周期用户离开或者跳转页面,作用域范围这个页面) request(生命周期用户离开页面,作用于这个页面) ses ...

  2. c#操作sqlite db3数据库

    首先添加引用 System.Data.SQLite.dll,引用只用添加这个,但SQLite.Interop.dll文件必须也和它同时放在Debug目录下 然后可用: SQLiteConnection ...

  3. HDU-1260_Tickets

    Tickets Time Limit: 2000/1000 MS (Java/Others) Memory Limit: 65536/32768 K (Java/Others) Problem Des ...

  4. mysql 开篇

    Mysql 教程 mysql是最流行的关系型数据库管理系统,在wab应用方面mysql是最好的RDBMS(Relational Database Manangement System: 关系数据库管理 ...

  5. OpenStack组件系列☞horizon搭建

    第一步:部署horizon环境: 安装部署memcache 安装软件包 yum install memcached python-memcached 启动memcache并且设置开机自启动 syste ...

  6. j2se--异常机制

    版权声明:本文为博主原创文章,未经博主同意不得转载. https://blog.csdn.net/quwenzhe/article/details/35610853   java异常机制中主要包含一个 ...

  7. js this详解

    This的定义: 它代表函数运行时,自动生成的一个内部对象,只能在函数内部使用. this的指向在函数定义的时候是确定不了的,只有函数执行的时候才能确定this到底指向谁,实际上this的最终指向的是 ...

  8. @雅礼集训01/13 - T1@ union

    目录 @description@ @solution@ @part - 1@ @part - 2@ @part - 3@ @accepted code@ @details@ @description@ ...

  9. 当flex遇到white-space: nowrap; 排版就飞了的神奇问题 吐血

    在做一个移动端排班的时候需要某一行的字超出的部分用省略号代替  然后写着发现后面排版乱了 HTML结构如下  我想让第二个span的内容加省略号 css正确的代码如下 .list { width: 1 ...

  10. angular安装应用

    首先你要有node 和npm 全局安装angular    npm install -g @angular/cli 安装一个angular项目     ng new 项目名称 cd进入新建的项目 跑页 ...