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

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


背景技术：

2.空间中任意位置的描述需要六个自由度，其中三个位置自由度，三个姿态自由度。六轴pieper构型机械臂由于存在六个自由度，所以能够通过运动规划以任意姿态到达工作空间中的任意点；然而六自由度机械臂逆解并不唯一，在非奇异点最多存在8组解，需要对多组解进行筛选，得到最优逆解。
3.求解机械臂逆解常采用的是解析解与数值解两种方法，相比于数值解，解析解有求解精度高，求解速度快等优点。然而，由于pieper构型机械臂结构的特殊性(存在三轴平行或三轴互相垂直)导致同一末端位置产生的逆解并不唯一，针对多组逆解，如何选取最优解是机械臂学者们一直在研究的问题。然后，学者们对最优解选取的策略多采用的是最短路径策略，即只考虑机械臂的规划耗时并没有考虑末端姿态是否符合安全规范。
4.机械臂在运动规划时，不健康的运动学逆解会导致末端姿态的不安全，增大规划任务失败的可能性，同时也可能会伤及工程人员。因此，如何筛选出既能保证姿态安全又能保证耗能最低的解，确保规划的快速性以及安全性是必须要考虑的问题。这极大减少了机械臂运动学规划的风险同时一定程度上降低能量损耗，所以既考虑最短路径又考虑姿态安全的逆解筛选策略是十分重要的。


技术实现要素：

