﻿ 多节双八面体变几何桁架臂逆运动学解析算法
 文章快速检索 高级检索
 浙江大学学报(工学版)  2017, Vol. 51 Issue (1): 75-81  DOI:10.3785/j.issn.1008-973X.2017.01.009 0

### 引用本文 [复制中英文]

dx.doi.org/10.3785/j.issn.1008-973X.2017.01.009
[复制中文]
QIAN Long-hao, HU Shi-qiang, YANG Yong-sheng. Analytical inverse kinematics algorithm for double-octahedral variable geometry truss manipulators[J]. Journal of Zhejiang University(Engineering Science), 2017, 51(1): 75-81.
dx.doi.org/10.3785/j.issn.1008-973X.2017.01.009
[复制英文]

### 作者简介

orcid.org/0000-0002-3967-1368.
E-mail: ARJ21@sjtu.edu.cn

### 通信联系人

orcid.org/0000-0002-9362-4642.
E-mail: sqhu@sjtu.edu.cn

### 文章历史

Analytical inverse kinematics algorithm for double-octahedral variable geometry truss manipulators
QIAN Long-hao , HU Shi-qiang , YANG Yong-sheng
School of Aeronautics and Astronautics, Shanghai Jiao Tong University, Shanghai 200240, China
Abstract: An analytical inverse kinematics (IK) algorithm was proposed in order to obtain fast and accurate IK solution for multi-section double-octahedral variable geometry truss (VGT) manipulator. A rotation matrix was introduced without using Euler angle and Denavit-Hartenberg (D-H) parameters based on VGT manipulator symmetric structure property and mirror transformation matrix. The full IK problem was reduced into sub-problems involving finding the rotation matrices of auxiliary coordinate systems and determining position vector in the auxiliary coordinate system by establishing two auxiliary coordinate systems. A two-section VGT manipulator IK algorithm was given. A simplified kinematic configuration capable of converting multi-section VGT manipulator into a single-section manipulator was given by analyzing two-section VGT manipulator IK solution. The N-section VGT manipulator was equivalent to two-section VGT manipulator. The N-section VGT manipulator IK solution was obtained by using two-section VGT manipulator IK algorithm and the simplified kinematic configuration. Simulation results indicate that the proposed IK algorithm has improved computation speed and accuracy than solutions from Jacobian matrix method. The effectiveness of the algorithm was verified on a VGT manipulator test device.
Key words: double-octahedral variable geometry truss manipulator    parallel manipulator    analytical inverse kinematics

1 机械臂运动学模型

 图 1 双八面体变几何桁架机械臂的简化模型 Fig. 1 Simplified model of double-octahedral variable geometry truss
 $\left. {\begin{array}{*{20}{c}} {^{k - 1}{\mathit{\boldsymbol{T}}_k} = \left[ {\begin{array}{*{20}{c}} {^{k - 1}{\mathit{\boldsymbol{R}}_k}}&{^{k - 1}{\mathit{\boldsymbol{P}}_k}}\\ 0&1 \end{array}} \right]{;^{k - 1}}{\mathit{\boldsymbol{R}}_k} \in {{\bf{R}}^{3 \times 3}},}\\ {^{k - 1}{\mathit{\boldsymbol{P}}_k} \in {{\bf{R}}^{3 \times 1}},{{\left( {^{k - 1}{\mathit{\boldsymbol{R}}_k}} \right)}^{{\rm{T}}k - 1}}{\mathit{\boldsymbol{R}}_k} = \mathit{\boldsymbol{I}}.} \end{array}} \right\}$ (1)

 $^{k - 1}\mathit{\boldsymbol{X}}{\mathit{\boldsymbol{ = }}^{k - 1}}\mathit{\boldsymbol{R}}_k^k\mathit{\boldsymbol{X = }}{\left[ {^{k - 1}{x_x},\;\;{\;^{k - 1}}{x_y},\;\;\;{\;^{k - 1}}{x_z}} \right]^{\rm{T}}}.$ (2)

 $\begin{array}{*{20}{c}} {^0{\mathit{\boldsymbol{P}}_{\rm{e}}} \in {{\bf{R}}^{3 \times 1}}和{^0}{\mathit{\boldsymbol{n}}_{\rm{e}}} \in {{\bf{R}}^{3 \times 1}}.}\\ {\left. \begin{array}{l} ^0{\mathit{\boldsymbol{P}}_{\rm{e}}}{ = ^0}{\mathit{\boldsymbol{P}}_{\rm{1}}} + \sum\limits_{i = 2}^N {{{\left( {\prod\limits_{k = 1}^{i - 1} {^{k - 1}{\mathit{\boldsymbol{R}}_k}} } \right)}^{i - 1}}{\mathit{\boldsymbol{P}}_i}} ,\\ ^0{\mathit{\boldsymbol{n}}_{\rm{e}}} = \left( {\prod\limits_{k = 1}^N {^{k - 1}{\mathit{\boldsymbol{R}}_k}} } \right){\left[ {1,\;\;\;\;0,\;\;\;\;0} \right]^{\rm{T}}}. \end{array} \right\}} \end{array}$ (3)
