一种六自由度手腕偏置串联机器人的运动学逆解求解方法技术

本发明专利技术公开了一种六自由度手腕偏置串联机器人的运动学逆解求解方法，所述方法是利用六自由度手腕无偏置串联机器人的运动学逆解的解析解作为六自由度手腕偏置串联机器人的运动学逆解近似解和迭代出发点，通过不断迭代逼近，求得满足精度的六自由度手腕偏置串联机器人运动学逆解数值解，收敛速度快，相比传统求解方法的计算量少，减少了机器人控制器的运算量，提高实时性。

A inverse kinematics solution for a six degree of freedom wrist offset series robot

The invention discloses a six DOF wrist kinematics bias series robot inverse solution method, the method is the solution of approximate solution and iterative point kinematics unbiased serial robot with six degrees of freedom of the wrist the inverse analytic solution as six degrees of freedom robot wrist bias inverse, through continuous iterative approximation six degrees of freedom, satisfy the accuracy of numerical solution of wrist bias series solutions of the inverse kinematics of a robot, fast convergence speed and computation compared to traditional method less, reduce the amount of computation of the robot controller, improve the real-time.

【技术实现步骤摘要】
一种六自由度手腕偏置串联机器人的运动学逆解求解方法
本专利技术涉及一种机器人的运动学逆解求解方法，具体来说是一种六自由度手腕偏置串联机器人的运动学逆解求解方法。
技术介绍
机器人的逆运动学是其轨迹规划和控制等方面的前提和基础。一般情况下，六自由度串联机器人要获得解析解，通常要采用无偏置手腕，但它无法实现中间关节360度旋转、结构强度偏低。因此在实际生产过程中通常采用六自由度手腕偏置机器人替代，但限于目前的数学工具，上述运动学逆解通常没有解析解，只有数值解。现有针对六自由度手腕偏置机器人运动学逆解的求解算法，是利用几何法和代数消元法，以及禁忌搜索法或爬山法优化方法，但上述算法所需的计算量大，对机器人控制器造成较大负担，实时性较差。
技术实现思路
为解决上述问题，本专利技术在于提供一种六自由度手腕偏置串联机器人的运动学逆解求解方法，能够快速求得运动学逆解数值解，能够减小机器人控制器计算量，提高实时性。本专利技术解决其问题所采用的技术方案是：提供一种用于计算六自由度手腕偏置串联机器人的运动学逆解的算法，包括以下步骤：A.将六自由度手腕偏置串联机器人末端坐标系的位姿当作六自由度手腕无偏置串联机器人末端坐标系的位姿，求得六自由度手腕无偏置串联机器人运动学逆解的解析解θ′1～θ′6；B.将解析解θ1′～θ6′作为所述六自由度手腕偏置串联机器人的运动学逆解的近似解和迭代法计算的出发点，由六自由度手腕偏置串联机器人的运动学正解求解公式，求得末端坐标系的近似位姿；C.计算得到末端坐标系期望位姿相对于末端坐标系近似位姿的位姿增量为dX，利用dX计算末端位姿综合误差为e；D.判断综合误差e是否在合理误差范围内，如果是，则停止迭代，将返回此时的关节变量数值作为六自由度手腕偏置串联机器人运动学逆解的迭代数值解；如果否，则利用六自由度手腕偏置串联机器人的雅可比矩阵求得关节变量θ′，并将新的关节变量θ′代入步骤B及步骤C，进行迭代计算，直到满足合理误差范围或者达到最大迭代次数，返回此时的关节变量数值作为六自由度手腕偏置串联机器人运动学逆解的迭代数值解。进一步，在所述步骤A之前，以六自由度手腕无偏置串联机器人的关节1旋转轴与关节2旋转轴交点为原点，关节1旋转轴所在直线为坐标系Z轴,关节2旋转轴所在直线为坐标系Y轴，建立基座坐标系；从基座往机器人末端方向，根据六个自由度依次建立6个坐标系，分别为坐标系{0}～坐标系{6}，其中坐标系{0}即所述基座坐标系{0}，坐标系{6}即末端坐标系{6}。进一步，所述步骤A包括如下步骤：A1.设所述六自由度手腕偏置串联机器人的末端坐标系{6}原点的期望位置为(Xd,Yd,Zd)，末端坐标系{6}的期望姿态矩阵为RXYZ(αd,βd,γd)，参考系为基座坐标系{0}。将六自由度手腕偏置串联机器人末端坐标系的位姿当作六自由度手腕无偏置串联机器人末端坐标系的位姿，利用六自由度手腕无偏置串联机器人的运动学逆解求解方法，求解六自由度手腕无偏置串联机器人运动学逆解的解析解θ1′～θ6′。先计算运动学逆解的前三个角度θ1′、θ2′和θ3′：由于无偏置手腕满足Pieper准则，并根据坐标系齐次变换矩阵，求得：其中解上述方程组可求得θ1′、θ2′和θ3′的解析解，其中为无偏置手腕末端位置点的向量表达式，a2为坐标系{2}Z轴与坐标系{3}Z轴在坐标系{2}X轴方向的距离，d2为为坐标系{1}X轴与坐标系{2}X轴在坐标系{2}Z轴方向的距离，d3为坐标系{2}X轴与坐标系{3}X轴在坐标系{3}Z轴方向的距离，d4为坐标系{3}X轴与坐标系{4}X轴在坐标系{4}Z轴方向的距离；A2.通过欧拉角XYZ(αd,βd,γd)变换矩阵RXYZ(αd,βd,γd)求得六自由度手腕无偏置串联机器人运动学逆解的后三个角度θ4′、θ5′和θ6′：其中为坐标系{4}相对于基座坐标系{0}的旋转矩阵，为坐标系{6}相对于坐标系{4}的旋转矩阵，为坐标系{6}经过后三个旋转轴旋转得到的旋转矩阵；通过计算上式得到的矩阵表达式，当cosθ5′≠0时，可得θ4′、θ5′和θ6′的解析式。进一步，所述步骤B包括如下步骤：B1.由步骤A所得的六自由度手腕偏置串联机器人运动学逆解的解析解θ1′～θ6′，作为六自由度手腕偏置串联机器人运动学逆解的近似解和迭代出发点；根据六自由度手腕偏置串联机器人运动学正解的矩阵表达公式：式中欧拉角通过求解上式，求得末端坐标系{6}原点的近似位置(Xc,Yc,Zc)和末端坐标系{6}近似姿态矩阵RXYZ(αc,βc,γc)。进一步，所述步骤C包括如下步骤：C1.根据末端坐标系{6}原点的期望位置(Xd,Yd,Zd)和末端坐标系{6}原点的近似位置(Xc,Yc,Zc)，求得末端坐标系{6}原点的位置增量为dP。C2.根据末端坐标系{6}期望姿态矩阵RXYZ(αd,βd,γd)和末端坐标系{6}近似姿态矩阵RXYZ(αc,βc,γc)之间的旋转关系，求得末端坐标系{6}姿态旋转增量为dAng。C3.用笛卡尔空间的微分运动dX表示末端坐标系原点的位置增量dP和末端坐标系姿态的旋转增量dAng，表示为并定义末端位姿综合误差为e＝‖dP‖2+‖dAng‖2。进一步，所述步骤D包括如下步骤：D1.由上述步骤C计算综合误差e是否在合理误差范围内，如果是，则停止迭代，将返回此时的关节变量数值作为六自由度手腕偏置串联机器人运动学逆解的迭代数值解；如果否，进行步骤D2。D2.利用六自由度手腕偏置串联机器人的雅可比矩阵J，通过dX＝Jdθ，计算dθ为：dθ＝J-1dX求得关节变量θ′＝θ′+dθ，并将新的关节变量θ′代入步骤B1进行迭代计算，直到e满足合理误差范围或者达到最大迭代次数，返回此时的关节变量数值作为六自由度手腕偏置串联机器人运动学逆解的迭代数值解。本专利技术的有益效果是：本算法巧妙利用六自由度手腕无偏置串联机器人的运动学逆解的解析解作为六自由度手腕偏置串联机器人的运动学逆解近似解，求得近似位姿，对比近似位姿和期望位姿之间的误差，利用等效轴角来表述末端近似位姿与期望位姿之间的姿态旋转增量和雅可比矩阵J，求得关节变量增量dθ′，从而求得新的迭代点，通过六自由度手腕偏置串联机器人运动学正解求解公式迭代计算，不断地更新迭代点，最后获得满足实际精度需求的运动学逆解数值解。本算法计算量较小，收敛速度更快,效率更高,对于机器人控制器来说负担较小，能提高响应速度，实时性更好。本算法在选择迭代法的初始点时，在六自由度手腕无偏置串联机器人运动学逆解的基础上，有针对性地将迭代初始点选择在六自由度手腕偏置串联机器人运动学逆解期望数值解的附近，这样做的好处，其一，能够在很大程度保证，迭代收敛于附近的期望数值解，因此有效避免了收敛于其它不合理的运动学逆解；其二，本算法计算量少，迭代收敛的速度快，效率高，对机器人控制器来说运算负担少，实时运算响应快，能够提高生产效率。其三，无论是附图中的手腕侧偏机器人，还是其它不满足Pieper准则的六自由度串联机器人，利用对应的六自由度手腕无偏置串联机器人运动学逆解作为迭代初始点，对求解六自由度手腕偏置串联机器人运动学逆解数值解均有上述优点。因此本算法，也可用于求解其它手腕偏置类型的六自由度串联机器人的运动学逆解数值解。附图说本文档来自技高网...