5.本发明提供一种基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，筛选出既能保证姿态安全又能保证耗能最低的解，确保了规划的快速性以及安全性。
6.为实现以上目的，本发明采用以下技术方案：
7.一种基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，包括以下步骤：
8.s1:基于机械臂的二维模型，根据正运动学与几何参数建立机械臂d-h(modify)连杆坐标模型，使用建立逆解方程组；
9.s2：面向末端姿态规范的需求，提出安全姿态筛选策略，将8组逆解的末端姿态分为三类，通过对三类姿态进行分析，得出符合末端安全姿态的充分条件，并进一步得到joint2与joint3对应的关节角度符合安全末端姿态充分条件，实现对末端安全姿态的筛选；
10.s3：面向最短路径的需求，提出能量函数最优策略，根据起始位姿与终止位姿对应的关节角度构建能量函数，将s2中得到的符合安全姿态要求之后的解通过能量函数最优策
略，得到既符合安全姿态又满足能量最优的运动学逆解。
11.有益效果：本发明提供了一种基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，针对现有的六轴pieper构型机械臂解析解求解以及最优解挑选策略，改进了六自由度pieper构型机械臂运动学逆解求解算法，有效简化了方程组复杂度，降低了推导运动学解析解的难度，降低求解逆解的难度，简化了解析解符号表达式易于编程实现，使得逆解结果表达式更加简洁；本发明旨在为逆解求解和筛选提供一种新的解决办法，不同于现有技术中基于建立逆解方程组用于求解机械臂运动学逆解，提出使用建立逆解方程组，结合机械臂构型，提出了运动学逆解安全筛选策略，将运动学8组逆解姿态分为3类，根据常见抓取位姿对3类姿态进行分析，得出符合安全抓取姿态的条件；针对运动规划能耗低，规划时间短的需求，引入能量函数来计算经过安全姿态筛选的解对应的损耗能量，从而挑选出能量函数最优的运动学逆解。仿真实验表明，对于六轴pieper构型机械臂，本发明能够有效的求解pieper构型机械臂运动学逆解，并挑选出考虑安全姿态和能量函数最优的运动学逆解，有效保障了抓取姿态的安全性及有效降低规划的能量损耗；逆解算法在逆解存在奇异位形导致的少解状况仍可正常运行，具有一定的鲁棒性；为求解能耗最优与姿态安全的运动学逆解提供完备的策略。
附图说明
12.图1为本发明实施例中6-dof机械臂d-h运动学模型；
13.图2为本发明实施例中机械臂2，3轴姿态分类；
14.图3为本发明实施例中解析解分布；
15.图4为本发明实施例中基于安全姿态筛选与能量函数最优的逆解算法伪代码；
16.图5为本发明实施例中matlab gui仿真界面；
17.图6为本发明实施例中逆解结果；
18.图7为本发明实施例中插值轨迹规划结果示意图，其中(a)为机械臂轨迹规划结果，(b)为机械臂各个关节角度规划结果；
19.图8为本发明实施例中机械臂运动学逆解算法示意图；
20.图9为本发明实施例中安全位姿筛选算法与能量函数最优算法示意图。
具体实施方式
21.下面结合附图和具体实施例对本发明进行详细说明：
22.一种基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，包括以下步骤：
23.1、改进运动学解析解求解
24.(1)六自由度机械臂运动学建模
25.基于机械臂的二维模型，根据正运动学与几何参数建立如图1所示的机械臂d-h(modify)连杆坐标模型，其中d-h参数数值如表1所示：
26.表1 d-h(modify)参数表
[0027][0028]
其中d-h参数表中各项参数定义为：
[0029]
(1)a
i-1
：将(x
i-1
，y
i-1
，z
i-1
)沿x
i-1
的方向平移a
i-1
距离，使得z
i-1
轴与zi轴重合；
[0030]
(2)α
i-1
：将(x
i-1
，y
i-1
，z
i-1
)绕x
i-1
轴的右手螺旋方向旋转α
i-1
角度，使得z
i-1
轴与zi轴平行；
[0031]
(3)di：将(x
i-1
，y
i-1
，z
i-1
)沿zi方向平移di距离，使得x
i-1
轴与xi轴重合；
[0032]
(4)θi：关节i绕zi轴右手螺旋方向旋转的角度；
[0033]
机械臂连杆之间的齐次变换矩阵为：
[0034][0035]
机械臂的正运动学方程为：
[0036][0037]
(2)逆运动学求解
[0038]
机器人的2、3、4轴相互平行，由pieper准则中运动学逆解存在的充分条件可知，该机械臂满足pieper构型，因此机械臂逆运动学存在封闭解，逆解的推导过程基于常规的解析解推导，改进出一种简洁的逆解推导过程：
[0039]
[0040]
其中，c1＝cos(θ1)，c
23
＝cos(θ2+θ3)，c
234
＝cos(θ2+θ3+θ4)，s1＝sin(θ1)，s
23
＝sin(θ2+θ3)，s
234
＝sin(θ2+θ3+θ4)，n，o，a为旋转矩阵对应列向量，p为位置向量；
[0041]
令(3)式矩阵元素与分别相等，得：
[0042][0043]
令其中a、b、r、为中间变量，用于求解θ1；则：
[0044][0045]
当θ1存在解：
[0046][0047]
其中，atan2(x，y)为可以根据x和y的符号自动切换象限的反正切函数，求出θ1后，可以基于θ1求取θ5：
[0048][0049]
令(3)式矩阵元素与分别相等(θ5≠0)，得：
[0050][0051]
令(3)式矩阵元素与分别相等得：
[0052][0053]
令(3)式矩阵元素与分别相等得：
[0054][0055]
令p
x
c1+pys1+d5s
234-d6c
234
s5＝b1，p
z-d
1-d5c
234-d6s
234
s5＝b2，可得：
[0056][0057]
令其中b1、b2、β为中间变量用于求解θ2，当可得：
[0058][0059]
根据求解得到的θ2可以解得θ3：
[0060][0061]
最后根据求得的θ2，θ3求解θ4：
[0062]
θ4＝(θ2+θ3+θ4)-(θ2+θ3)
ꢀꢀꢀ
(14)
[0063]
至此，六个关节角度求解完毕，共产生8组逆解，下一小节将对8组逆解进行分析并选取最优解；
[0064]
2、安全姿态筛选技术
[0065]
在轨迹规划之前需要先选择好最适合的一个解，要考虑到轨迹规划之后以及过程中关节位姿的安全性以及规划时间，能耗等，在引入选取最优解算法之前需要先对8组解的性质进行分析，为算法提供理论基础。
[0066]
基于逆解进行轨迹规划，不能只考虑规划的速度，更应该考虑安全性，否则可能会造成严重的工程安全事件，考虑进行平面抓取工作的工作状态，图3中的第一组解与第8组解并不适合进行抓取工作，即便其与初始角度之间的距离可能是最小。
[0067]
影响机械臂整体位姿的主要因素是joint2与joint3，对不安全解排除的策略基于常用的平面抓取姿态，对逆解获得的姿态进行分析。
[0068]
首先对joint2与joint3的取值进行分类，不考虑joint1的取值，joint2与joint3的分类结果有：
[0069]
(1)joint2≥0，joint3≥0
[0070]
(2)joint2≥0，joint3≤0
[0071]
(3)joint2≤0，joint3≥0
[0072]
(4)joint2≤0，joint3≤0
[0073]
考虑待抓取物体在机械臂正前方，此时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的角度，满足安全姿态充分条件对应的关节角度为安全姿态解。
[0074]
进而对安全姿态的充分条件进行改进可得，满足逆解姿态为安全姿态的充分条件为：
[0075]
(θ2+θ3≤0 and θ3≤0)or(θ2+θ3≥0 and θ3≥0)
[0076]
考虑末端位姿为：
[0077]
位置：[p
x
，py，pz]＝[121.5
ꢀ‑
368.37 701.37]单位：cm
[0078]
姿态：[r
x
，ry，rz]＝[-π 0 0]单位：rad
[0079]
产生的四组解如图(3)所示(此处存在4组奇异位形)。
[0080]
其中第一组解joint1位于π半区，此时joint2≥0，joint3≤0；第二组解joint1位于π半区，此时joint2≤0，joint3≤0；第七组解joint1位于0半区，此时joint2≥0，joint3≥0；第八组解joint1位于0半区，此时joint2≤0，joint3≥0；根据安全姿态的关节角充分条件可以得出以上四组解满足安全姿态充分条件的解为第二组解与第七组解；得到符合安全姿态要求之后的解通过下一小节能量函数最优策略即可得到既符合安全姿态又满足能量最优的运动学逆解。
[0081]
3、最短距离算法
[0082]
(1)最小行程法
[0083]
根据最小行程法准则，通常只有一组最优解，因此构建以关节角度距离为单位的能量函数，该函数既考虑关节角度距离又考虑不同质量关节单位角度运动下的能量损耗：
[0084][0085]
其中m(n)为n连杆的质量，theta
safe
为其中一组安全解，theta
start
为起始关节角度；
[0086]
基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解算法伪代码如图(4)，考虑：
[0087]
位置初始：θ＝[0 0 0 0 0 0]
[0088]
末端位置：[p
x
，py，pz]＝[121.5
ꢀ‑
368.37 701.37]单位：cm
[0089]
末端姿态：[r
x
，ry，rz]＝[-π 0 0]单位：rad
[0090]
图(4)所示算法的结果如表2，此时既考虑姿态安全又考虑能量最优的解为第七组解，逆解结果如图(6)所示。
[0091]
表2逆解算法求解结果
[0092][0093][0094]
(2)最优解的连续性
[0095]
根据机械臂运动的连续性可得：在不考虑经过奇异位形与工作边界时，机械臂沿预定路径连续运动过程中，轨迹规划计算插补点逆解选用的解序列与末端点对应解序列一致时，既能满足姿态安全，能量损耗最小，且关节速度不会出现突变。
[0096]
证明如下：
[0097]
解析解描述为末端位姿到关节角度的映射函数其中i为最优解对应的序列号，映射函数的微分特性为：
[0098][0099]
其中δt为绕基座标系微分旋转矢量，d为微分运动矢量，因此δt
→
0时，有dt
→
0，根据初等函数的连续性可知，当δt
→
0趋于0时fi(t+δt)-fi(t)
→
0；当序列号不一致时，出现关节角速度上的不连续，且不满足能量函数最优甚至导致末端姿态不安全。运动学插值规划结合三次轨迹规划效果如图7(a)，其中初始角度为[0 0 0 0 0 0]，末端逆解角度为插值点对应逆解角度为各个关节角度规划结果如图7(b)，其中插补点在t＝20s，可以看出引入插补点对整体的角度变化轨迹影响很小，轨迹十分平滑，这是因为插补点逆解序列与终点选取的序列一致均为最优解。
[0100]
以上仅是本发明的优选实施例，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