2 机械臂逆运动学解析算法 2.1 问题描述

2.2 桁架机械臂的旋转矩阵

 图 2 M坐标系与P坐标系的定义 Fig. 2 Definition of M coordinate frame and P coordinate frame
 $\left. \begin{array}{l} ^{k - 1}{\mathit{\boldsymbol{R}}_k} = \left( {\mathit{\boldsymbol{I}} - {2^{k - 1}}{\mathit{\boldsymbol{U}}_k}{{\left( {^{k - 1}{\mathit{\boldsymbol{U}}_k}} \right)}^{\rm{T}}}} \right)\mathit{\boldsymbol{D}};\\ \mathit{\boldsymbol{D = }}\left[ {\begin{array}{*{20}{c}} { - 1}&{}&{\bf{0}}\\ {}&1&{}\\ {\bf{0}}&{}&1 \end{array}} \right],\\ ^{k - 1}{\mathit{\boldsymbol{U}}_k}{ = ^{k - 1}}{\mathit{\boldsymbol{P}}_k}/{p_k},{p_k} = \left\| {^{k - 1}{\mathit{\boldsymbol{P}}_k}} \right\|. \end{array} \right\}$ (4)

2.3 最小工作单元

 $\left. {\begin{array}{*{20}{c}} {^0{\mathit{\boldsymbol{P}}_{\rm{e}}}{ = ^0}{\mathit{\boldsymbol{P}}_{\rm{1}}}{ + ^0}{\mathit{\boldsymbol{R}}_{\rm{1}}}^1{\mathit{\boldsymbol{P}}_{\rm{2}}},}\\ {^0{\mathit{\boldsymbol{n}}_{\rm{e}}}{ = ^0}{\mathit{\boldsymbol{R}}_{\rm{1}}}^1{\mathit{\boldsymbol{R}}_{\rm{2}}}{{\left[ {1,\;\;\;\;0,\;\;\;\;0} \right]}^{\rm{T}}},} \end{array}} \right\}$ (5)

