柔性机械臂在线臂形规划方法

技术领域

本申请属于机器人自动控制技术领域，涉及柔性机械臂臂形控制，尤其涉及在特定环境中运动过程中的柔性机械臂臂形规划问题。

背景技术

柔性机械臂在给定初始状态和目标状态的前提下，可通过逆运动学模型求解计算出柔性机械臂的驱动空间解，进而控制柔性臂运动至指定位置。但是，在特定环境(例如空间存在障碍物)的运动过程中，如何进行臂形规划以对柔性机械臂的臂形进行控制，使其在运动过程中不会出现与环境发生碰撞，同时能完成从初始状态至目标状态的运动，是一个尚待解决的问题。

申请号为CN202110500442.2的申请文件公开了基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，首先建立刚性冗余机械臂正逆运动学模型，然后通过逆运动学模型求解目标状态下的机械臂关节角，再将障碍物向机械臂的关节空间映射，得到冗余机械臂在关节空间内的无碰撞自由空间，随后在该无碰撞自由空间内建立搜索树，根据选取规则有条件地选取随机采样节点，最终在无碰撞自由空间内规划轨迹。

然而，由于刚性机械臂的空间臂形与关节角之间存在线性化数学联系，在将障碍物向机械臂的关节空间映射的过程不具有普适性，因此上述冗余机械臂避障轨迹规划方法只适用于刚性机械臂，即该方法无法适用于机械臂本体可变形的柔性机械臂装置。而且，由于该方法在属于离线式轨迹规划，必须先构建描述机械臂和障碍物之间空间位置关系的自由-碰撞地图，才能利用现有规划方案在该二维栅格地图中进行臂形规划，无碰撞自由空间的获取是一个计算耗时而又非必要的环节，存在计算量大的弊端。

发明内容

本发明目的旨在提供一种柔性机械臂在线臂形规划方法，解决障碍物空间中末端位姿导向下的带有形变特性的柔性机械臂臂形规划问题。

为达到上述目的，本发明采取以下技术方案来实现。

本发明提供的柔性臂在线臂形规划方法，应用RRT规划出臂形空间的无碰撞路径，同时生成在线碰撞-自由地图；结合柔性臂逆运动学模型，得到柔性臂控制信号，实现柔性臂在末端位姿导向的过程中无碰撞地运动。

基于上述分析，本发明提供的柔性机械臂在线臂形规划方法，具体包括以下步骤：

S1依据柔性机械臂初始末端位姿和目标末端位姿，通过柔性机械臂逆运动学模型得到柔性机械臂初始臂形和目标臂形；

S2基于快速扩展随机树算法从包含柔性机械臂和障碍物的空间中确定连接初始臂形和目标臂形之间的若干有效臂形序列，构成柔性机械臂的无碰撞路径规划方案，同时生成在线碰撞-自由地图；以柔性机械臂臂形作为待生成随机树中的节点；该步骤包括以下分步骤：

S20对随机树进行初始化；

S21在包含柔性机械臂和障碍物的空间进行采样，得到一个随机节点q

S22寻找随机树中与随机节点q

S23基于随机节点q

S24判断候选节点q

S25判断目标臂形与生成的有效节点q

S26迭代次数增加1，并判断是否达到最大迭代次数；若是，则无碰撞路径规划失败，返回步骤S20，重新对随机树进行初始化；否则返回步骤S21，同时构建包含节点q

上述步骤S1中，柔性机械臂逆运动学模型没有特别限制，这里使用的柔性机械臂逆运动学模型如下：

