﻿ 同步三指式末端执行器的目标位姿估计方法
«上一篇
 文章快速检索 高级检索

 哈尔滨工程大学学报  2019, Vol. 40 Issue (2): 359-364  DOI: 10.11990/jheu.201705043 0

### 引用本文

FAN Shaowei, WU Jun, JIN Minghe, et al. Pose estimation method for a simultaneous three-fingered end-effector[J]. Journal of Harbin Engineering University, 2019, 40(2), 359-364. DOI: 10.11990/jheu.201705043.

### 文章历史

Pose estimation method for a simultaneous three-fingered end-effector
FAN Shaowei , WU Jun , JIN Minghe , FAN Chunguang , LIU Hong
State Key Laboratory of Robotics and System, Harbin Institute of Technology, Harbin 150080, China
Abstract: To determine the target pose in the process of end-effector capturing, this study develops a pose estimation method for a simultaneous three-fingered end effector based on contact force information. The pose estimation method is proposed after analyzing the geometric constraint characteristics of the end effector. First, the contact finger is estimated with the contact force. Then, the contact position and normal direction are estimated by combining the kinematic analysis results of the end effector. The spatial plane intersection method is adopted for pose estimation with two effective contact information. When obtaining redundant contact information, the least squares method is adopted to update the estimation results. Finally, simulation validates the effectiveness of the estimation method. The proposed method has general applicability to a certain extent and specific applicability to claw-shaped end effectors.
Keywords: pose estimation    end-effector    contact force information    least squares method    dock and capture    simultaneous three-fingered

1 三指式末端执行器 1.1 机械系统

 ${}^e{\mathit{\boldsymbol{T}}_j} = \left[ {\begin{array}{*{20}{c}} {{}^e{T_{jx}}}\\ {{}^e{T_{jy}}}\\ {{}^e{T_{jz}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{f_{{e_{{T_{jx}}}}}}\left( \varphi \right)}\\ {{f_{{e_{{T_{jy}}}}}}\left( \varphi \right)}\\ {{f_{{e_{{T_{jz}}}}}}\left( \varphi \right)} \end{array}} \right],\left( {j = {\rm{I}},{\rm{II}},{\rm{III}}} \right)$ (1)

1.2 位姿估计任务假设

1) 碰撞过程中只考虑单点接触的情况；

2) 忽略点接触情况下的摩擦力影响；

3) 忽略末端执行器传动系统中的间隙、柔性等非线性因素的影响；

4) 只分析具有相对较大位姿偏差的初始包络阶段；

5) 对接接口大致向上安装，如图 1所示。

2 位姿估计方法 2.1 接触手指识别方法

 Download: 图 2 对接接口投影平面 Fig. 2 Projection plane of the grapple interface
2.2 接触点位置/方向估计

 ${}^e\mathit{\boldsymbol{C}} = {}^e\mathit{\boldsymbol{T}} - r{}^e\mathit{\boldsymbol{F}}/\left\| {{}^e\mathit{\boldsymbol{F}}} \right\|$ (2)

 ${}^e\mathit{\boldsymbol{n}} = {}^e\mathit{\boldsymbol{F}}/\left\| {{}^e\mathit{\boldsymbol{F}}} \right\|$ (3)