2.4 辅助坐标系旋转矩阵子问题

 ${\mathit{\boldsymbol{P}}_{\rm{2}}} \cdot \left( {{\mathit{\boldsymbol{P}}_{\rm{e}}} \times {\mathit{\boldsymbol{P}}_{\rm{1}}}} \right) = 0.$ (6)

 $\left. \begin{array}{l} ^P{\mathit{\boldsymbol{R}}_{\rm{0}}} = \left[ {\begin{array}{*{20}{c}} {{c_3}} & {{s_3}} & 0\\ { - {s_3}} & {{c_3}} & 0\\ 0 & 0 & 1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{c_2}} & 0 & { - {s_2}}\\ 0 & 1 & 0\\ {{s_2}} & 0 & {{c_2}} \end{array}} \right];\\ {\sigma _2} = - a\tan \left( {\frac{{^0{u_{{\rm{e}}z}}}}{{^0{u_{{\rm{e}}x}}}}} \right),{\sigma _3} = - a\cos \left( {^0{u_{{\rm{e}}y}}} \right). \end{array} \right\}$ (7)

 图 3 向量Pe、P1和P2在M平面上的关系 Fig. 3 Vector relationship between Pe, P1 and P2 on M plane
 $^M{\mathit{\boldsymbol{R}}_P} = \left[ {\begin{array}{*{20}{c}} {\cos \varphi }&0&{ - \sin \varphi }\\ 0&1&0\\ {\sin \varphi }&0&{\cos \varphi } \end{array}} \right].$ (8)

 $\left. {\begin{array}{*{20}{c}} {^0{\mathit{\boldsymbol{n}}_{\rm{e}}}{ = ^0}{\mathit{\boldsymbol{R}}_{\rm{1}}}\left( {\mathit{\boldsymbol{I}} - {2^1}{\mathit{\boldsymbol{U}}_2}{{\left( {^1{\mathit{\boldsymbol{U}}_2}} \right)}^{\rm{T}}}} \right){\mathit{\boldsymbol{D}}^0}\mathit{\boldsymbol{b}},}\\ {^0\mathit{\boldsymbol{b}} = {{\left[ {1,\;\;\;\;0,\;\;\;\;0} \right]}^{\rm{T}}}.} \end{array}} \right\}$ (9)

 ${\left( {\mathit{\boldsymbol{I}} - {2^M}{\mathit{\boldsymbol{U}}_2}{{\left( {^M{\mathit{\boldsymbol{U}}_2}} \right)}^{\rm{T}}}} \right)^M}{\mathit{\boldsymbol{n}}_{\rm{e}}} = {\left( {\mathit{\boldsymbol{I}} - {2^M}{\mathit{\boldsymbol{U}}_1}{{\left( {^M{\mathit{\boldsymbol{U}}_1}} \right)}^{\rm{T}}}} \right)^M}\mathit{\boldsymbol{b}}.$ (10)

 $\mathit{\boldsymbol{I}} - {2^M}{\mathit{\boldsymbol{U}}_1}{\left( {^M{\mathit{\boldsymbol{U}}_1}} \right)^{\rm{T}}} = \left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0& * & * \\ 0& * & * \end{array}} \right].$ (11)
 $\mathit{\boldsymbol{I}} - {2^M}{\mathit{\boldsymbol{U}}_2}{\left( {^M{\mathit{\boldsymbol{U}}_2}} \right)^{\rm{T}}} = \left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0& * & * \\ 0& * & * \end{array}} \right].$ (12)

 ${\left( {^M{\mathit{\boldsymbol{R}}_P}^P{\mathit{\boldsymbol{n}}_{\rm{e}}}} \right)_x} = {\left( {^M{\mathit{\boldsymbol{R}}_P}^P\mathit{\boldsymbol{b}}} \right)_x}.$ (13)

 $\left. \begin{array}{l} \sin \left( {\beta + \varphi } \right) = 0;a = {\left( {^P{\mathit{\boldsymbol{n}}_{\rm{e}}}} \right)_x} - {\left( {^P\mathit{\boldsymbol{b}}} \right)_x},\\ b = {\left( {^P\mathit{\boldsymbol{b}}} \right)_z} - {\left( {^P{\mathit{\boldsymbol{n}}_{\rm{e}}}} \right)_z},\beta = {\rm{atan2}}\left( {a,b} \right). \end{array} \right\}$ (14)

 $\varphi = - \beta .$ (15)