【技术保护点】
一种六自由度手腕偏置串联机器人的运动学逆解求解方法，其特征在于：包括以下步骤：A.将六自由度手腕偏置串联机器人末端坐标系的位姿当作六自由度手腕无偏置串联机器人末端坐标系的位姿，求得六自由度手腕无偏置串联机器人运动学逆解的解析解θ′1～θ′6；B.将解析解θ1′～θ6′作为所述六自由度手腕偏置串联机器人的运动学逆解的近似解和迭代法计算的出发点，由六自由度手腕偏置串联机器人的运动学正解求解公式，求得末端坐标系的近似位姿；C.计算得到末端坐标系期望位姿相对于末端坐标系近似位姿的位姿增量为dX，利用dX计算末端位姿综合误差为e；D.判断综合误差e是否在合理误差范围内，如果是，则停止迭代，将返回此时的关节变量数值作为六自由度手腕偏置串联机器人运动学逆解的迭代数值解；如果否，则利用六自由度手腕偏置串联机器人的雅可比矩阵求得关节变量θ′，并将新的角度变量θ′代入步骤B及步骤C，进行迭代计算，直到满足合理误差范围或者达到最大迭代次数，返回此时的关节变量数值作为六自由度手腕偏置串联机器人运动学逆解的迭代数值解。