2.3 基于最少接触信息的位姿估计

 ${}^o{n_{1x}}{}^ox + {}^o{n_{1y}}{}^oy + {}^o{n_{1z}}{}^oz + {p_1} + {d_c} = 0$ (4)

 ${}^o{n_{2x}}{}^ox + {}^o{n_{2y}}{}^oy + {}^o{n_{2z}}{}^oz + {p_2} + {d_c} = 0$ (5)

 ${}^o\mathit{\boldsymbol{s}} = \left( {{}^o{\mathit{\boldsymbol{n}}_2} \times {}^o{\mathit{\boldsymbol{n}}_1}} \right)/\left\| {{}^o{\mathit{\boldsymbol{n}}_2} \times {}^o{\mathit{\boldsymbol{n}}_1}} \right\|$ (6)

 ${}^o{\mathit{\boldsymbol{T}}_{ia}} = {}^o{\mathit{\boldsymbol{T}}_a}{\rm{Rot}}\left( {z, - {\theta _a}} \right) = \left[ {\begin{array}{*{20}{c}} {{}^o{\mathit{\boldsymbol{x}}_{ia}}}&{{}^o{\mathit{\boldsymbol{y}}_{ia}}}&{{}^o{\mathit{\boldsymbol{z}}_{ia}}}&{{}^o\mathit{\boldsymbol{I}}}\\ 0&0&0&1 \end{array}} \right]$ (7)

 ${}^o{\mathit{\boldsymbol{T}}_{ic}} = \left[ {\begin{array}{*{20}{c}} {{}^o{\mathit{\boldsymbol{x}}_{ic}}}&{{}^o{\mathit{\boldsymbol{y}}_{ic}}}&{{}^o{\mathit{\boldsymbol{z}}_{ic}}}&{{}^o\mathit{\boldsymbol{I}}}\\ 0&0&0&1 \end{array}} \right]$ (8)

 ${}^o{\mathit{\boldsymbol{y}}_i} = \left( {{}^o{\mathit{\boldsymbol{y}}_{ia}} + {}^o{\mathit{\boldsymbol{y}}_{ic}}} \right)/\left\| {{}^o{\mathit{\boldsymbol{y}}_{ia}} + {}^o{\mathit{\boldsymbol{y}}_{ic}}} \right\|$ (9)

 ${}^o{\mathit{\boldsymbol{T}}_i} = \left[ {\begin{array}{*{20}{c}} {{}^o{\mathit{\boldsymbol{x}}_i}}&{{}^o{\mathit{\boldsymbol{y}}_i}}&{{}^o{\mathit{\boldsymbol{z}}_i}}&{{}^o\mathit{\boldsymbol{I}}}\\ 0&0&0&1 \end{array}} \right]$ (10)

3 冗余接触估计更新方法

3.1 中心轴线估计更新