2.5 辅助坐标系内位置矢量子问题

 图 4 M平面上θb, θt, θ1和θ2角度关系 Fig. 4 Relatioship among θb, θt, θ1 and θ2 on M plane
 $\left. {\begin{array}{*{20}{c}} {{\theta _{{\rm{tm}}}} = {\theta _2} + {\rm{\pi }} - \left( {{\theta _{\rm{t}}} - {\theta _{\rm{2}}}} \right) = 2{\theta _2} + {\rm{\pi }} - {\theta _{\rm{t}}},}\\ {{\theta _{{\rm{bm}}}} = {\theta _1} + {\rm{\pi }} - \left( {{\theta _{\rm{b}}} - {\theta _{\rm{1}}}} \right) = 2{\theta _1} + {\rm{\pi }} - {\theta _{\rm{b}}},}\\ {{\theta _{\rm{t}}} = {\rm{atan2}}\left( {^M{n_{{\rm{e}}z}}{,^M}{n_{{\rm{e}}y}}} \right),}\\ {{\theta _{\rm{b}}} = {\rm{atan2}}\left( {^M{b_z}{,^M}{b_y}} \right).} \end{array}} \right\}$ (16)

 $\left. \begin{array}{l} {\theta _2} = {\theta _1} + \frac{{{\theta _{\rm{t}}} - {\theta _{\rm{b}}}}}{2},\\ {\theta _2} = {\theta _1} + \frac{{{\theta _{\rm{t}}} - {\theta _{\rm{b}}}}}{2} + {\rm{\pi }}{\rm{.}} \end{array} \right\}$ (17)

 $\left. {\begin{array}{*{20}{c}} {{\eta _1} = {\rm{atan2}}\left( {{\rm{s}}\left( {\frac{{{\theta _{\rm{t}}} - {\theta _{\rm{b}}}}}{2}} \right),\;\;\;{\rm{c}}\left( {\frac{{{\theta _{\rm{t}}} - {\theta _{\rm{b}}}}}{2}} \right)} \right),}\\ {{\eta _2} = {\rm{atan2}}\left( {{\rm{s}}\left( {\frac{{{\theta _{\rm{t}}} - {\theta _{\rm{b}}}}}{2} + {\rm{\pi }}} \right),\;\;\;{\rm{c}}\left( {\frac{{{\theta _{\rm{t}}} - {\theta _{\rm{b}}}}}{2} + {\rm{\pi }}} \right)} \right),}\\ {\eta = {\eta _1}或{\eta _2},\left| {{\theta _1}} \right| + \left| {{\theta _2}} \right| = \left| \eta \right|.} \end{array}} \right\}$ (18)

 $\left. {\begin{array}{*{20}{c}} {\sin {\theta _2} = \frac{{{p_1}\sin \eta }}{{{p_{\rm{t}}}}},}\\ {{\theta _2} = \arcsin \left( {\frac{{{p_1}\sin \eta }}{{{p_{\rm{t}}}}}} \right)或{\rm{\pi }} - \arcsin \left( {\frac{{{p_1}\sin \eta }}{{{p_{\rm{t}}}}}} \right).} \end{array}} \right\}$ (19)

 $\left. {\begin{array}{*{20}{c}} {{\theta _1} = \left| \eta \right| - {\theta _2},}\\ {{p_2} = \sqrt {p_{\rm{t}}^2 + p_{\rm{1}}^2 - 2{p_1}{p_{\rm{t}}}\cos {\theta _1}} .} \end{array}} \right\}$ (20)
 $\left[ {\begin{array}{*{20}{c}} {{\theta _1}}\\ {{\theta _2}} \end{array}} \right] = {\mathop{\rm sgn}} \left( \eta \right)\left[ {\begin{array}{*{20}{c}} { - {\theta _1}}\\ {{\theta _2}} \end{array}} \right].$ (21)
 $\left. \begin{array}{l} ^M{\mathit{\boldsymbol{U}}_1} = {\left[ {0,\;\;\;\;\cos {\theta _1},\;\;\;\;\;\sin {\theta _1}} \right]^{\rm{T}}},\\ ^M{\mathit{\boldsymbol{U}}_2} = {\left[ {0,\;\;\;\;\cos {\theta _2},\;\;\;\;\;\sin {\theta _2}} \right]^{\rm{T}}}. \end{array} \right\}.$ (22)

θ1θ2的符号根据式(21) 作相应改变.利用式(22) 计算出U1U2p1p2.算法流程如图 5所示.根据式(18) 可知, η有两个值：η1η2.根据式(19) 可知, 对于每一个η有两个θ1θ2, 所以根据式(21) 和(22), θ1θ2的解有4组.算法使用这4组解计算4组驱动器长度.在算法输出结果时, 只输出驱动器长度, 满足结构限制的解.

 图 5 最小工作单元逆运动学解析算法 Fig. 5 Analytical inverse kinematics of minimum taskunit