【技术特征摘要】
1.一种六自由度手腕偏置串联机器人的运动学逆解求解方法，其特征在于：包括以下步骤：A.将六自由度手腕偏置串联机器人末端坐标系的位姿当作六自由度手腕无偏置串联机器人末端坐标系的位姿，求得六自由度手腕无偏置串联机器人运动学逆解的解析解θ′1～θ′6；B.将解析解θ1′～θ6′作为所述六自由度手腕偏置串联机器人的运动学逆解的近似解和迭代法计算的出发点，由六自由度手腕偏置串联机器人的运动学正解求解公式，求得末端坐标系的近似位姿；C.计算得到末端坐标系期望位姿相对于末端坐标系近似位姿的位姿增量为dX，利用dX计算末端位姿综合误差为e；D.判断综合误差e是否在合理误差范围内，如果是，则停止迭代，将返回此时的关节变量数值作为六自由度手腕偏置串联机器人运动学逆解的迭代数值解；如果否，则利用六自由度手腕偏置串联机器人的雅可比矩阵求得关节变量θ′，并将新的角度变量θ′代入步骤B及步骤C，进行迭代计算，直到满足合理误差范围或者达到最大迭代次数，返回此时的关节变量数值作为六自由度手腕偏置串联机器人运动学逆解的迭代数值解。2.根据权利要求1所述的一种六自由度手腕偏置串联机器人的运动学逆解求解方法，其特征在于：在所述步骤A之前，以六自由度手腕无偏置串联机器人的关节1旋转轴与关节2旋转轴交点为原点，关节1旋转轴所在直线为坐标系Z轴，关节2旋转轴所在直线为坐标系Y轴，建立基座坐标系；从基座往机器人末端方向，根据六个自由度依次建立6个坐标系，分别为坐标系{0}～坐标系{6}，其中坐标系{0}即所述基座坐标系{0}，坐标系{6}即末端坐标系{6}。3.根据权利要求1所述的一种六自由度手腕偏置串联机器人的运动学逆解求解方法，其特征在于，所述步骤A包括以下步骤：A1.设所述六自由度手腕偏置串联机器人的末端坐标系{6}原点的期望位置为(Xd，Yd，Zd)，末端坐标系{6}的期望姿态矩阵为RXYZ(αd，βd，γd)，参考系为基座坐标系{0}，将六自由度手腕偏置串联机器人末端坐标系的位姿当作六自由度手腕无偏置串联机器人末端坐标系的位姿，利用六自由度手腕无偏置串联机器人的运动学逆解求解方法，求解六自由度手腕无偏置串联机器人运动学逆解的解析解(θ1′～θ6′)；先计算运动学逆解的前三个角度θ1′、θ2′和θ3′：由于无偏置手腕满足Pieper准则，并根据坐标系齐次变换矩阵，求得：其中解上述方程组可求得θ1′、θ2′和θ3′的解析解其中为无偏置手腕末端位置点的向量表达式，a2为坐标系{2}Z轴与坐标系{3}Z轴在坐标系{2}X轴方向的距离，d2为为坐标系{1}X轴与坐标系{2}X轴在坐标系{2}Z轴方向的距离，d3为坐标系{2}X轴与坐标系{3}X轴在坐标系{3}Z轴方向的距离，d4为坐标系{3}X轴与坐标系{4}X轴在坐标系{4}Z轴方向的距离；A2.通过欧拉角XYZ(αd，βd，γd)变换矩阵RXYZ(αd，βd，γd)求得六自由度手腕无偏置串联机器人运动学逆解的后三个角度θ4′、θ5′和θ6′：<...

【专利技术属性】
技术研发人员：周星，陈统书，黄石峰，陈思敏，杨林，邓旭高，杨海滨，王群，
申请(专利权)人：佛山华数机器人有限公司，佛山智能装备技术研究院，
类型：发明
国别省市：广东,44