式中，(X

依据柔性机械臂初始末端位姿和目标末端位姿，通过求解上述逆运动学模型，可以得到柔性机械臂初始臂形和目标臂形。

上述步骤S2中，目的是基于快速扩展随机树算法(Rapidly-Exploring RandomTrees，RRT算法)和柔性机械臂与障碍物空间信息(包括柔性机械臂与障碍物尺寸及位置信息)，确定连接初始臂形和目标臂形之间的若干有效臂形序列，形成柔性机械臂的无碰撞路径规划方案。

上述快速扩展随机树算法中，以T＝(V，E)表示随机树，V表示随机树中的节点集合，E表示结点集合中各节点之间的边。本发明在包含柔性机械臂和障碍物的空间中，基于初始臂形、目标臂形、柔性机械臂与障碍物以及RRT算法本身的一些初始化参数(包括设定的扩展概率p、扩展距离d、终节点误差q

上述步骤S21中，基于当前臂形、目标臂形、柔性机械臂与障碍物空间信息以及设定的扩展概率，通过SampleFree()函数，在柔性机械臂在线碰撞-自由地图上进行采样，得到一个随机节点q

上述步骤S22中，基于随机树和步骤S21得到的随机节点q

上述步骤S23中，基于随机节点q

上述步骤S24中，在对候选节点q

上述步骤S25和S26中，是以目标臂形为导向的过程中，所寻找的候选节点会按照预设精度逐步逼近目标臂形q

步骤S25中，当目标臂形与生成的有效节点(即有效臂形)满足||q

步骤S26中，是为了判断是否规划失败。当达到最大迭代次数，表明目前规划方案是失败的，需要从柔性机械臂初始臂形重新开始规划。当未到达最大迭代次数时，表明可以继续对柔性机械臂路径进行规划；此时，可以直接基于得到的当前有效节点q

基于柔性机械臂的无碰撞路径规划方案，通过柔性机械臂逆运动学模型得到柔性机械臂驱动信号。本发明中，基于柔性机械臂的无碰撞路径规划方案可以得到相邻两个臂形之间的臂形变量，然后通过关节空间与驱动空间的逆运动学关系得到转动角速度，如下式所示：

式中，

本发明中，柔性机械臂是由驱动柔性机械臂中的驱动丝来带动柔性机械臂运动。因此，基于柔性机械臂的转动角速度，可按照以下公式得到与驱动丝连接的丝杆驱动电机转速(即驱动信号)，进而实现对柔性机械臂的驱动：

式中，D表示丝杆导程，即丝杆上螺母转动一圈螺母行走的直线距离。

式中，

对于前面给出的实时驱动信号生成过程类似，基于当前有效节点q

本发明主要针对柔性机械臂的障碍物空间避障问题，在臂形空间采用在线臂形规划方法解决，本发明提供的柔性机械臂在线臂形规划方法具有以下有益效果：

1)本发明首先基于柔性机械臂初始末端位姿和目标末端位姿获取柔性机械臂初始臂形和目标臂形；然后通过快速扩展随机树算法，得到柔性机械臂初始臂形和目标臂形的若干有效臂形序列，构成局部柔性机械臂的无碰撞路径规划方案，解决了具有变形特性的柔性机械臂臂形在线规划问题；

2)本发明不需要预先构建完全自由-碰撞地图，可以在得到有效臂形节点的同时，结合其之前生成的有效臂形节点和初始臂形构建实时在线碰撞-自由地图，从而节省大量计算时间成本；

3)本发明通过不断构建局部的有效臂形，来逐步逼近目标臂形，可提升柔性机械臂的无碰撞路径规划方案的准确率；

4)本发明通过构建的实时在线碰撞-自由地图，可以辅助操作人员对柔性机械臂运动进行校核，完成对柔性机械臂的闭环控制。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为柔性机械臂物理实体结构示意图；其中，(a)为整体结构示意图，(b)为公环示意图，(c)为母环示意图。

图2为柔性机械臂臂形空间与臂形配置序列空间所属关系。

图3为柔性机械臂在线臂形规划方法流程示意图。

图4为柔性机械臂运动矢量图。

图5为应用例中构建的多段柔性机械臂多障碍体避障场景。

图6为应用例中一段柔性机械臂在多障碍体空间的在线臂形规划结果。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

以下实施例中针对的柔性机械臂物理实体，如图1所示，其包括经连接体4连接在一起的柔性臂第一段1、柔性臂第二段2以及穿过柔性臂第一段和柔性臂第二段的弹性支撑体7；柔性臂第二段末端为末端关节5(用于表征末端位姿)。柔性臂第一段1和柔性臂第二段2结构相同，均包括若干万向节3和将若干万向节串联在一起的三根驱动丝。相邻两个万向节之间通过虎克铰链方式连接。柔性机械臂第一段中驱动丝一端穿过基座6与相应驱动电机相连，另一端与连接体4固连。柔性臂第二段2中驱动丝一端穿过基座6与相应驱动电机相连，另一端与末端关节5固连。