3 N节机械臂逆运动学解析算法 3.1 同向偏转简化

 $\mathit{\boldsymbol{c}} \cdot \left( {{\mathit{\boldsymbol{P}}_2} \times {\mathit{\boldsymbol{P}}_1}} \right) = {\bf{0}}.$ (23)

 图 6 同向偏转简化 Fig. 6 Equal turn mode simplification
 $\sigma = {\rm{atan2}}\left( {^{{P_2}}{p_{{\rm{e}}z}}{,^{{P_2}}}{p_{{\rm{e}}y}}} \right).$ (24)
 $\left. \begin{array}{l} ^{{P_2}}{\mathit{\boldsymbol{R}}_0} = \left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0&{\cos \sigma }&{\sin \sigma }\\ 0&{ - \sin \sigma }&{\cos \sigma } \end{array}} \right];\\ \delta = \arccos \frac{{^{{P_2}}{p_{{\rm{e}}y}}}}{{{p_{\rm{e}}}}},r = \frac{{{p_{\rm{e}}}}}{{2\cos \delta }},\\ \theta = {{ { π} }} - 2\delta ,\gamma \equiv \theta /{N_1}. \end{array} \right\}$ (25)

 $\left. \begin{array}{l} ^{{P_2}}{\mathit{\boldsymbol{P}}_1} = r{\left[ {\sin \gamma ,\;\;\;1 - \cos \gamma ,\;\;\;0} \right]^{\rm{T}}};\\ ^0{\mathit{\boldsymbol{P}}_1}{ = ^0}{\mathit{\boldsymbol{R}}_{{P_2}}}^{{P_2}}{\mathit{\boldsymbol{P}}_1}{,^{i - 1}}{\mathit{\boldsymbol{P}}_i}{ = ^0}{\mathit{\boldsymbol{P}}_1},i = 2, \cdots ,{N_1}. \end{array} \right\}$ (26)

3.2 N节机械臂的逆运动学解析算法

 图 7 N节VGT机械臂的逆运动学算法 Fig. 7 Inverse kinematics of VGT manipulator with N sections
4 仿真验算和实物验证 4.1 仿真验算

 $\left. \begin{array}{l} {p_{{\rm{err}}}} = \left\| {^0{\mathit{\boldsymbol{P}}_{{\rm{e,d}}}}{ - ^0}{\mathit{\boldsymbol{P}}_{{\rm{e,c}}}}} \right\|,\\ {n_{{\rm{err}}}} = \left\| {^0{\mathit{\boldsymbol{n}}_{{\rm{e,d}}}}{ - ^0}{\mathit{\boldsymbol{n}}_{{\rm{e,c}}}}} \right\|. \end{array} \right\}$ (27)

 图 8 雅可比矩阵法和本文方法的位置方向计算误差 Fig. 8 Position and normal vector error between Jacobian matrix method and proposed method

 ${T_n} = {T_k}/{T_2}.$ (28)

 图 9 本文提出方法的计算时间和节数的关系 Fig. 9 Computation time with respect to number of sections of manipulator
4.2 机械臂运动测试平台验证

 图 10 VGT机械臂运动测试平台 Fig. 10 VGT manipulator motion test platform

1) 给定一组期望的末端运动平面位置和方向后, 利用本文的逆运动学算法解出驱动器长度, 根据计算结果操作机构运动.

2) 如图 10所示, 将坐标纸铺在基座坐标系上, 利用铅锤、卷尺测量出末端运动平台在X0Y0Z0O0坐标系中坐标.利用铅锤和量角器测出末端运动平台的偏转角.对每个位置多次测量求平均值, 再将测量出的偏转角转换为末端运动平面在X0Y0Z0O0坐标系中的法向量.位置测量精度为±5 mm, 角度测量误差为±2°.

3) 从表 2可以看出, 位置误差大多数在5%以内, 方向误差在10%以内, 而且呈现一定的递增趋势.这些误差并不是由于逆运动学的算法引起的, 而是部分来源于测量误差, 部分来源于轴承间隙和重力作用下的杆件弯曲.在相对基座X轴较远的地方, 重力使杆部件弯曲和变形, 同时轴承间隙使得机构中等效杆长变长, 产生比在X轴附近区域更大的定位误差. 表 2的测量结果可以证明, 采用本文的逆向运动学方法可以准确地计算期望末端位置所对应的作动器杆长.

