一种基于安全姿态筛选与能量函数最优的Pieper构型机械臂逆解筛选方法

摘要

本发明公开了一种基于安全姿态筛选与能量函数最优的Pieper构型机械臂逆解筛选方法，面向机械臂抓取姿态安全性需求，结合机械臂构型，提出了运动学逆解安全筛选策略，将运动学8组逆解姿态分为3类，根据常见抓取位姿对3类姿态进行分析，得出符合安全抓取姿态的条件；针对运动规划能耗低，规划时间短的需求，引入能量函数来计算经过安全姿态筛选的解对应的损耗能量，从而挑选出能量函数最优的运动学逆解。本发明有效保障了抓取姿态的安全性及有效降低规划的能量损耗。

说明书

技术领域

本发明属于机械臂运动学规划技术领域，具体涉及一种基于安全姿态筛选与能量函数最优的Pieper构型机械臂逆解筛选方法。

背景技术

空间中任意位置的描述需要六个自由度，其中三个位置自由度，三个姿态自由度。六轴Pieper构型机械臂由于存在六个自由度，所以能够通过运动规划以任意姿态到达工作空间中的任意点；然而六自由度机械臂逆解并不唯一，在非奇异点最多存在8组解，需要对多组解进行筛选，得到最优逆解。

求解机械臂逆解常采用的是解析解与数值解两种方法，相比于数值解，解析解有求解精度高，求解速度快等优点。然而，由于Pieper构型机械臂结构的特殊性(存在三轴平行或三轴互相垂直)导致同一末端位置产生的逆解并不唯一，针对多组逆解，如何选取最优解是机械臂学者们一直在研究的问题。然后，学者们对最优解选取的策略多采用的是最短路径策略，即只考虑机械臂的规划耗时并没有考虑末端姿态是否符合安全规范。

机械臂在运动规划时，不健康的运动学逆解会导致末端姿态的不安全，增大规划任务失败的可能性，同时也可能会伤及工程人员。因此，如何筛选出既能保证姿态安全又能保证耗能最低的解，确保规划的快速性以及安全性是必须要考虑的问题。这极大减少了机械臂运动学规划的风险同时一定程度上降低能量损耗，所以既考虑最短路径又考虑姿态安全的逆解筛选策略是十分重要的。

发明内容

本发明提供一种基于安全姿态筛选与能量函数最优的Pieper构型机械臂逆解筛选方法，筛选出既能保证姿态安全又能保证耗能最低的解，确保了规划的快速性以及安全性。

为实现以上目的，本发明采用以下技术方案：

一种基于安全姿态筛选与能量函数最优的Pieper构型机械臂逆解筛选方法，包括以下步骤：

S1:基于机械臂的二维模型，根据正运动学与几何参数建立机械臂D-H(modify)连杆坐标模型，使用

S2：面向末端姿态规范的需求，提出安全姿态筛选策略，将8组逆解的末端姿态分为三类，通过对三类姿态进行分析，得出符合末端安全姿态的充分条件，并进一步得到joint2与joint3对应的关节角度符合安全末端姿态充分条件，实现对末端安全姿态的筛选；

S3：面向最短路径的需求，提出能量函数最优策略，根据起始位姿与终止位姿对应的关节角度构建能量函数，将S2中得到的符合安全姿态要求之后的解通过能量函数最优策略，得到既符合安全姿态又满足能量最优的运动学逆解。

有益效果：本发明提供了一种基于安全姿态筛选与能量函数最优的Pieper构型机械臂逆解筛选方法，针对现有的六轴Pieper构型机械臂解析解求解以及最优解挑选策略，改进了六自由度Pieper构型机械臂运动学逆解求解算法，有效简化了方程组复杂度，降低了推导运动学解析解的难度，降低求解逆解的难度，简化了解析解符号表达式易于编程实现，使得逆解结果表达式更加简洁；本发明旨在为逆解求解和筛选提供一种新的解决办法，不同于现有技术中基于

附图说明

图1为本发明实施例中6-DOF机械臂D-H运动学模型；

图2为本发明实施例中机械臂2，3轴姿态分类；

图3为本发明实施例中解析解分布；

图4为本发明实施例中基于安全姿态筛选与能量函数最优的逆解算法伪代码；

图5为本发明实施例中MATLAB GUI仿真界面；

图6为本发明实施例中逆解结果；

图7为本发明实施例中插值轨迹规划结果示意图，其中(a)为机械臂轨迹规划结果，(b)为机械臂各个关节角度规划结果；

图8为本发明实施例中机械臂运动学逆解算法示意图；

图9为本发明实施例中安全位姿筛选算法与能量函数最优算法示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明：

一种基于安全姿态筛选与能量函数最优的Pieper构型机械臂逆解筛选方法，包括以下步骤：

1、改进运动学解析解求解

(1)六自由度机械臂运动学建模

基于机械臂的二维模型，根据正运动学与几何参数建立如图1所示的机械臂D-H(modify)连杆坐标模型，其中D-H参数数值如表1所示：

表1 D-H(modify)参数表

