一种连续型机械臂的空间避障轨迹规划方法

摘要

本发明公开了一种连续型机械臂的空间避障轨迹规划方法，包括以下步骤：建立连续型机械臂的运动学模型，根据任务要求生成末端空间轨迹，利用模态函数建立机械臂构型的空间参考曲线，使其末端能够达到期望位置，根据单节机械臂变形特点将机械臂构型与空间参考曲线进行拟合，根据参考拟合点计算单段臂段的构型，检测连续型机械臂是否与障碍物发生碰撞，根据避障策略调整机械臂的构型，实现避障。本发明提出的连续型机械臂的空间避障轨迹规划方法，能够实现连续型机械臂的空间避障路径规划。

说明书

技术领域

本发明涉及机器人技术领域，尤其涉及一种连续型机械臂的空间避障轨迹规 划方法。

背景技术

受自然界生物体结构例如象鼻、藤蔓、蛇类等的启发，具有连续变形结构的 连续型机器人相关设计和技术越来越受重视。连续型机器人是指具有能够连续变 形结构的一类机械臂。相比于传统机械臂，连续型机械臂具有很强的灵活性和内 在的柔顺性，因此特别适合于复杂非结构环境中的操作以及与人发生交互的任务。 然而，由于连续型机械臂的运动模型比较复杂，其在复杂环境下的避障路径规划 是一个非常具有挑战性的问题，尤其是对于具有超冗余度的连续型机械臂，如果 采用常用的雅可比矩阵伪逆方法，计算量较大，因而研究一种比较简单的空间避 障规划算法是非常必要的。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不 必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请 的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创 造性。

发明内容

为解决上述技术问题，本发明提出一种连续型机械臂空间的避障轨迹规划方 法，能够实现连续型机械臂的空间避障路径规划。

为了达到上述目的，本发明采用以下技术方案：

本发明公开了一种连续型机械臂的空间避障轨迹规划方法，包括以下步骤：

S1：建立连续型机械臂的运动学模型，其中所述连续型机械臂包括整臂，所 述整臂包括多节臂段；

S2：根据任务要求，生成整臂的末端跟踪的空间轨迹；

S3：结合整臂的末端跟踪的空间轨迹，利用模态函数建立连续型机械臂构型 的参考曲线；

S4：在步骤S3中的参考曲线中选取每节臂段末端位置的参考点；

S5：结合步骤S1中的连续型机械臂的运动学模型以及步骤S4中每节臂段 末端位置的参考点计算每节臂段的构型参数，拟合得到连续型机械臂的构型；

S6：检测每节臂段是否与环境中的障碍物发生碰撞，如果发生碰撞，则执行 步骤S7；

S7：修正每节臂段末端位置的参考点，直到新的机械臂构型能够避开障碍物；

S8：结合步骤S1中的连续型机械臂的运动学模型以及步骤S7中修正后的 每节臂段末端位置的参考点更新计算每节臂段的构型参数，拟合得到更新后的连 续型机械臂的构型。

优选地，步骤S1具体包括：建立连续型机械臂的运动学模型，得到多节臂 段构成的连续型机械臂的齐次变换矩阵为：

其中，^i-1T_i为单节臂段的齐次坐标变换矩阵，M为臂段数量；且^i-1T_i的表>

其中，R_i∈SO(3)描述单节臂段的根部与末端之间的方向关系，描述 单节臂段的末端在根部坐标系下的相对位置；q_i＝[Θ_i,Φ_i]^T描述第i节臂段的构>i为弯曲角度，Φ_i为弯曲方向，L为每节臂段的长度，N为每节臂段>

优选地，步骤S5具体为：结合步骤S1中单节臂段的齐次坐标变换矩阵的 表达式(2)以及步骤S4中每节臂段末端位置的参考点计算每节臂段的构型参数， 拟合得到连续型机械臂的构型。

优选地，步骤S8具体为：结合步骤S1中单节臂段的齐次坐标变换矩阵的 表达式(2)以及步骤S7中修正后的每节臂段末端位置的参考点计算每节臂段的 构型参数，拟合得到更新后的连续型机械臂的构型。

优选地，步骤S2还包括对所述空间轨迹进行离散取点处理，得到整臂末端 期望位置序列[X_d1,X_d2,…,X_dj,…]。

