CN116252303B - 一种绳驱连续型机器人快速运动规划系统 - Google Patents

Abstract

本发明公开一种绳驱连续型机器人快速运动规划系统，包括：终止参数定义模块、绳长向量定义模块、绳长距离计算模块、轨迹优化模块；终止参数定义模块基于机器人起始关节参数，确定终止关节参数；绳长向量定义模块基于起始关节参数和终止关节参数定义起始关节和终止关节对应绳长向量；绳长距离计算模块基于起始关节对应绳长向量与终止关节对应绳长向量计算绳长距离；轨迹优化模块基于参数正交化将绳长距离转化为空间距离；并基于空间距离获得最优轨迹。本发明可以为绳驱连续型机器人规划全局绳长最短路径，节约绳驱连续型机器人工作能耗；将末端电机转角优化问题转化为关节空间优化，简化绳驱连续型机器人多重空间之间的复杂映射问题。

Description

一种绳驱连续型机器人快速运动规划系统

技术领域

本发明属于机器人轨迹规划技术领域，具体涉及一种绳驱连续型机器人快速运动规划系统。

背景技术

绳驱连续型机器人由于具有高集成度和良好的环境适应性，在执行面向复杂环境的任务时具有天然优势，但目前仍然存在部分技术难题亟待解决。绳驱连续型机器人运动学规划方法不同于移动机器人。移动机器人在进行规划的时可以将机器人抽象为一个点，在笛卡尔空间内规划出一条从初始位置到目标重点的避障路径。而对于连续型机器人，由于在作业过程中需要保持整臂避障且连续型机器人臂形可以进行丰富变换，因此在规划时无法简单的抽象为单点。2010年Xiao等人针对平面连续型机器人提出了一种快速规划算法，并在样机上完成了物体抓取实验，证明了规划算法的可行性。2014年Palmer等人提出了一种基于路径跟随的蛇形机器人运动规划算法，并给出了一种创建目标函数的高效方法。但是，Xiao等人提出的方法仅适用于平面连续型机器人。Palmer提出的方法适用于电机驱动的关节式蛇形机器人。

对于连续型机器人，轨迹优化存在两个主要的难点：第一是由于机器人的连续型性，终止末端位姿往往对应关节空间中的一个流形，需要在流形上确定期望的机器人关节参数；第二是在确定的机器人初始关节参数与机器人终止关节参数间寻找最优轨迹。但是由于连续型机器人的运动学模型是非线性的，所以问题会转化为求解一个HJB方程，往往需要利用近似动态规划的工具求解。

发明内容

本发明旨在针对现有技术中存在的不足，提出一种绳驱连续型机器人的快速运动规划系统用于对绳驱连续型机器人全局绳长轨迹进行优化。

为实现上述目的，本发明提供了如下方案：

一种绳驱连续型机器人快速运动规划系统，包括：

终止参数定义模块、绳长向量定义模块、绳长距离计算模块、轨迹优化模块；

所述终止参数定义模块用于基于机器人起始关节参数，确定终止关节参数；

所述绳长向量定义模块用于基于所述起始关节参数和所述终止关节参数定义起始关节对应绳长向量和终止关节对应绳长向量；

所述绳长距离计算模块用于基于所述起始关节对应绳长向量与所述终止关节对应绳长向量计算绳长距离；

所述轨迹优化模块用于基于参数正交化将所述绳长距离转化为空间距离；并基于所述空间距离获得最优轨迹。

优选地，所述终止参数定义模块确定所述终止关节参数方法包括：

将笛卡尔坐标系下的机器人末端执行器的目标位姿通过逆运动学映射到机器人关节的广义空间中，确定机器人的所述终止关节参数。

优选地，所述绳长距离计算模块计算所述绳长距离的方法包括：

利用所述起始关节对应绳长向量和所述终止关节对应绳长向量的二范数定义关节参数空间的距离为所述绳长距离；

所述绳长距离包括：

式中，l ₁表示终止关节对应绳长向量，l ₀表示起始关节对应绳长向量，l _i,1表示终止关节参数对应绳长向量对应分量，l _i,0表示起始关节对应绳长向量对应分量。

优选地，所述轨迹优化模块将所述绳长距离转化为所述空间距离的方法包括：

将所述起始关节参数和所述终止关节参数转化为正交形式，得到正交化起始关节参数和正交化终止关节参数；

对所述正交化起始关节参数和所述正交化终止关节参数进行线性插值，得到所述空间距离。

优选地，所述线性插值的方法包括：

式中，表示正交参数/>线性插值对于x轴的分量，/>表示正交参数/>线性插值对于y轴的分量，/>表示正交起始关节参数，/>表示正交终止关节参数，t表示参变量。