其中D-H参数表中各项参数定义为：

(1)a

(2)α

(3)d

(4)θ

机械臂连杆之间的齐次变换矩阵为：

机械臂的正运动学方程为：

(2)逆运动学求解

机器人的2、3、4轴相互平行，由Pieper准则中运动学逆解存在的充分条件可知，该机械臂满足Pieper构型，因此机械臂逆运动学存在封闭解，逆解的推导过程基于常规

其中，c

令(3)式矩阵元素

令

当

其中，atan2(x，y)为可以根据x和y的符号自动切换象限的反正切函数，求出θ

令(3)式矩阵元素

令(3)式矩阵元素

令(3)式矩阵元素

令p

令

根据求解得到的θ

最后根据求得的θ

θ

至此，六个关节角度求解完毕，共产生8组逆解，下一小节将对8组逆解进行分析并选取最优解；

2、安全姿态筛选技术

在轨迹规划之前需要先选择好最适合的一个解，要考虑到轨迹规划之后以及过程中关节位姿的安全性以及规划时间，能耗等，在引入选取最优解算法之前需要先对8组解的性质进行分析，为算法提供理论基础。

基于逆解进行轨迹规划，不能只考虑规划的速度，更应该考虑安全性，否则可能会造成严重的工程安全事件，考虑进行平面抓取工作的工作状态，图3中的第一组解与第8组解并不适合进行抓取工作，即便其与初始角度之间的距离可能是最小。

影响机械臂整体位姿的主要因素是joint2与joint3，对不安全解排除的策略基于常用的平面抓取姿态，对逆解获得的姿态进行分析。

首先对joint2与joint3的取值进行分类，不考虑joint1的取值，joint2与joint3的分类结果有：

(1)joint2≥0，joint3≥0

(2)joint2≥0，joint3≤0

(3)joint2≤0，joint3≥0

(4)joint2≤0，joint3≤0

考虑待抓取物体在机械臂正前方，此时joint1的取值有两种，分别是0和π，广义上分为0半区与π半区，机械臂0位姿参考图(1)机械臂运动学模型初始状态，其中(joint1＝0，joint2≥0，joint3≥0)与(joint1＝π，joint2≤0，joint3≤0)的2，3轴姿态一致；(joint1＝0，joint2≤0，joint3≥0)与(joint1＝π，joint2≥0，joint3≤0)的2，3轴姿态一致，然而当joint1＝0时不可能出现joint2≤0，joint3≤0，此时机械臂位姿与工作空间相反，同理当joint1＝π时不可能出现joint2≥0，joint3≥0；综上joint2，joint3面向工作空间的姿态情况主要有三种，如图(2)所示，姿态2、姿态3均为安全姿态，姿态1为非安全姿态，其中逆解姿态为安全姿态的充分条件为：Ψ≤π，其中Ψ为以joint3为圆心将连杆2逆时针转至连杆3的角度，满足安全姿态充分条件对应的关节角度为安全姿态解。

进而对安全姿态的充分条件进行改进可得，满足逆解姿态为安全姿态的充分条件为：

(θ

考虑末端位姿为：

位置：[p

姿态：[r

产生的四组解如图(3)所示(此处存在4组奇异位形)。

其中第一组解joint1位于π半区，此时joint2≥0，joint3≤0；第二组解joint1位于π半区，此时joint2≤0，joint3≤0；第七组解joint1位于0半区，此时joint2≥0，joint3≥0；第八组解joint1位于0半区，此时joint2≤0，joint3≥0；根据安全姿态的关节角充分条件可以得出以上四组解满足安全姿态充分条件的解为第二组解与第七组解；得到符合安全姿态要求之后的解通过下一小节能量函数最优策略即可得到既符合安全姿态又满足能量最优的运动学逆解。

3、最短距离算法

(1)最小行程法

根据最小行程法准则，通常只有一组最优解，因此构建以关节角度距离为单位的能量函数，该函数既考虑关节角度距离又考虑不同质量关节单位角度运动下的能量损耗：

其中m(n)为n连杆的质量，theta

基于安全姿态筛选与能量函数最优的Pieper构型机械臂逆解算法伪代码如图(4)，考虑：

位置初始：θ＝[0 0 0 0 0 0]

末端位置：[p

末端姿态：[r

图(4)所示算法的结果如表2，此时既考虑姿态安全又考虑能量最优的解为第七组解，逆解结果如图(6)所示。

表2逆解算法求解结果

(2)最优解的连续性

根据机械臂运动的连续性可得：在不考虑经过奇异位形与工作边界时，机械臂沿预定路径连续运动过程中，轨迹规划计算插补点逆解选用的解序列与末端点对应解序列一致时，既能满足姿态安全，能量损耗最小，且关节速度不会出现突变。

证明如下：

解析解描述为末端位姿到关节角度的映射函数

其中δt为绕基座标系微分旋转矢量，d为微分运动矢量，因此δt→0时，有dT→0，根据初等函数的连续性可知，当ΔT→0趋于0时f

以上仅是本发明的优选实施例，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。