优选地，步骤S3具体包括：将连续型机械臂的多节臂段描述为一条空间光 滑曲线为：

其中，l为曲线长度，s∈[0,1]为归一化的位置参数，X(s)为s处曲线上的点 对应的空间坐标，u(σ)代表曲线在s＝σ处的切线矢量。

优选地，其中当u(σ)进行参数化表示后，式(3)表示为：

α(s)和β(s)是模态函数的线性组合：

其中，f_i(s)是模态函数，为待求解的模态系数。

优选地，模态系数a采用以下数值迭代方法来计算：

a_i＝a_i-1+ηJ^-1(a_i-1,1)(X_d-X_i-1)>

其中，J(a_i-1,1)为模态系数矩阵，X_i-1为第i-1迭代下的整臂末端的位置，η>d为从步骤S2中得到的整臂末端期望位置序列[X_d1,X_d2,…,X_dj,…]中>

优选地，步骤S6具体包括：通过计算拟合的整臂与障碍物几何中心之间的 最小距离，来判断整臂是否与障碍物发生碰撞。

优选地，步骤S7具体包括：记录下当前拟合结果下发生碰撞时的整臂与障 碍物几何中心的最小距离d_s及其方向矢量并采用式(8)对每节臂段末端位 置的参考点进行修正，直到新的机械臂构型能够避开障碍物；

其中，是经第q次修正得到的每节臂段末端位置的参考点，r代表障碍物 几何中心到其表面的最大半径，γ是修正系数。

与现有技术相比，本发明的有益效果在于：本发明的连续型机械臂空间避障 轨迹规划方法采用了模态函数方法，通过建立机械臂构型的参考曲线，将复杂的 超冗余机械臂的逆运动学问题简化为单节连续变形臂段的逆运动学求解，能够实 现连续型机械臂的空间避障路径规划；其中采用模态函数的方法实现连续型机械 臂的空间避障路径规划具有以下优点：(1)算法简单，容易实现，具有简单、高 效、可靠等优点；(2)能够生成具有末端位置约束的连续参考曲线，利用拟合确 定关节位置，从而将连续型机械臂的逆运动学问题简化为单节臂段的逆运动学。 本发明的连续型机械臂空间避障轨迹规划方法不仅能够实现多节连续型机械臂 在空间中追踪指定的轨迹，同时能够有效避开复杂工作空间中的障碍物。本发明 的方法适用于多节连续型机械臂系统，在执行探测、跟踪等任务时能够迅速得到 机械臂构型参数，大大简化实际机器人系统的控制。

在进一步的方案中，本发明提出了一种避障策略，通过特定的公式对每节臂 段末端位置的参考点进行修正，使得新的机械臂构型能够避开障碍物，从而实现 更加简单可靠的空间避障路径规划。

附图说明

图1是本发明优选实施例的连续型机械臂空间避障轨迹规划方法的流程示 意图；

图2是本发明优选实施例的连续型机械臂的结构示意图；

图3是本发明优选实施例的连续型机械臂的简图；

图4是利用模态函数生成的机械臂的参考曲线；

图5是单节臂段的等效臂段长度与弯曲角度的关系示意图；

图6是检测整臂与空间任意障碍物是否发生碰撞的示意图；

图7是本发明具体实例的某一5节连续型机械臂采用本发明的方法在存在单 个障碍物的空间下追踪空间圆弧的规划过程；

图8是本发明具体实例的某一5节连续型机械臂采用本发明的方法在存在多 个障碍物的空间下追踪空间圆弧的规划过程。

具体实施方式

下面对照附图并结合优选的实施方式对本发明作进一步说明。

如图1所示，本发明优选实施例公开了一种连续型机械臂空间避障轨迹规划 方法，包括以下步骤：

S1：建立连续型机械臂的运动学模型。

如图2，在本实施例中，连续型机械臂由驱动箱10、驱动绳索20和整臂30 组成，其中整臂30由多节臂段组成，每节臂段均能够弯曲变形，得到多节臂段 构成的连续型机械臂的齐次变换矩阵为：

其中，^i-1T_i为单节臂段的齐次坐标变换矩阵，M为臂段数量。

对于连续变形的单节臂段，采用常曲率假设等对其变形进行简化，得到^i-1T_i的表达式如下：