优选地，所述空间距离包括：

式中，表示正交终止关节参数，/>表示正交起始关节参数。

与现有技术相比，本发明的有益效果为：

1.本发明可以为绳驱连续型机器人规划全局绳长最短路径，可大大节约绳驱连续型机器人工作能耗；

2.将末端电机转角的优化问题转化为关节空间优化，简化绳驱连续型机器人多重空间之间的复杂映射问题；

3.本发明将复杂的非线性目标规划问题转化为简单线性目标问题，提高计算效率，可以实现在线规划；

本发明适用于任意段数的绳驱连续型机器人，具有广泛适用性。

附图说明

为了更清楚地说明本发明的技术方案，下面对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例绳驱连续型机器人快速运动规划系统结构示意图；

图2为本发明连续型机器人示意图；

图3为本发明绳孔标号分布示意图；

图4为全局最优求解示意图；

图5为本发明基于变分法获得的绳长最短路径规划示意图；

图6为利用指标P衡量近似轨迹的效果示意图；

图7为绳长变化量最小目标下的近似全局最优规划示意图；

图8为具有关节限位约束的近似全局最优规划示意图；

图9为近似全局最优解的迭代曲线示意图；

图10为含关节限位约束的近似全局最优解的迭代曲线示意图；

图11为绳长变化量为优化目标的迭代曲线示意图；

图12为基于伪逆法的路径规划的迭代曲线示意图；

图13为局部最优规划路径的迭代曲线示意图；

图14为不同规划方法下的绳长变化量示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

连续型机器人多采用基于微分运动学迭代求解的规划方法，其规划结果通常是局部最优解的组合，无法得到全局最优解。本发明在初始状态下的机器人关节参数已知的情况下，先将笛卡尔坐标下的机器人末端执行器的目标位姿通过逆运动学映射到机器人关节的广义空间中，确定机器人的终止关节参数，再设计从关节空间的起始点到终止点的，对绳长变化量进行优化以获得一条轨迹，如图4所示。这样既保证了任务过程中速度和加速度的连续性，同时又找到了能量最小的轨迹，节约了资源。

首先，本发明实施例对方法的实验应用于样机绳驱连续型机器人上，因此，对机器人进行相关描述。

本发明中，机器人主体部分由一系列圆盘与中心弹性杆及驱动绳索组成；由相同一组驱动绳索驱动的一系列圆盘与弹性杆定义为连续型机器人的一段，每两个圆盘及其中间弹性杆定义为连续型机器人的一小节，具体如图2所示，穿过圆盘中心的蓝色直线代表中心弹性杆，相同颜色的圆环与实线代表同一段的圆盘与驱动绳索，连续型机器人共有m段，每段有n小节，每段由s根驱动绳索驱动。

驱动绳孔均匀的分布在圆盘上。驱动第i段机器人的驱动绳索末端利用绳头固定在n×i+1号圆盘上，因此，每经过一段驱动绳索就会减少s根。绳孔编号方式为：第i段第j号驱动绳索编号为3(i-1)+j，布线圆盘上固连的坐标系以圆盘几何中心为原点，原点与1号绳孔中心连线定义为X轴，正方向为从原点到1号绳孔中心方向，记第i段对应的驱动绳索中的1号驱动绳索和布线圆盘上的固连坐标系X轴的夹角为，如图3所示。

实施例一

如图1所示，本实施例提供一种绳驱连续型机器人快速运动规划系统，包括：终止参数定义模块、绳长向量定义模块、绳长距离计算模块、轨迹优化模块；

终止参数定义模块用于基于机器人起始关节参数，确定终止关节参数；终止参数定义模块的具体工作过程包括：

将笛卡尔坐标系下的机器人末端执行器的目标位姿通过逆运动学映射到机器人关节的广义空间中，确定机器人的终止关节参数。

定义机器人的起始关节参数为：，终止关节参数为：。

式中，表示起始关节参数，/>表示终止关节参数。

绳长向量定义模块用于基于起始关节参数和终止关节参数定义起始关节对应绳长向量和终止关节对应绳长向量；

绳长向量定义模块的具体工作过程包括：

本实施例中，定义起始关节对应绳长向量为：，终止关节对应绳长向量为：/>。

绳长距离计算模块用于基于起始关节对应绳长向量与终止关节对应绳长向量计算绳长距离；

本实施例中，绳长距离为利用绳长向量空间上的二范数定义关节参数空间的距离；计算方法如下：

，（1）

