文章快速检索 高级检索
 山东大学学报(工学版)  2016, Vol. 46 Issue (5): 54-63  DOI: 10.6040/j.issn.1672-3961.0.2016.262 0

### 引用本文

YANG Yuanhui, LI Guodong, WU Chunfu, WANG Xiaolong, CAI Xiaowei. Hand-eye calibration and visual servo control for mobile manipulator[J]. JOURNAL OF SHANDONG UNIVERSITY (ENGINEERING SCIENCE), 2016, 46(5): 54-63. DOI: 10.6040/j.issn.1672-3961.0.2016.262.

### 文章历史

Hand-eye calibration and visual servo control for mobile manipulator
YANG Yuanhui, LI Guodong, WU Chunfu, WANG Xiaolong, CAI Xiaowei
School of Mechanical and Electrical Engineering, Longyan University, Longyan 364012, Fujian, China
Abstract: The eye-in-hand visual servo system integrated vision sensor into mobile manipulator was widely used for object grasping and delivering operation. However, there were some challenges, such as nonlinearity and complexity of the existing hand-eye calibration algorithm, together with the nonholonomic constraint of the mobile manipulator. To solve these problems, the mobile manipulator was proceeded as a generalized manipulator whose kinematic model was established, and the inverse kinematic analytical solutions were acquired. Then, a novel linear active vision based camera calibration method was proposed to determine the hand-eye relationship. Finally, a visual servo switching control law composed of eye-fixed approaching and look-then-doing open loop handling was designed to control the mobile manipulator to finish household objects handling operation. Simulation and experimental results showed that the linear active vision-based hand-eye calibration method could be utilized conveniently and precisely. Meanwhile, the designed control law could reduce the control difficulty brought by the nonholonomic constraint of the mobile manipulator.
Key words: visual servo    hand-eye calibration    mobile manipulator    switch control    homography decomposition    active vision
0 引言

1 系统建模与参数标定

1.1 服务机器人运动学建模

 图 1 移动机械臂及各连杆固联坐标系 Figure 1 Mobile manipulator and its link coordinate system

 ${}^{0}{{T}_{E}}=\left[\begin{array}{*{35}{l}} {{n}_{x}} & {{o}_{x}} & {{a}_{x}} & {{p}_{x}} \\ {{n}_{y}} & {{o}_{y}} & {{a}_{y}} & {{p}_{y}} \\ {{n}_{z}} & {{o}_{z}} & {{a}_{z}} & {{p}_{z}} \\ 0 & 0 & 0 & 1 \\ \end{array} \right],$ (1)
 \begin{align} & {{n}_{x}}={{c}_{13}}{{c}_{4}}{{c}_{5}}{{c}_{6}}+{{c}_{13}}{{s}_{4}}{{s}_{6}}-{{s}_{13}}{{s}_{5}}{{c}_{6}};{{n}_{y}}={{s}_{13}}{{c}_{4}}{{c}_{5}}{{c}_{6}}+{{s}_{13}}{{s}_{4}}{{s}_{6}}+{{c}_{13}}{{s}_{5}}{{c}_{6}}; \\ & {{n}_{z}}=-{{s}_{4}}{{c}_{5}}{{c}_{6}}+{{c}_{4}}{{s}_{6}};{{o}_{x}}=-{{c}_{13}}{{c}_{4}}{{s}_{5}}-{{s}_{13}}{{c}_{5}}; \\ & {{o}_{y}}=-{{s}_{13}}{{c}_{4}}{{s}_{5}}\text{+}{{c}_{13}}{{c}_{5}};{{o}_{z}}={{s}_{4}}{{s}_{5}}; \\ & {{a}_{x}}=-{{c}_{13}}{{c}_{4}}{{c}_{5}}{{s}_{6}}+{{\operatorname{c}}_{13}}{{s}_{4}}{{c}_{6}}+{{s}_{13}}{{s}_{5}}{{s}_{6}}; \\ & {{a}_{y}}=-{{s}_{13}}{{c}_{4}}{{c}_{{}}}{{s}_{6}}+{{s}_{13}}{{s}_{4}}{{c}_{6}}-{{c}_{13}}{{s}_{5}}{{s}_{6}}; \\ & {{a}_{z}}={{s}_{4}}{{c}_{5}}{{s}_{6}}+{{c}_{4}}{{c}_{6}}; \\ & {{p}_{x}}=-28{{c}_{13}}{{c}_{4}}{{c}_{5}}{{s}_{6}}+28{{\operatorname{c}}_{13}}{{s}_{4}}{{c}_{6}}+6.5{{c}_{13}}{{c}_{4}}+28.5{{c}_{13}}{{s}_{4}}-6.5{{c}_{13}}+28{{s}_{13}}{{s}_{5}}{{s}_{6}}+d{{c}_{1}}; \\ & {{p}_{y}}=-28{{s}_{13}}{{c}_{4}}{{c}_{5}}{{s}_{6}}+28{{s}_{13}}{{s}_{4}}{{c}_{6}}+6.5{{s}_{13}}{{c}_{4}}+28.5{{s}_{13}}{{s}_{4}}-6.5{{s}_{13}}-28{{c}_{13}}{{s}_{5}}{{s}_{6}}+d{{s}_{1}}; \\ & {{p}_{z}}=28{{s}_{4}}{{c}_{5}}{{s}_{6}}+28{{c}_{4}}{{c}_{6}}-6.5{{s}_{4}}+28.5{{c}_{4}}+25. \\ \end{align} (2)