如图1所示，万向节3节具有两种结构：公环31和母环32，公环31和母环32交错排布。公环31两侧环面上分别设置有凸台312，且两个环面上的凸台位置相互垂直；母环32两侧环面上分别开设有凹槽322，且两个环面上的凹槽位置相互垂直，从而使万向节两侧与相邻万向节之间沿水平转轴(A轴)和垂直转轴(B轴)旋转。母环环面上开设的凹槽与其相对的公环环面设置的凸台相适配。母环两侧端面和公环两侧端面均采用斜面设计，以使公环能绕凹槽中心具有一定的旋转角度；具体的，母环两侧端面和公环两侧端面均设计有自中间向两侧倾斜的定位面，这样可以使母环和公环构成的第一柔性臂/第二柔性臂活动具有较大的活动范围。

连接体4和末端关节5和与之连接的万向节(公环31或母环32)端面结构适配。当与连接体4/末端关节5连接的为公环31时，其端面与母环端面结构一致；当与连接体4/末端关节5连接的为母环32时，其端面与公环端面结构一致。

实施例1

这里，首先统一柔性机械臂臂形规划问题的相关定义。定义

而本发明提供的柔性机械臂在线臂形规划方法只需经过①③⑤，不仅节省了从①到②的大量计算时间，还可以直接从更紧凑的③中直接探索得到⑤，从而大量降低臂形规划方案计算用时，提高搜索效率和精准度。

本发明结合算法RRT，给出的柔性机械臂在线臂形规划方法，如图3所示，包括以下步骤：

S1依据柔性机械臂初始末端位姿和目标末端位姿，通过柔性机械臂逆运动学模型得到柔性机械臂初始臂形和目标臂形。

本实施例中，使用的柔性机械臂逆运动学模型如下：