式中，l ₁表示终止关节对应绳长向量，l ₀表示起始关节对应绳长向量，l _i,1表示终止关节参数对应绳长向量对应分量，l _i,0表示起始关节对应绳长向量为对应分量。

其中，d _l 满足如下预设条件：

1.非负性、非退化行：

由欧式空间二范数的性质与单关节参数空间到绳长空间向量的映射为单射可得：，当且仅当/>时等号成立。

2.对称性：

。（2）

3.三角不等式

。（3）

此时，目标函数为凸函数，其局部最优解即为全局最优解。求解最优路径即可转化为寻找令绳长距离最小的路径，为变分问题，利用变分法求解：

由参数空间到绳长向量空间的映射关系得：

，（4）

，（5）

式中，l表示绳长向量。

关节参数空间为二维空间，只需要确定函数关系/>即可获得最优路径。因此有宗量为的泛函，其被积函数L为：

，（6）

其中，。

由欧拉-拉格朗日方程得：

，（7）

将L代入整理可得微分方程如下：

，（8）

为了公式的表达简洁，引入w、p两个中间变量。

其中，

。

为了公式的表达简洁，引入p ₁-p ₅作为中间变量。w作为分母，不影响等式的成立。去掉不恒等于0的项，微分方程可简化为：

。（9）

由于已知起始关节参数为，终止关节参数为：/>，为第一类边界条件，联立最优轨迹满足的微分方程为：

。（10）

上述微分方程没有解析解，只能采用数值解法进行求解。如图5所示，黑色实线为对一单段连续型机器人，其弹性杆长h=10cm，绳孔分布半径r _h =2cm，选取起始关节参数，终止关节参数/>时，求解微分方程获得最优轨迹，对应的绳长距离d _l =1.2268cm，求解时间为62.3秒。

轨迹优化模块用于基于参数正交化将绳长距离转化为空间距离；并基于空间距离获得最优轨迹。

由于变分法的全局最优规划算法的计算量大，求解时间长，因此本实施例基于参数正交化，将绳长距离转化为空间距离进而获得最短距离下的最优路径。具体的：

首先，将起始关节参数，终止关节参数/>转化为正交形式，得到正交化起始关节参数和正交化终止关节参数：

对上述正交化起始关节参数和正交化终止关节参数进行线性插值，插值方法包括：

，其中，/>。（13）

再将转换为/>：

。（14）

计算与/>:

，（15）

其中，。

，（16）

为了公式的表达简洁，引入c₁-c₇、d₁-d₆为中间变量；其中，

将公式（14）、（15）、（16）代入公式（8）左侧得到关于t的函数，将弹性杆长设置为h=10cm，绳孔分布半径r _h =2cm，选取起始关节参数取,终止关节参数取代入可得函数曲线如图6中黑色实线所示，作为对比，图6中黑色虚线为直接对关节参数/>进行线性插值得到的函数曲线。

利用函数衡量相应轨迹与绳长变化量最小时最优轨迹差距的大小。正交轨迹对应的F=0.0595，直接线性插值轨迹对应的F=0.2453，可见，正交轨迹与绳长变化量指标下的最优轨迹的差距很小。

由正交关节参数空间的欧式距离也可以定义为关节参数空间距离：

。（17）

验证也满足：

1.非负性、非退化性：

由欧式空间二范数的性质与单关节参数空间到正交关节参数空间的映射为单射可得，当且仅当/>时等号成立。

2.对称性：

。（18）

3.三角不等式：

（19）

由此，通过优化代替d₁来获取近似绳长变化量指标下的最优轨迹，其中/>有解析表达式，便于求解，给出了难点2的一个近似高效的解决方案。

由公式（5）的具体形式发现d _l 并不是的函数，因此当连续型机器人中的第i段上的圆盘间的各组驱动绳索对应的dd _l 相等。连续型机器人中的第i段上的圆盘间的所有驱动绳索对应的绳长变化量为：

。（20）

采用正交轨迹与正交距离近似代替绳长变化量指标下的最优轨迹与绳长距离进行优化，如图7所示。因此难点1转化为优化问题：

，（21）

式中，P（q）表示机械臂的位姿，P _end 表示给定目标位姿。

其中，绳长距离与起始角度无关，因此总的绳长距离只需对各段的绳长距离加权求和即可获得：

，（22）

式中，表示第i段机械臂的绳长距离。

因此，全局最优规划问题可以利用梯度投影法求解出最优终止关节参数然后与起始关节参数构造线性变化轨迹作为近似全局最优轨迹。其中，梯度投影法的迭代表达式为：

，（23）