N次接触中，各接触面ck(k=1, 2, …, N)沿-nk方向平移dc距离得到平面ck应交于一条公共直线。但由于结构加工误差、力/矩传感器误差等因素的影响，各平面ck间的交线存在差异。我们通过求得对接接口有效区域内的两个点，使每点到各平面ck间的距离和最小，由两点确定的直线即为对接接口中心轴线估计结果。

 ${D_{1k}} = \left| {{}^o{n_{kx}}{}^o{A_{1x}} + {}^o{n_{ky}}{}^o{A_{1y}} + {}^o{n_{kz}}{}^o{A_{1z}} + {p_k} + {d_k}} \right|$ (11)

 ${\mathit{\boldsymbol{j}}_1} = \left[ {\begin{array}{*{20}{c}} {{}^o{A_{1x}}}\\ {{}^o{A_{1y}}}\\ {{}^o{A_{1z}}}\\ 1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{A_{11}}}&{{A_{12}}}&{{A_{31}}}&{{A_{14}}}\\ {{A_{12}}}&{{A_{22}}}&{{A_{23}}}&{{A_{24}}}\\ {{A_{31}}}&{{A_{23}}}&{{A_{33}}}&{{A_{34}}}\\ {{A_{14}}}&{{A_{24}}}&{{A_{34}}}&{{A_{44}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{}^o{A_{1x}}}\\ {{}^o{A_{1y}}}\\ {{}^o{A_{1z}}}\\ 0 \end{array}} \right]$ (12)

 $\left\{ \begin{array}{l} {A_{11}} = \sum\limits_{k = 1}^N {{}^on_{kx}^2} ,{A_{22}} = \sum\limits_{k = 1}^N {{}^on_{ky}^2} ,{A_{33}} = \sum\limits_{k = 1}^N {{}^on_{kz}^2} ,\\ {A_{44}} = \sum\limits_{k = 1}^N {{{\left( {{p_k} + {d_c}} \right)}^2}} ,{A_{12}} = \sum\limits_{k = 1}^N {{}^o{n_{kx}}{}^o{n_{ky}}} ,\\ {A_{23}} = \sum\limits_{k = 1}^N {{}^o{n_{ky}}{}^o{n_{kz}}} ,{A_{31}} = \sum\limits_{k = 1}^N {{}^o{n_{kz}}{}^o{n_{kx}}} ,\\ {A_{14}} = \sum\limits_{k = 1}^N {{}^o{n_{kx}}\left( {{p_k} + {d_c}} \right)} ,\\ {A_{24}} = \sum\limits_{k = 1}^N {{}^o{n_{ky}}\left( {{p_k} + {d_c}} \right)} ,\\ {A_{34}} = \sum\limits_{k = 1}^N {{}^o{n_{kz}}\left( {{p_k} + {d_c}} \right)} 。\end{array} \right.$

 ${}^o{\mathit{\boldsymbol{A}}_{1xy}} = \left[ {\begin{array}{*{20}{c}} {{}^o{A_{1x}}}\\ {{}^o{A_{1y}}} \end{array}} \right] = - {\left[ {\begin{array}{*{20}{c}} {{A_{11}}}&{{A_{12}}}\\ {{A_{12}}}&{{A_{22}}} \end{array}} \right]^{ - 1}}\left[ {\begin{array}{*{20}{c}} {{A_{31}}}&{{A_{14}}}\\ {{A_{23}}}&{{A_{24}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{}^o{A_{1z}}}\\ 1 \end{array}} \right]$ (13)

3.2 综合位姿估计更新

 Download: 图 5 第k次接触的更新结果 Fig. 5 Estimation update from the kth contact

 $\begin{array}{*{20}{c}} {{}^o{\mathit{\boldsymbol{T}}_{ik}} = {}^o{\mathit{\boldsymbol{T}}_k}{\rm{Rot}}\left( {z,{\varphi _k}} \right) = }\\ {\left[ {\begin{array}{*{20}{c}} {{}^o{\mathit{\boldsymbol{y}}_k} \times {}^o\mathit{\boldsymbol{s}}}&{{}^o{\mathit{\boldsymbol{y}}_k}}&{{}^o\mathit{\boldsymbol{s}}}&{{}^o\mathit{\boldsymbol{I}}}\\ {\bf{0}}&{\bf{0}}&{\bf{0}}&1 \end{array}} \right]{\rm{Rot}}\left( {z,{\varphi _k}} \right) = }\\ {\left[ {\begin{array}{*{20}{c}} {{}^o{\mathit{\boldsymbol{x}}_{ik}}}&{{}^o{\mathit{\boldsymbol{y}}_{ik}}}&{{}^o{\mathit{\boldsymbol{z}}_{ik}}}&{{}^o\mathit{\boldsymbol{I}}}\\ {\bf{0}}&{\bf{0}}&{\bf{0}}&1 \end{array}} \right]} \end{array}$ (14)

 ${}^o{\mathit{\boldsymbol{y}}_i} = \sum\limits_{k = 1}^N {{}^o{\mathit{\boldsymbol{y}}_{ik}}} /\left\| {\sum\limits_{k = 1}^N {{}^o{\mathit{\boldsymbol{y}}_{ik}}} } \right\|$ (15)

4 位姿估计仿真 4.1 仿真设计

4.2 仿真结果

 $\left| {{d_k} - {d_{k - 1}}} \right| < {d_{{\rm{th}}}}\;{\rm{or}}\;\left| {{\mathit{\Theta }_k} - {\mathit{\Theta }_{k - 1}}} \right| < {\mathit{\Theta }_{{\rm{th}}}}\;{\rm{or}}\;k > {k_{{\rm{th}}}}$ (16)

1) 只有位置偏差

2) 只有姿态偏差

3) 位置和姿态偏差同时存在

 ${\mathit{\boldsymbol{P}}_i} = {\left[ {\begin{array}{*{20}{c}} {5.5\;\;{\rm{mm}}}&{10\;{\rm{mm}}}&{ - 15\;{\rm{mm}}}&{{2^ \circ }}&{{4^ \circ }}&{{3^ \circ }} \end{array}} \right]^{\rm{T}}}$