式中，(X

依据柔性机械臂初始末端位姿

S2基于快速扩展随机树算法从包含柔性机械臂和障碍物的空间中确定连接初始臂形和目标臂形之间的若干有效臂形序列，构成柔性机械臂的无碰撞路径规划方案，同时生成在线碰撞-自由地图。

本步骤目的是基于快速扩展随机树算法和柔性机械臂与障碍物空间信息(包括柔性机械臂与障碍物尺寸及位置信息)，确定连接初始臂形和目标臂形之间的若干有效臂形序列，形成柔性机械臂的无碰撞路径规划方案。

上述快速扩展随机树算法中，以T＝(V，E)表示随机树，V表示随机树中的节点集合，E表示结点集合中各节点之间的边。本发明以柔性机械臂臂形作为树中的节点，基于初始臂形、目标臂形、柔性机械臂与障碍物空间信息以及RRT算法本身的一些初始化参数(包括设定的扩展概率p、扩展距离d、终节点误差q

结合表1，本步骤具体包括以下分步骤：

S20对随机树进行初始化。

这里，包括对随机树空间(即包含柔性机械臂和障碍物的空间)以及初始化参数(包括设定的扩展概率p、扩展距离d、终节点误差q

S21在包含柔性机械臂和障碍物的空间进行采样，得到一个随机节点q

本步骤中，基于当前臂形、目标臂形、柔性机械臂与障碍物空间信息以及设定的扩展概率，通过SampleFree()函数，在柔性机械臂在线碰撞-自由地图上进行采样，得到一个随机节点q

S22寻找随机树中与随机节点q

本步骤中，基于随机树和步骤S21得到的随机节点q

S23基于随机节点q

本步骤中，基于随机节点q

S24判断候选节点q

在对候选节点q

S25判断目标臂形与生成的有效节点q

当目标臂形与生成的有效节点(即有效臂形)满足||q

可以进一步基于柔性机械臂的无碰撞路径规划方案，获取柔性机械臂驱动信号，完成对柔性机械臂的驱动。同时，还可以构建包含连接初始臂形和目标臂形之间的若干有效臂形序列的在线碰撞-自由地图，用于辅助操作人员对柔性机械臂运动进行校核，完成对柔性机械臂的闭环控制。

S26迭代次数增加1，并判断是否达到最大迭代次数；若是，则无碰撞路径规划失败，返回步骤S20，重新对随机树进行初始化；否则返回步骤S21，同时构建包含节点q

本步骤是为了判断是否规划失败。迭代次数增加1(即k←k+1)后，当达到最大迭代次数，表明目前规划方案是失败的，需要从柔性机械臂初始臂形重新开始规划。当未到达最大迭代次数时，表明可以继续对柔性机械臂路径进行规划；此时，可以直接基于得到的当前有效节点q

可以进一步基于柔性机械臂的无碰撞路径规划方案，获取柔性机械臂驱动信号，完成对柔性机械臂的驱动。

表1柔性机械臂在线臂形规划RRT算法伪代码

基于柔性机械臂的无碰撞路径规划方案，可以得到相邻两个臂形之间的臂形变量。如图4所示，然后通过关节空间与驱动空间的逆运动学关系得到转动角速度，如下式所示：

式中，

本发明中，柔性机械臂是由驱动柔性机械臂中的驱动丝来带动柔性机械臂运动。因此，基于柔性机械臂的转动角速度，可按照以下公式得到与驱动丝连接的丝杆驱动电机转速(即驱动信号)，进而实现对柔性机械臂的驱动：

式中，D表示丝杆导程，即丝杆上螺母转动一圈螺母行走的直线距离；

式中，

r为每段柔性机械臂3根驱动丝通过的过孔所在分度圆的半径。

将公式(6)、(7)、(8)代入(5)即可求得

进而通过公式(4)可得到与驱动丝连接的丝杆驱动电机的转速，即柔性机械臂的驱动信号，从而实现对柔性机械臂的驱动。

对于前面给出的实时驱动信号生成过程类似，基于当前有效节点q

应用例

为验证本发明提供的柔性机械臂在线臂形规划方法的有效性，构建如图5所示为多段柔性臂在多障碍体空间的避障场景(参数见表2和表3所示)，柔性机械臂需要完成从初始末端位姿至目标末端位姿的运动，且运动过程需要规划双段臂形避开空间中的两个障碍物。按照实施例1提供的柔性机械臂在线臂形规划方法，进行柔性机械臂在线规划，图6示为其中一段柔性机械臂在多障碍体空间内的在线臂形规划结果，图中红色曲线即为规划完成后，相应段柔性机械臂在臂形空间内的运动轨迹，零星的黑色块区域即为在线建图的结果，从图中可以看出通过在线规划机制，无需构建全局自由-碰撞地图，因此整体计算时间上也将大大降低。

表4示出了本应用例和传统离线方案的计算用时对比情况。其中传统离线方案实现方式参见CN202110500422.2。从表4中可以看出，本应用例能够极大缩短柔性机械臂臂形规划时间，提高臂形规划效率，满足柔性机械臂在线实时规划需求。

表2两段式柔性机械臂参数

表3障碍物参数

表4两种不同臂形规划方案计算耗时情况对比

注：上述方案平均是指10次仿真计算的平均总规划时间；

上述建图平均是指10次仿真计算的建图环节平均时间。

由此可见，传统臂形规划方案只适用于有限自由度的刚性机械臂或多冗余机械臂，该类方法依赖于机械臂刚性关节角的转动角度数据，因此无法适用于本发明中所针对的柔性机械臂；传统臂形规划方案必须先构建描述机械臂和障碍物之间空间位置关系的自由-碰撞地图，才能利用现有规划方案在该二维栅格地图中进行臂形规划，计算相当耗时，而本方案利用在线机制完成臂形规划的同时能实现局部地图的构建，不需要再花费额外的时间去建图，因此节省了大量因计算构建栅格地图的时间。

综上所述，本发明提供的柔性机械臂在线臂形规划方法，最大的特点是：

1)将传统的离线式碰撞-自由地图的构建改进为在线式构建方法，提高了整体规划效率；

2)解决了类似具有形变特性的柔性机械臂臂形规划问题。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。