式中：θ代表关节参数向量；代表雅可比矩阵的伪逆阵；/>表示末端执行器的广义速度；I表示正交矩阵，k为放大系数；/>表示优化目标函数的梯度。

对于绳长变化量最小的优化目标，可以令：

（24）

将关节限位约束融入规划中，如图8所示，由于关节限位约束并不会在欧式空间中引入额外的障碍物，并不影响欧式距离的计算。只需要在梯度投影法中加入关节限位优化目标函数。此时可以令：

，（25）

迭代求解表达式为：

。（26）

设定阈值，当/>时即可认为迭代结束。

通过仿真算例证明全局规划算法的有效性。选取4段连续型机器人作为仿真对象，单段机器人长度设置为1m，每段只含有1小节，起始关节参数为，目标位姿为：

。

利用梯度投影法求解最优正交距离解，以获得近似全局最优解的迭代过程如图9所示。若对各关节加入±100°的关节限位约束，同样利用梯度投影法求解含约束的最优正交距离解，以获得近似的含约束的全局最优解的迭代过程如图10所示，可以看到其结果满足了关节限位约束。作为对比，直接以绳长变化量为优化目标进行梯度投影迭代获得目标解的迭代过程如图11所示，但此规划路径在运行过程中无法保证各驱动绳索始终满足张紧条件，驱动绳索在运行过程中会发生松弛，导致末端运动轨迹不光滑；采用伪逆法在笛卡尔空间均匀插值获得的规划路径如图12所示；采用梯度投影法在笛卡尔空间均匀插值的同时优化绳长变化量获得的规划路径如图13所示。

各规划方法的绳长变化量对比如图14所示。图中黑色实线表示基于伪逆法的笛卡尔空间均匀插值规划方法的绳长变化量，黑色虚线表示利用梯度投影法优化绳长变化量后的笛卡尔空间均匀插值规划方法的绳长变化量，红色实线表示本文提出的在正交关节空间进行优化得到近似全局最优规划方法的绳长变化量，红色虚线表示含关节限位约束条件的绳长变化量近似全局最优规划方法的绳长变化量，蓝色实线表示直接在绳长空间进行最优规划的方法的绳长变化量。可以看出本文提出的在正交关节空间进行绳长变化量近似全局最优规划方法是有效的，相比于局部最优规划方案大大降低了连续型机器人运动过程中的绳长变化量，且相比于在绳长空间进行最优规划的方案可以保证各驱动绳索始终处于绷紧状态，保证了连续型机器人运行轨迹的连续性。

以上所述的实施例仅是对本发明优选方式进行的描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (2)

1.一种绳驱连续型机器人快速运动规划系统，其特征在于，包括：

终止参数定义模块、绳长向量定义模块、绳长距离计算模块、轨迹优化模块；

所述终止参数定义模块用于基于机器人起始关节参数，确定终止关节参数；

所述绳长向量定义模块用于基于所述起始关节参数和所述终止关节参数定义起始关节对应绳长向量和终止关节对应绳长向量；

所述绳长距离计算模块用于基于所述起始关节对应绳长向量与所述终止关节对应绳长向量计算绳长距离；

所述轨迹优化模块用于基于参数正交化将所述绳长距离转化为空间距离；并基于所述空间距离获得最优轨迹；

所述终止参数定义模块确定所述终止关节参数方法包括：

将笛卡尔坐标系下的机器人末端执行器的目标位姿通过逆运动学映射到机器人关节的广义空间中，确定机器人的所述终止关节参数；

所述轨迹优化模块将所述绳长距离转化为所述空间距离的方法包括：

将所述起始关节参数和所述终止关节参数转化为正交形式，得到正交化起始关节参数和正交化终止关节参数；

对所述正交化起始关节参数和所述正交化终止关节参数进行线性插值，得到所述空间距离；

所述线性插值的方法包括：

式中，/>表示正交参数线性插值对于x轴的分量，/>表示正交参数/>线性插值对于y轴的分量，/>表示正交起始关节参数，/>表示正交终止关节参数，t表示参变量；

所述空间距离包括：

式中，/>表示正交终止关节参数，/>表示正交起始关节参数。

2.根据权利要求1所述绳驱连续型机器人快速运动规划系统，其特征在于，所述绳长距离计算模块计算所述绳长距离的方法包括：

利用所述起始关节对应绳长向量和所述终止关节对应绳长向量的二范数定义关节参数空间的距离为所述绳长距离；

所述绳长距离包括：

式中，l ₁表示终止关节对应绳长向量，l ₀表示起始关节对应绳长向量，l _i,1表示终止关节参数对应绳长向量对应分量，l _i,0表示起始关节对应绳长向量对应分量。