1.2 服务机器人运动学逆解

 $^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)

1.3 服务机器人手眼关系标定

 图 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)

 ${}^{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)

 $a_{k}^{i}x_{l}^{k}b_{j}^{i}=x_{j}^{i},$ (17)

 $\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)

 $Hx=0,$ (20)

 $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)

 ${{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)

 $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],$

 $R=\pm \sqrt{3}R'.$ (25)

 $\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 视觉伺服切换控制律设计

2.1 眼注视逼近

 图 3 眼注视逼近控制律示意图 Figure 3 Schematic of eye-fixed approaching control law

 ${{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)

 ${}^{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)

 $-\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)

2.2 Look-then-doing开环抓取

 图 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)

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)

$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)

 $\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)

 ${}^{c'}n=R{{'}^{c}}n,{}^{c'}d={}^{c}d-{}^{c'}{{n}^{T}}t'.$ (44)
2.2.2 计算｛c′｝与｛c*｝间的相对位姿c*Tc′

 ${}^{d}m\cong \left( R'-\frac{t\centerdot {}^{c'}{{n}^{T}}}{{}^{c'}d} \right){}^{c'}m\cong {{H}^{c'}}m.$ (45)

 $\alpha H=R-\frac{t\centerdot {}^{c'}{{n}^{T}}}{{}^{c'}d}.$ (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)

 ${{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)

 $R_{{{k}_{1}},{{\theta }_{1}}}^{c'}n={{e}_{3}}.$ (51)

 $\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)

 ${{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)

 $\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)

 $t={}^{c'}d\left( R-\alpha H \right){}^{c'}n.$ (56)

 ${}^{0}{{T}_{E}}={}^{0}{{T}_{E'}}{}^{E}{{T}_{c}}{}^{c*}{{T}^{-1}}_{c'}{}^{E}{{T}^{-1}}_{c}.$ (57)

3 试验与分析 3.1 手眼关系标定试验

 图 5 2D标定物角点提取图像 Figure 5 Corner extraction images of 2D chessboard

 ${}^{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.2 切换控制实验

 图 6 期望图像和当前图像 Figure 6 Desired image and current image

 图 7 移动机械臂控制输入 Figure 7 Control inputs of the mobile manipulator

 图 8 已知运动前后摄像机采集图像 Figure 8 Images corresponding coordinate ｛c｝ and ｛c′｝

4 结论