0 引言 移动机械臂由移动平台和固定于平台上的多自由度串联机械臂构成。相比传统基座固定式机械臂,移动机械臂有效地扩展了工作空间,在服务机器人领域得到了广泛的应用[1-2]。为了处理移动平台固有的运动非完整性约束,使移动机械臂能够快速而高效地完成指定任务,传统的移动机械臂研究工作多集中在运动规划[3-8]和协调控制[9-13]两个子领域。
以CCD(charge-coupled device)摄像机为代表的视觉传感器能够提供具有大量程、非接触式、测量速度快、精度适中等优点的视觉信息,将视觉信息引入到机器人控制闭环,可显著提高工作于非结构化环境中机器人的定位精度,并有效增强机器人对各类参数不确定因素和非参数不确定因素的鲁棒性。将视觉传感器集成到基座固定式机械臂上,则构成机械臂视觉伺服[14-16]系统,这类系统又可根据控制闭环中的反馈信号的类型细分为基于位置的视觉伺服(position-based visual servo,PBVS)[17]、基于图像的视觉伺服(image-based visual servo,IBVS)[18]和混合视觉伺服(hybrid visual servo,HYBVS)[19]系统;将视觉传感器集成到移动平台上,则构成移动机器人智能视觉导航系统[20-22],通过设计合适的跟踪控制器,可以有效地处理移动平台的动力学不确定参数和视觉系统的标定误差带来的影响,实现移动平台智能自主导航; 将视觉传感器集成到移动机械臂上是最新的研究趋势,由于移动平台具有较低的运动及自定位精度,鲜有成功的应用先例。LUCA A D建立了非完整约束移动机械臂的微分运动学模型,设计了基于图像的视觉控制器,并通过仿真验证了方案的可行性[23];MANSARD N对运动中的仿人机器人物品抓取问题进行了初步研究[24]; WANG Y设计了鲁棒轮式移动机械臂视觉伺服仿真系统,并提出了一种新颖的基于Q-learning算法的控制器来保证视觉特征在伺服过程中的可见性[25];杨唐文利用双目泛倾斜变焦摄像机,实现了移动双臂机械手系统的视觉伺服协调操作[26]。
为了对实际移动机械臂视觉伺服系统进行控制,本研究首先通过将移动机械臂转换为广义串联杆式机械臂进而进行运动学建模的方法,求得该系统的运动学解析逆解,然后针对手眼关系标定问题,提出一种全新的基于主动视觉的摄像机内外参数标定算法,最后设计包含眼注视逼近和开环look-then-doing抓取的切换控制策略。仿真及实际试验证明:新型手眼关系标定法易于实现,且具有极高的标定精度,同时切换控制律能够有效克服移动机械臂的非完整性约束带来的控制难度。
1 系统建模与参数标定 为了实现家庭环境中物品的抓取操作,将四自由度PowerCube机械臂的末端手爪上加装了Topspeed V80型摄像头并将机械臂安装在Grandar越野型履带机器人上以构成眼在手上构型(eye-in-hand)的视觉伺服系统。本研究将分别就此视觉伺服系统的正运动学建模、逆运动学解的求取和手眼标定问题展开分析。
1.1 服务机器人运动学建模 移动机器人有3个自由度,机械臂有4个自由度,只有两者互相配合,才能保证机械臂末端执行器的位姿0TE可控。将移动机器人抽象为旋转-平移-旋转的3自由度机械臂,同时固定4自由度机械臂的底座旋转自由度,整个视觉伺服系统就可被抽象为1个6自由度的扩展机械臂。其中除“关节2”为移动关节外,其他连杆均为转动关节。按照Denavit J和Hartenberg R S提出的方法[27],在每个连杆上固联一个坐标系,如图 1所示。
图 1
| | 图 1 移动机械臂及各连杆固联坐标系 Figure 1 Mobile manipulator and its link coordinate system |
表 1给出了各“连杆”的运动学参数,其中α,a,θ,d分别代表连杆的扭角、长度、转角和偏置。连杆类型中的R代表转动关节,其变量为转角θi,满足θi∈(-π，π)；T代表移动关节,其变量为偏置d，满足d＞0。
表 1
表 1 扩展机械臂D-H运动学参数 Table 1 D-H parameters of the generalized manipulator | 连杆 | α/(°) | a/cm | θ/(°) | d/cm | 类型 | | 1 | 0 | 0 | π/2+θ1 | 0 | R | | 2 | π/2 | 0 | 0 | 0+d | T | | 3 | -π/2 | 0 | -π/2+θ3 | 25.0 | R | | 4 | -π/2 | -6.5 | 0+θ4 | 0 | R | | 5 | π/2 | 6.5 | 0+θ5 | 28.5 | R | | 6 | π/2 | 0 | 0+θ6 | 0 | R | | E | -π/2 | 0 | 0 | 28.0 | |
| 表 1 扩展机械臂D-H运动学参数 Table 1 D-H parameters of the generalized manipulator |
根据连杆变换通式计算出各个连杆的变换矩阵,继而可求得末端执行器坐标系｛E｝相对基坐标系｛0｝的位姿0TE:
| 0TE=[nxoxaxpxnyoyaypynzozazpz0001], | (1) |
| nx=c13c4c5c6+c13s4s6−s13s5c6;ny=s13c4c5c6+s13s4s6+c13s5c6;nz=−s4c5c6+c4s6;ox=−c13c4s5−s13c5;oy=−s13c4s5+c13c5;oz=s4s5;ax=−c13c4c5s6+c13s4c6+s13s5s6;ay=−s13c4cs6+s13s4c6−c13s5s6;az=s4c5s6+c4c6;px=−28c13c4c5s6+28c13s4c6+6.5c13c4+28.5c13s4−6.5c13+28s13s5s6+dc1;py=−28s13c4c5s6+28s13s4c6+6.5s13c4+28.5s13s4−6.5s13−28c13s5s6+ds1;pz=28s4c5s6+28c4c6−6.5s4+28.5c4+25. | (2) |
式中,ci=cos(θi),si=sin(θi),cjk=cos(θi+θj),sjk=sin(θj+θj)。
1.2 服务机器人运动学逆解 运动学逆解是指根据末端执行器坐标系｛E｝的位姿0TE反求机器人各关节的位移。将式(1)两端分别左乘0T3-1把移动机器人变量θ1,d,θ3分离出来:
| ^3{T_E} = \left[ {\begin{array}{*{20}{c}}{{c_4}{c_5}{c_6} + {s_4}{s_6}}&{ - {c_4}{s_5}}&{ - {c_4}{c_5}{s_6} + {s_4}{c_6}}&{ - 28{c_4}{c_5}{s_6} + 28{s_4}{c_6} + 6.5{c_4} + 28.5{s_4} - 6.5}\\{{s_5}{c_6}}&{{c_5}}&{ - {s_5}{s_6}}&{ - 28{s_5}{s_6}}\\{ - {s_4}{c_5}{c_6}}&{{s_4}{s_5}}&{{s_4}{c_5}{s_6} + {c_4}{c_6}}&{28{s_4}{c_5}{s_6} + 28{c_4}{c_6} - 6.5{s_4} + 28.5{c_4}}\\0&0&0&1\end{array}} \right], | (3) |
| {}^{3}{{T}_{E}}=\left[\begin{matrix} {{c}_{13}}{{n}_{x}}+{{s}_{13}}{{n}_{y}} & {{c}_{13}}{{o}_{x}}+{{s}_{13}}{{o}_{y}} & {{c}_{13}}{{a}_{x}}+{{s}_{13}}{{a}_{y}} & {{c}_{13}}{{p}_{x}}+{{s}_{13}}{{p}_{y}}+d{{c}_{3}} \\ -{{s}_{13}}{{n}_{x}}+{{c}_{13}}{{n}_{y}} & -{{s}_{13}}{{o}_{x}}+{{c}_{13}}{{o}_{y}} & -{{s}_{13}}{{a}_{x}}+{{c}_{13}}{{a}_{y}} & -{{s}_{13}}{{p}_{x}}+{{c}_{13}}{{p}_{y}}+d{{s}_{3}} \\ {{n}_{z}} & {{o}_{z}} & {{a}_{z}} & {{p}_{z}}-25 \\ 0 & 0 & 0 & 1 \\\end{matrix} \right]. | (4) |
求得四组解析解:
| {{\theta }_{4}}=A\tan 2\left( 28.5,6.5 \right)+A\tan 2\left( \frac{28{{a}_{z}}-{{p}_{z}}+25}{\sqrt{{{6.5}^{2}}+{{28.5}^{2}}}},\pm \sqrt{1-\frac{{{\left( 28{{a}_{z}}-{{p}_{z}}+25 \right)}^{2}}}{{{6.5}^{2}}+{{28.5}^{2}}}} \right), | (5) |
| {{\theta }_{1}}+{{\theta }_{3}}=A\tan 2\left( \frac{-{{c}_{4}}{{o}_{z}}}{{{s}_{4}}\sqrt{{{o}_{x}}^{2}+{{o}_{y}}^{2}}},\pm \sqrt{1-\frac{{{\left( {{c}_{4}}{{o}_{z}} \right)}^{2}}}{s_{4}^{2}\left( {{o}_{x}}^{2}+{{o}_{y}}^{2} \right)}} \right)-A\tan 2\left( {{o}_{x}},{{o}_{y}} \right), | (6) |
| {{\theta }_{5}}=A\tan 2\left( \frac{{{o}_{z}}}{{{s}_{4}}},-{{s}_{13}}{{o}_{x}}+{{c}_{13}}{{o}_{y}} \right), | (7) |
| {{\theta }_{6}}=A\tan 2\left( \frac{{{s}_{13}}{{a}_{x}}-{{c}_{13}}{{a}_{y}}}{{{s}_{5}}},\frac{\left( -{{s}_{13}}{{n}_{x}}+{{c}_{13}}{{n}_{y}} \right)}{{{s}_{5}}} \right) | (8) |
| d={{\left\{ \begin{align} & {{\left( {{c}_{13}}{{p}_{x}}+{{s}_{13}}{{p}_{y}}+28{{c}_{4}}{{c}_{5}}{{s}_{6}}-28{{s}_{4}}{{c}_{6}}-6.5{{c}_{4}}-28.5{{s}_{4}}+6.5 \right)}^{2}} \\ & +{{\left( -28.5{{s}_{5}}{{s}_{6}}+{{s}_{13}}{{p}_{x}}+{{c}_{13}}{{p}_{y}} \right)}^{2}} \\ \end{align} \right\}}^{1/2}}, | (9) |
| {{\theta }_{3}}=A\tan 2\left( \begin{align} & -28.5{{s}_{5}}{{s}_{6}}+{{s}_{13}}{{p}_{x}}+{{c}_{13}}{{p}_{y}},{{c}_{13}}{{p}_{x}}+{{s}_{13}}{{p}_{y}} \\ & +28{{c}_{4}}{{c}_{5}}{{s}_{6}}-28{{s}_{4}}{{c}_{6}}-6.5{{c}_{4}}-28.5{{s}_{4}}+6.5 \\ \end{align} \right). | (10) |
表 2所示为应用逆解解析式(5)～(10)由末端执行器位姿0TE求得的扩展机械臂各关节角度的解,其中末端执行器姿态由RPY角描述,软件采用Matlab R2007a,扩展机械臂的建模由Peter I. Corke编写的Robotics Toolbox for Matlab R7.1实现。
求得全部四组解析解后,根据表 1中各关节的取值范围可确定出物理可实现解,若此时还存在多组物理可实现解,则按照“最短行程”准则确定出最优解。
表 2
表 2 扩展机械臂运动学逆解结果 Table 2 Inverse kinematic results of the generalized manipulator | 组别 | 翻滚/(°) | 俯仰/(°) | 偏转/(°) | px/cm | py/cm | pz/cm | θ1/(°) | d/cm | θ3/(°) | θ4/(°) | θ5/(°) | θ6/(°) | | 1 | 25.491 | 14.702 | -71.425 | 129.412 | 15.709 | 76.797 | 13.853 | 122.6000 | -45.014 | 7.830 | -36.010 | -22.507 | | 11.016 | 123.7487 | 66.364 | 7.830 | -144.047 | -35.202 | | 5.044 | 127.4757 | -78.927 | -33.534 | 8.333 | -62.105 | | 20.099 | 121.3515 | 99.887 | -33.534 | 171.724 | 4.396 | | 2 | -13.814 | -26.453 | -12.983 | 12.593 | 22.455 | 69.223 | 58.477 | 19.3710 | -58.627 | 34.314 | -13.916 | 62.191 | | 15.251 | 40.1966 | 141.526 | 34.314 | -166.142 | -4.860 | | 9.474 | 52.6000 | -25.723 | -60.019 | 9.004 | -31.047 | | 97.467 | 20.1375 | 75.288 | -60.019 | 171.054 | 88.378 |
| 表 2 扩展机械臂运动学逆解结果 Table 2 Inverse kinematic results of the generalized manipulator |
1.3 服务机器人手眼关系标定 首先打印一张边长为l的黑色正方形图案并将其布置到物品的表面组成简单的平面模板,然后以手动方式随机地改变平面模板在摄像机坐标系中的位姿并对每种位姿采集一幅图像,共采集n幅图像(n>2),根据张正友提出的经典平面模板标定法标定出摄像机的内参矩阵K。确定K后,即可根据平面模板的度量结构和它的像计算出平面模板固联坐标系｛o｝和摄像机坐标系｛c｝间的位姿关系cTo。
机械臂末端执行器在初始位置｛E｝处观测平面模板,记此时摄像机坐标系为｛c｝,求出模板坐标系｛o｝相对摄像机坐标系｛c｝的位姿[cRo|cto],然后令机械臂作已知运动使末端执行器到达位置｛E*｝处,求出[c*Ro|c*to],根据图 2所示的齐次变换链,有:
图 2
| | 图 2 摄像机外参数标定原理图 Figure 2 Schematic of camera extrinsic parameters calibration |
| {}^{3}{{T}_{E}}\centerdot {}^{c}{{T}_{o}}={}^{E}{{T}_{{{E}^{*}}}}\centerdot {}^{E}{{T}_{c}}\centerdot {}^{{}^{c*}}{{T}_{o}}. | (11) |
因齐次变换矩阵可逆,式(11)两端分别左乘ETE*-1,右乘c*T-1o,并记:
| {}^{E*}{{T}_{E}}={}^{E}{{T}^{-1}}_{{{E}^{*}}}=\left[\begin{array}{*{35}{l}} {{R}_{A}} & {{t}_{A}} \\ {{0}^{T}} & 1 \\\end{array} \right], | (12) |
| {}^{E}{{T}_{c}}=\left[\begin{array}{*{35}{l}} R & t \\ {{0}^{T}} & 1 \\\end{array} \right], | (13) |
| {}^{c}{{T}_{c*}}={}^{c}{{T}_{o}}\centerdot {}^{c*}{{T}^{-1}}_{o}=\left[\begin{array}{*{35}{l}} {{R}_{B}} & {{t}_{B}} \\ {{0}^{T}} & 1 \\\end{array} \right]. | (14) |
展开,得方程:
| {{R}_{A}}\centerdot R\centerdot {{R}_{B}}=R, | (15) |
| {{R}_{A}}\centerdot R\centerdot {{t}_{B}}+{{R}_{A}}\centerdot t+{{t}_{A}}=t, | (16) |
只要求出R,则t就可求得,因此问题归结为对R的求取。
对于形如AXB=X的方程(15),X∈Rm×n,可展成张量分量的形式:
| a_{k}^{i}x_{l}^{k}b_{j}^{i}=x_{j}^{i}, | (17) |
其中,i,j为自由指标; k,l为哑指标,遵循爱因斯坦求和约定; xji表示矩阵X的第i行,第j列的元素。引入Kronecker δ:
| \delta _{\alpha }^{\beta }=\left\{ \begin{array}{*{35}{l}} 1,& 当\alpha =\beta ; \\ 0,& 当\alpha \ne \beta . \\\end{array} \right. | (18) |
则有:
| \left( a_{k}^{i}b_{j}^{i}-\delta _{k}^{i}\delta _{j}^{l} \right)x_{l}^{k}=0. | (19) |
根据i,j的不同取值,式(19)共i×j个方程,记x=[x1T,…,xmT]T,xiT为矩阵X的第i行行向量,则式(15)可进一步写为如下的线性方程组
其中,H∈R(i×j)×(i×j),且H的分量
| h_{\left( k-1 \right)\centerdot n+l}^{\left( i-1 \right)\centerdot n+j}=a_{k}^{i}b_{j}^{i}-\delta _{k}^{i}\delta _{j}^{l}. | (21) |
将式(15)展开为式(20)的形式,即H1x=0。此时还不足以确定R的唯一解,事实上,此时得到的H1满足rank(H1)=6。再取任易两种姿态观察目标人工地标,仿照式(12)~(16)写出另一组约束方程:
| {{R}_{A'}}\centerdot R\centerdot {{R}_{B'}}=R, | (22) |
| {{R}_{A'}}\centerdot R\centerdot {{t}_{B'}}+{{R}_{A'}}\centerdot t+{{t}_{A'}}=t. | (23) |
将R的约束方程式(22)展成式(20)的形式,即H2x=0,则:
| \left[\frac{{{H}_{1}}}{{{H}_{2}}} \right]x=Hx=0, | (24) |
此时H一般能满足rank(H)=9,x在‖x‖2=1的约束下的最小二乘解为HTH的最小特征值对应的单位特征向量。一个旋转矩阵的F-范数为\sqrt{3},而求得的x写成的旋转矩阵:
| R'=\left[\begin{array}{*{35}{l}} x_{1}^{1} & x_{2}^{1} & x_{3}^{1} \\ x_{1}^{2} & x_{2}^{2} & x_{3}^{3} \\ x_{1}^{3} & x_{2}^{3} & x_{3}^{3} \\\end{array} \right], |
其F-范数‖R′‖F=‖x‖2=1,因此式(15)的R取为
式(25)所求得的两个R中有且仅有一个满足右手定则r3=r1×r2,因此R被唯一确定。求得R后,将其代入式(16)与(23),则t也被唯一确定:
| \left[\frac{I-{{R}_{A}}}{I-{{R}_{A'}}} \right]t=\left[\frac{{{R}_{A}}\centerdot R\centerdot {{t}_{B}}+{{t}_{A}}}{{{R}_{A'}}\centerdot R\centerdot {{t}_{B'}}+{{t}_{A'}}} \right]. | (26) |
2 视觉伺服切换控制律设计 当机器人距离待操作物品较远时,视觉定位精度较低,直接采用look then doing开环方式控制服务机器人抓取极易导致失败。因此设计了一种切换控制律,首先控制服务机器人眼注视逼近目标物品,逼近到期望距离后再切换到look then doing开环方式来完成物品的抓取。
2.1 眼注视逼近 图3为眼注视逼近控制律的结构图。
图 3
| | 图 3 眼注视逼近控制律示意图 Figure 3 Schematic of eye-fixed approaching control law |
选择人工物标的共心圆环圆心ω0的非齐次射影坐标cm和共心圆外圆围成区域的零阶矩m00作为视觉特征集S1=cm,S2=m00,记
| {{L}_{{{c}_{m}}}}=\left[\begin{array}{*{35}{l}} -\frac{1}{{}^{c}Z} & 0 & \frac{x}{{}^{c}Z} & xy & -\left( 1+{{x}^{2}} \right) & y \\ 0 & -\frac{1}{{}^{c}Z} & \frac{y}{{}^{c}Z} & 1+{{y}^{2}} & -xy & -x \\\end{array} \right]\left[\begin{array}{*{35}{l}} {}^{E}R_{c}^{T} & -{}^{E}R_{c}^{T}{{\left[{}^{E}{{t}_{c}} \right]}_{\times }} \\ 0 & {}^{E}R_{c}^{T} \\\end{array} \right]{}^{E}J\left( q \right), | (27) |
则S1的微分与各关节速度间的关系:
| {{{\dot{S}}}_{1}}={{L}_{{{c}_{m}}}}\dot{q}. | (28) |
Lcm中存在未知量cZ,其可由当前图像与期望图像中外圆围成区域的零阶矩近似地估计出来:
| {}^{c}\hat{Z}={}^{c*}Z\sqrt{\frac{m_{00}^{*}}{{{m}_{00}}}}. | (29) |
考虑到机器人前3个自由度无法联动,因此取消第3个自由度:
| {}^{c}\dot{m}={{L}_{{{c}_{m}}}}diag\left[\begin{array}{*{35}{l}} 1 & 1 & 0 & 1 & 1 & 1 \\\end{array} \right]\dot{q}, | (30) |
将机器人前向运动与其他运动分离:
| {}^{c}\dot{m}={{L}_{{{c}_{m}}}}diag\left[\begin{array}{*{35}{l}} 0 & 1 & 0 & 0 & 0 & 0 \\\end{array} \right]\dot{q}+{{L}_{{{c}_{m}}}}diag\left[\begin{array}{*{35}{l}} 1 & 0 & 0 & 1 & 1 & 1 \\\end{array} \right]\dot{q}, | (31) |
记
| {{L}_{d}}={{L}_{{{c}_{m}}}}diag\left[\begin{array}{*{35}{l}} 0 & 1 & 0 & 0 & 0 & 0 \\\end{array} \right], | (32) |
| {{L}_{{{d}^{\bot }}}}={{L}_{{{c}_{m}}}}diag\left[\begin{array}{*{35}{l}} 1 & 0 & 0 & 1 & 1 & 1 \\\end{array} \right], | (33) |
构造任务函数
| e={{s}_{1}}-s_{1}^{*}, | (34) |
令其指数收敛\dot{e}=-\lambda e,有
| -\lambda e={{L}_{d}}{{{\dot{q}}}_{d}}+{{L}_{{{d}^{\bot }}}}{{{\dot{q}}}_{{{d}^{\bot }}}}. | (35) |
则:
| {{{\dot{q}}}_{d}}={{\left[\begin{array}{*{35}{l}} 0 & -{{\lambda }_{1}}sgn \left( {{m}_{00}}-m_{00}^{*} \right) & 0 & 0 & 0 & 0 \\\end{array} \right]}^{T}}, | (36) |
| {{{\dot{q}}}_{{{d}^{\bot }}}}=-L_{{{d}_{\bot }}}^{+}\left( {{\lambda }_{2}}e+{{L}_{d}}{{{\dot{q}}}_{d}} \right), | (37) |
式(36)中sgn(·)为取符号操作。在式(36)的控制下服务机器人以定常的速度逼近目标物品。式(37)中{{L}_{d}}{{{\dot{q}}}_{d}}为服务机器人的前向逼近引起的圆心射影坐标的变化,在式(37)的控制下,服务机器人在逼近过程中始终眼注视目标物品。
2.2 Look-then-doing开环抓取 服务机器人逼近到期望距离后,控制律切换为look then doing开环抓取方式。如图 4所示,机械臂首先作已知运动ETE′使摄像机由当前坐标系｛c｝运动到坐标系｛c′｝:
图 4
| | 图 4 look then doing开环抓取原理图 Figure 4 Schematic of look then doing open-loop handling |
| {}^{c'}{{T}_{c}}=\left[\begin{array}{*{35}{l}} R' & t' \\ {{0}^{T}} & 1 \\\end{array} \right]={}^{E}{{T}^{-1}}_{c}{}^{E}{{T}^{-1}}_{E'}{}^{E}{{T}_{c}}. | (38) |
通过已知运动ETE′和运动前后两视图的单应来确定景物平面在｛c′｝中的3D参数,然后通过景物平面3D参数和运动后视图与期望视图的单应来确定坐标系｛c′｝与期望摄像机坐标系｛c*｝的相对位姿{}^{{}^{c*}}{{T}_{o'}}=\left[\begin{array}{*{35}{l}} R & t \\ {{0}^{T}} & 1 \\\end{array} \right].。
下面分两部分来求c*Tc′。
2.2.1 计算景物平面π在｛c′｝中的3D结构 记π在｛c｝中为cπ=[cnT,cd]T,则两视图对应点的齐次射影坐标间满足:
| {}^{c'}m\cong \left( R'-\frac{t'\centerdot {}^{c}{{n}^{T}}}{{}^{c}d} \right){}^{c}m\cong H{{'}^{c}}m, | (39) |
式(39)中的代表等式两端相差一个尺度因子,H′为景物平面诱导的单应,只要已知最少4组景物平面对应点c′micmi,i=1…4,即可利用直接线性变换法(direct linear transformation,DLT)求得满足‖H′‖F=1的H′。
H'=\left[\begin{array}{*{35}{l}} h_{1}^{'} & h_{2}^{'} & h_{3}^{'} \\\end{array} \right],R'=\left[\begin{array}{*{35}{l}} {{r}_{1}} & {{r}_{2}} & {{r}_{3}} \\\end{array} \right],{}^{c}n={{\left[\begin{array}{*{35}{l}} {{n}_{x}} & {{n}_{y}} & {{n}_{z}} \\\end{array} \right]}^{T}},式(39)可被写为:
| \lambda h_{1}^{'}={{r}_{1}}-\frac{{{n}_{x}}}{{}^{c}d}t', | (40) |
| \lambda h_{2}^{'}={{r}_{2}}-\frac{{{n}_{y}}}{{}^{c}d}t', | (41) |
| \lambda h3={{r}_{3}}-\frac{{{n}_{z}}}{{}^{c}d}t', | (42) |
式(40)～(42)分别左乘rT1,rT2,rT3,有:
| \left[\begin{array}{*{35}{l}} r_{1}^{T}h_{1}^{'} & r_{1}^{T}t' & 0 & 0 \\ r_{2}^{T}h_{2}^{'} & 0 & r_{2}^{T}t' & 0 \\ r_{3}^{T}h_{3}^{'} & 0 & 0 & r_{3}^{T}t' \\ r_{2}^{T}h_{1}^{'} & r_{2}^{T}t' & 0 & 0 \\ r_{3}^{T}h_{1}^{'} & r_{3}^{T}t' & 0 & 0 \\ r_{1}^{T}h_{2}^{'} & 0 & r_{1}^{T}t' & 0 \\ r_{3}^{T}h_{2}^{'} & 0 & r_{3}^{T}t' & 0 \\ r_{1}^{T}h_{3}^{'} & 0 & 0 & r_{1}^{T}t' \\ r_{2}^{T}h_{3}^{'} & 0 & 0 & r_{2}^{T}t' \\\end{array} \right]\left[\begin{array}{*{35}{l}} \lambda \\ \frac{{{n}_{x}}}{{}^{c}d} \\ \frac{{{n}_{y}}}{{}^{c}d} \\ \frac{{{n}_{z}}}{{}^{c}d} \\\end{array} \right]=\left[\begin{array}{*{35}{l}} 1 \\ 1 \\ 1 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\\end{array} \right] | (43) |
显然,通过解线性方程组(43),可求出以｛c｝为参考坐标系的未知平面π,记π在｛c′｝中为c′{}^{c'}\pi ={{\left[{}^{c'}{{n}^{T}}{}^{c'}d \right]}^{T}},则有:
| {}^{c'}n=R{{'}^{c}}n,{}^{c'}d={}^{c}d-{}^{c'}{{n}^{T}}t'. | (44) |
2.2.2 计算｛c′｝与｛c*｝间的相对位姿c*Tc′ 求得c′π后,｛c′｝与｛c*｝的视图对应点的齐次射影坐标间满足:
| {}^{d}m\cong \left( R'-\frac{t\centerdot {}^{c'}{{n}^{T}}}{{}^{c'}d} \right){}^{c'}m\cong {{H}^{c'}}m. | (45) |
类似的,满足‖H‖F=1条件的H可利用DLT算法由至少4组点对应{}^{d}{{m}_{i}}\leftrightarrow {}^{c'}{{m}_{i}},i=1,\cdots ,4求出。结合H的性质det(H)>0,有α>0使得下式成立:
| \alpha H=R-\frac{t\centerdot {}^{c'}{{n}^{T}}}{{}^{c'}d}. | (46) |
记矢量c′n的反对称矩阵为[c′n]×,则根据式(46)一定有:
| \alpha {{\left[{}^{c'}n \right]}_{\times }}{{H}^{T}}={{\left[{}^{c'}n \right]}_{\times }}{{R}^{T}}, | (47) |
记Rk,θ为以单位矢量k为转轴旋转θ形成的旋转矩阵,则根据旋转变换通式,有:
| {{R}_{K,\theta }}=\cos \theta {{I}_{3\times 3}}+\sin \theta {{\left[k \right]}_{\times }}+\left( 1-\cos \theta \right)k{{k}^{T}}, | (48) |
其中I3×3为3阶单位阵。记{{e}_{3}}={{\left[\begin{array}{*{35}{l}} 0 & 0 & 1 \\\end{array} \right]}^{T}},选择转轴和转角:
| {{k}_{1}}=\frac{{}^{c'}n\times {{e}_{3}}}{{{\left\| {}^{c'}n\times {{e}_{3}} \right\|}_{2}}}, | (49) |
| {{\theta }_{1}}=\arccos {}^{c'}n\times {{e}_{3}}, | (50) |
按照式(48)构造旋转矩阵Rk1,θ1,一定有
| R_{{{k}_{1}},{{\theta }_{1}}}^{c'}n={{e}_{3}}. | (51) |
代入式(47),并根据Nanson公式[At]×A=A-T[t]×,有
| \alpha \left[\begin{array}{*{35}{l}} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \\\end{array} \right]{{R}_{{{k}_{1}},{{\theta }_{1}}}}{{H}^{T}}=\left[\begin{array}{*{35}{l}} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \\\end{array} \right]{{R}_{{{k}_{1}},{{\theta }_{1}}}}{{R}^{T}}, | (52) |
式(52)意味着αRk1,θ1 HT和Rk1,θ1RT的前两行相等,且αRk1,θ1 HT的前两行为正交单位向量,记
| {{R}_{{{k}_{1}},{{\theta }_{1}}}}{{H}^{T}}=\left[\begin{array}{*{35}{l}} r_{1}^{T} \\ r_{2}^{T} \\ r_{3}^{T} \\\end{array} \right]. | (53) |
尺度因子α和旋转矩阵R可以确定:
| \alpha =\frac{2}{\left\| {{r}_{1}} \right\|\left\| {}_{2}+ \right\|{{\left\| {{r}_{2}} \right\|}_{2}}}, | (54) |
| R=\left[\begin{array}{*{35}{l}} \alpha {{r}_{1}} & \alpha {{r}_{2}} & {{\alpha }^{2}}\left( {{r}_{1}}\times {{r}_{2}} \right) \\\end{array} \right]{{R}_{{{k}_{1}},{{\theta }_{1}}}}. | (55) |
将式(54)与(55)代入式(46),则有
| t={}^{c'}d\left( R-\alpha H \right){}^{c'}n. | (56) |
至此,求得了坐标系｛c′｝与期望摄像机坐标系｛c*｝的相对位姿c*Tc′,期望末端执行器坐标系｛E*｝相对基坐标系｛0｝的位姿
| {}^{0}{{T}_{E}}={}^{0}{{T}_{E'}}{}^{E}{{T}_{c}}{}^{c*}{{T}^{-1}}_{c'}{}^{E}{{T}^{-1}}_{c}. | (57) |
利用1.2节的运动学逆解公式求得各关节期望角度,即可最终控制服务机器人完成物品的开环抓取。
3 试验与分析 3.1 手眼关系标定试验 令移动机械臂的移动平台静止,机械臂随机作6组运动,并在每组运动后采集2D标定物的图像。所采集图像及图像角点提取如图 5所示。
图 5
| | 图 5 2D标定物角点提取图像 Figure 5 Corner extraction images of 2D chessboard |
表 3所示为对应6幅图像的机械臂的已知运动,由此可进一步求得末端执行器坐标系｛E｝相对机械臂基座标系｛0｝的位姿0TE,同时利用世界平面的单应分解可以求得摄像机坐标系｛c｝相对2D标定物上固联坐标系｛o｝的相对位姿cTo。根据图像1和图像2(记为｛12｝),可由1.3节提出的方法确定出H1∈R9×9,类似地根据｛13｝、｛14｝、｛15｝、｛16｝可确定出H2,H3,H4,H5。最终求得手眼关系
| {}^{E}{{T}_{c}}=\left[\begin{array}{*{35}{l}} 0.0644 & 0.9090 & 0.44117 & -10.4233 \\ -0.9879 & 0.1164 & -0.1023 & -6.1634 \\ -0.1409 & -0.4002 & 0.99055 & -17.2431 \\ 0 & 0 & 0 & 1 \\\end{array} \right]. |
表 3
表 3 广义机械臂已知运动 Table 3 Known movements of the generalized manipulator | 图像 | θ1,θ3/(°) | d2/cm | θ4/(°) | θ5/(°) | θ6/(°) | | 1 | 0 | 0 | 32.979 | -1.404 | -64.406 | | 2 | 0 | 0 | 32.664 | -12.743 | -62.928 | | 3 | 0 | 0 | 26.127 | -12.926 | -74.083 | | 4 | 0 | 0 | 12.278 | 2.063 | -88.064 | | 5 | 0 | 0 | -0.069 | 7.460 | -93.100 | | 6 | 0 | 0 | -22.769 | -10.726 | -110.443 |
| 表 3 广义机械臂已知运动 Table 3 Known movements of the generalized manipulator |
3.2 切换控制实验 图 6所示为期望图像和当前图像,以及由hough变换检测得到的同心椭圆。其中,蓝色同心圆环区域利用高斯模型进行建模和分割。
图 6
| | 图 6 期望图像和当前图像 Figure 6 Desired image and current image |
选择α=0.5,眼注视逼近控制律式(36)中的增益λ1=0.01,控制律式(37)中的增益λ=0.15,c*z=25cm,眼注视逼近控制输入如图 7所示。
图 7
| | 图 7 移动机械臂控制输入 Figure 7 Control inputs of the mobile manipulator |
在图 7所示控制输入作用下,移动机械臂的摄像机逼近到坐标系｛c｝,所采集图像如图 8(a)所示。
图 8
| | 图 8 已知运动前后摄像机采集图像 Figure 8 Images corresponding coordinate ｛c｝ and ｛c′｝ |
由图 6(b)和图 8(a)可以看出:眼注视逼近控制律成功控制移动机械臂逼近目标,且在目标逼近过程中始终将其保持在摄像机视野的中心位置,从而有效避免了伺服失败。
眼注视逼近结束后,令移动平台静止,机械臂作已知运动使摄像机由｛c｝运动到｛c′｝,在｛c′｝处采集到的图像如图 8(b)所示。根据2.2.1节方法求得标签所在景物平面以｛c′｝为参考坐标系描述的3D信息{}^{c'}\pi ={{\left[\begin{array}{*{35}{l}} -0.2428 & 0.2620 & -0.9340 & 15.7297 \\\end{array} \right]}^{T}}。
求得景物平面3D结构信息后,由图 8(b)所示｛c′｝处图像和图 6(a)所示期望图像可根据2.2.2节方法求得c′Tc*,进而求得0TE*,然后利用1.2节的运动学逆解方法求出look-then-doing模式下的移动机械臂控制量q={{\left[\begin{array}{*{35}{l}} -0.18457 & 5.3829 & 0.1237 & 1.3305 & -0.0058 & -0.1220 \\\end{array} \right]}^{T}},最终实现物品的抓取操作。显然,look-then-doing开环抓取方法由于引入了基于主动视觉的空间景物平面3D结构参数,使得单应矩阵的计算和分解避免了矩阵奇异值分解运算和唯一解的确定,大大提高了运算效率。
4 结论 通过将移动机械臂等效为广义串联杆式机械臂的方法建立移动机械臂的微分运动学模型,并通过变量分离法进一步推导出该广义机械臂的运动学解析逆解;以此为基础,利用主动视觉方法求出移动机械臂的手眼标定关系,最后针对移动机械臂的运动非完整性约束,提出先眼注视逼近目标,然后基于位置的look-then-doing开环视觉伺服抓取的切换控制律。结果表明该方案能够简单高效地标定出移动机械臂手眼关系,同时切换控制律能够控制移动机械臂较好地完成家庭物品的抓取和传递任务。眼注视逼近算法依赖目标物中同心圆环的检测,同时look-then-doing开环抓取算法依赖目标物中二维码的检测,当移动机械臂逼近目标物后检测到的二维码过于偏斜时,成功率不高,这些问题将在以后的研究中改进。由于不变矩特征在目标识别中有良好效果,下一步研究利用矩特征进行目标识别与跟踪的方法,减轻该算法对人工物标的过度依赖。