5 结语

 [1] BATRAM ATILLA. Trajectory Tracking of a redundant hybrid manipulator using a switching control method[J]. World Academy of Science, Engineering and Technology, International Journal of Mechanical, Aerospace, Industrial, Mechatronic and Manufacturing Engineering, 2016, 10(6): 920–929. [2] 吴江, 徐礼钜, 雷勇. 基于神经网络的冗余度二重八面体变几何桁架机器人运动学求解[J]. 四川大学学报:工程科学版, 2000, 32(2): 90–103. WU Jiang, XU Li-ju, LEI Yong. The kinematics ofredundant double-octahedron variable geometry truss manipulators based on neural network[J]. Journal of Sichuan University: Engineering Science Edition, 2000, 32(2): 90–103. [3] MACARENO L, AGIRREBEITIA J, ANGULO C, et al. FEM subsystem replacement techniques for strength problems in variable geometry trusses[J]. Finite Elements in Analysis and Design, 2008, 44(6/7): 346–357. [4] ATSUHIKO S, KOSUKE O, MORIO T, et al. Vibration reduction by natural frequency optimization formanipulation of a variable geometry truss[J]. Structural and Multidisciplinary Optimization, 2013, 48(5): 939–954. DOI:10.1007/s00158-013-0933-6 [5] DE Z I O, AGUIRREBEITIA J, AVILES R, et al. A finite element approach to the inverse dynamics andvibrations of variable geometry trusses[J]. Finite Elements in Analysis and Design, 2011, 47(3): 220–228. DOI:10.1016/j.finel.2010.10.009 [6] TONDU B. Closed-form redundancy solving of serial chain robots with a weak generalized inverse approach[J]. Robotics and Autonomous Systems, 2015, 74(PB): 360–370. [7] DULEBA I, OPAKA M. A Comparison of jacobian-based methods of inverse kinematics for serial robotmanipulators[J]. International Journal of Applied Mathematics and Computer Science, 2013, 23(2): 373–382. [8] GIORELLI M, RENDA F, CALISTI M, et al. Learning the inverse kinetics of an octopus-like manipulator in three-dimensional space[J]. Bioinspiration and Biomimetics, 2015, 10(3): 1–13. [9] SHUKLA, SINGLA E, WAHI P, et al. A direct variational method for planning monotonically optimal paths for redundant manipulators in constrained workspaces[J]. Robotics and Autonomous Systems, 2013, 61(2): 209–220. DOI:10.1016/j.robot.2012.08.012 [10] YAHYA S, MOGHAVVEMI M, MOHAMMED A. Geometrical approach of planar hyper-redundant manipulators: inverse kinematics, path planning and workspace[J]. Simulation Modelling Practice and Theory, 2011, 19(1): 406–422. DOI:10.1016/j.simpat.2010.08.001 [11] 姜柏森, 杨永胜, 胡士强. 八面体变几何桁架机器人的运动学分析及仿真研究[J]. 制造业自动化, 2015, 37(3): 20–22. JIANG Bai-sen, YANG Yong-sheng, HU Shi-qiang. The solution and simulation for the kinematic problem of octahedral VGT manipulator[J]. ManufacturingAutomation, 2015, 37(3): 20–22. [12] YANG Y, JIANG B, HU S. Fast trajectory planning for VGT manipulator via convex optimization[J]. International Journal of Advanced Robotic Systems, 2015, 12(9): 1–17. [13] 杭鲁滨, 王彦, 邓辉宇, 等. 基于Groebner基的八面体变几何桁架机构位置正解分析[J]. 机械科学与技术, 2004, 23(6): 745–747. HANG Lu-bin, WANG Yan, DENG Hui-yu, et al. Forward displacement analysis of the octahedron variable geometry truss based on Groebner basis[J]. Mechanical Science and Technology, 2004, 23(6): 745–747.