技术特征：
1.一种基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，其特征在于，包括以下步骤：s1：基于机械臂的二维模型，根据正运动学与几何参数建立机械臂d-h连杆坐标模型，使用建立逆解方程组，求解关节角度产生多组逆解；s2：面向末端姿态规范的需求，提出安全姿态筛选策略，将s1中得到的多组逆解的末端姿态进行分类，通过对姿态进行分析，得出满足逆解姿态为安全姿态的充分条件，并进一步得到joint2与joint3对应的关节角度符合安全末端姿态充分条件，实现对末端安全姿态的筛选；s3：面向最短路径的需求，提出能量函数最优策略，根据起始位姿与终止位姿对应的关节角度构建能量函数，将s2中得到的符合安全姿态要求之后的解通过能量函数最优策略，得到既符合安全姿态又满足能量最优的运动学逆解。2.根据权利要求1所述的基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，其特征在于，s1中使用建立的逆解方程组为：其中：c1＝cos(θ1)，c
23
＝cos(θ2+θ3)，c
234
＝cos(θ2+θ3+θ4)。3.根据权利要求2所述的基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，其特征在于，s1中通过逆解方程组求解六个关节角度产生8组逆解。4.根据权利要求3所述的基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，其特征在于，求解的过程为：令(3)式矩阵元素与分别相等，得：令则：当θ1存在解：
其中，atan2(x，y)为可以根据x和y的符号自动切换象限的反正切函数。求出θ1后，可以基于θ1求取θ5：令(3)式矩阵元素与分别相等(θ5≠0)，得：令(3)式矩阵元素与分别相等得：令(3)式矩阵元素与分别相等得：令p
x
c1+p
y
s1+d5s
234-d6c
234
s5＝b1，p
z-d
1-d5c
234-d6s
234
s5＝b2，可得：令当可得：根据求解得到的θ2可以解得θ3：最后根据求得的θ2，θ3求解θ4：
θ4＝(θ2+θ3+θ4)-(θ2+θ3)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)至此六个关节角度求解完毕。5.根据权利要求1或3所述的基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，其特征在于，s2中逆解的末端姿态分为三类，具体为：影响机械臂整体位姿的主要因素是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位姿参考机械臂运动学模型初始状态，其中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面向工作空间的姿态情况主要有三种。6.根据权利要求5所述的基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛选方法，其特征在于，s2中得出的满足逆解姿态为安全姿态的充分条件为：(θ2+θ3≤0 and θ3≤0)or(θ2+θ3≥0 and θ3≥0)。7.根据权利要求1所述的基于安全姿态筛选与能量函数最优的pieper构型机械臂逆解筛洗方法，其特征在于，s3中构建的能量函数为：其中m(n)为n连杆的质量，theta
safe
为其中一组安全解，theta
start
为起始关节角度。

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


技术研发人员：解明扬 吴伟 徐天天 张民 姜斌
受保护的技术使用者：南京航空航天大学
技术研发日：2023.05.22
技术公布日：2023/9/5