其中，R_i∈SO(3)描述单节臂段的根部与末端之间的方向关系，描述 单节臂段的末端在根部坐标系下的相对位置；q_i＝[Θ_i,Φ_i]^T描述第i节臂段的构>i为弯曲角度，Φ_i为弯曲方向。所有臂段的构型参数构成了整个机械>1,q₂,…,q_M]^T。L为每节臂段的长度，N为每节臂段中包>0、T₁、……、T_M-1、T_M是各节臂段的根部坐标系的原点。

S2：根据任务要求，生成一条整臂的末端跟踪的空间轨迹。

对该空间轨迹进行离散取点处理，得到整臂末端期望位置序列 [X_d1,X_d2,…,X_dj,…]；该离散方法应该能够满足末端位置控制的精度等要求。其>

S3：结合整臂的末端跟踪的空间轨迹，利用模态函数建立机械臂构型的参 考曲线。

根据微分几何学，将机械臂的多节臂段描述为一条长度一定的空间光滑曲线 如下：

其中，l为曲线长度，s∈[0,1]为归一化的位置参数，X(s)为s处曲线上的点 对应的空间坐标，u(σ)代表曲线在s＝σ处的切线矢量。参考图4，当u(σ)进行 参数化表示后，式(3)可以进一步表示为：

α(s)和β(s)是模态函数的线性组合：

其中，f_i(s)是模态函数，该模态函数根据任务的需求进行选择或者设计。为待求解的模态系数，采用以下数值迭代方法来计算：

a_i＝a_i-1+ηJ^-1(a_i-1,1)(X_d-X_i-1)(7)

其中，J(a_i-1,1)为模态系数矩阵，X_i-1为第i-1迭代下的整臂末端的位置，η>d为从步骤S2中得到的整臂末端期望位置序列[X_d1,X_d2,…,X_dj,…]中>

S4：根据连续型柔性臂的特点，在步骤S3中获得的参考曲线中选取每节臂 段末端位置的参考点

定义单节臂段的等效臂段长度为弯曲后每节臂段的根部与末端的直线距离， 参考图5，示意出不同弯曲角度下的单节臂段的等效臂段长度，可以动态地调整 等效臂段长度，使得所有参考点都位于参考曲线上。

S5：根据单节臂段的齐次坐标变换矩阵的表达式(2)以及步骤S4中每节 臂段的末端参考点坐标计算每节臂段的构型参数q_i，拟合得到连续型机械臂的构>

S6：检测每节臂段是否与环境中的障碍物发生碰撞，如果发生碰撞，则执 行步骤S7。

参考图6，通过视觉观测等方式，计算出已经拟合的整臂的与障碍物几何中 心C₀之间的最小距离，进而可以判断二者之间是否发生碰撞。

S7：若步骤S6中发生碰撞，记录下当前拟合结果下发生碰撞时的整臂与障 碍物几何中心的最小距离d_s及其方向矢量并采用下式(8)进行避碰策略， 调整机械臂构型参考点，直到新的机械臂构型能够避开空间障碍物，参考图6， 连续型机械臂从fit₁的位置调整至fit₂的位置。

其中，是经第q次修正得到的每节臂段末端位置的参考点，r代表障碍物 几何中心到其表面的最大半径，γ是修正系数，控制每次修正的程度。

S8：根据单节臂段的齐次坐标变换矩阵的表达式(2)以及步骤S7中修正 后的每节臂段的末端参考点坐标更新计算每节臂段的构型参数q_i′，拟合得到更>

如图7、图8，是采用本发明优选实施例的连续型机械臂空间避障轨迹规划 方法来对某一5节连续型机械臂进行空间避障规划的实例；其中图7中该机械臂 在存在单个障碍物的工作空间中追踪指定的空间圆弧轨迹，采用本发明优选实施 例的方法，可以有效避免某些期望位置时机械臂与空间障碍物的碰撞；图8中该 机械臂在多个障碍物存在的复杂空间中追踪空间直线轨迹，采用本发明优选实施 例的避障策略之后，最终的机械臂构型能够有效地避免碰撞同时追踪直线轨迹。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能 认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员 来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而 且性能或用途相同，都应当视为属于本发明的保护范围。