CN109202888A - 一种双臂医疗辅助机器人软件平台设计方法及系统 - Google Patents

Abstract

本发明公开了一种双臂医疗辅助机器人软件平台设计方法，包括以下步骤：进行运动学建模，求解关节位置值；机器人作业模块对示教过程进行解析，得到各个关节的关节值，保证机器人示教再现；根据双臂间的运动学及动力学约束关系，控制双臂的协调与碰撞。本发明还提供一种双臂医疗辅助机器人软件平台系统及双臂医疗辅助机器人。本发明具有可以进行复杂的药品调配过程的有益效果。

Description

一种双臂医疗辅助机器人软件平台设计方法及系统

技术领域

本发明涉及机器人技术领域，尤其是涉及双臂医疗辅助机器人软件 平台设计方法及系统。

背景技术

目前，在医疗行业日益飞速发展的今天，机器人自动化已经逐渐走 入到医疗系统当中去。传统的辅助医疗辅助器械装配工艺越来越无法满 足现代医疗生产的需求。同时在医疗器械辅助药品调配过程中，高安全 性、高稳定性、高灵活性的辅助协同作业的机器人的需求也越来越多。 医疗辅助药品调配许多工作都可以用机器人替代。配药，辅助固定引导 针，肿瘤切除等医疗辅助器械等人工解决方案已经落后。

发明内容

本发明旨在克服现有医疗辅助药品调配依赖人工的技术问题，提供 一种双臂医疗辅助机器人软件平台设计方法，能够使双臂医疗辅助机器 人进行复杂的药品调配过程。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种双 臂医疗辅助机器人软件平台设计方法，包括以下步骤：

进行运动学建模，求解关节位置值；

机器人作业模块对示教过程进行解析，得到各个关节的关节值，保 证机器人示教再现；

根据双臂间的运动学及动力学约束关系，控制双臂的协调与碰撞。

一些实施例中，进行运动学建模，求解关节位置值的具体步骤包括：

通过建立正运动学方程、构建MP广义逆矩阵方程、建立牛顿迭代 方程组、迭代方程组计算得到双臂医疗辅助机器人双臂部分关节的关节 位置值；

根据所述部分关节位置值求解剩余的关节位置值；对计算得到的关 节多解进行挑选。

一些实施例中，通过建立正运动学方程、构建MP广义逆矩阵方程、 建立牛顿迭代方程组、迭代方程组计算得到部分关节位置值的具体步骤 为：

根据MDH坐标关系表建立传递变换矩阵方程，

其中，α_i-1表示连杆的长度，是MDH坐标中z_i-1与z_i之间的公垂线， a_i-1表示连杆的转角，为MDH坐标中z_i-1与z_i之间的角度，d_i表示连杆的 偏距，为MDH坐标中z_i轴上两条相邻的公垂线之间的距离，θ_i表示关 节角，为MDH坐标中x_i-1与x_i相互平行绕z轴旋转的角度；i表示当前 坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i；T旋转矩 阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述。

运用正解相乘矩阵乘积得到运动学正解关系表达式，所述运动学正 解关系表达式为：

其中，表示机器人的本体转移矩阵；旋转矩阵T是{i}坐标系 相对于{i-1}坐标系的描述。采用相乘矩阵乘积，即可求出机器人本体矩 阵；

牛顿法下山法的迭代公式为：

xⁿ⁺¹＝xⁿ-ω(F′(xⁿ))^-1F(xⁿ)

其中，x＝[θ₁ θ₂ θ₃ θ₄]表示当前时刻的四个关节值；n+1表示当 前时刻；n表示上一个时刻；ω表示下山因子，一般ω＝0.5；F(xⁿ)为雅 克比矩阵，表示当前时刻x的表达关系式；其中雅克比矩阵F(xⁿ)，即

对于七自由度的双臂医疗辅助机器人，根据正运动学方程、MP广义 逆矩阵方程、牛顿法下山法的迭代公式建立包含四个方程的牛顿迭代方 程组：

其中，表示机器人世界坐标系下的位置值；F₁、F₂、F₃表示关节值与位置点之间的方程关系；D表示机器人的连杆长度；i表 示当前坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i。

求解雅克比矩阵J，且保证其始终可逆：

其中θ₁、θ₂、θ₃和θ₄表示当前时刻的四个关节值；从而求得前四轴 关节的关节位置值，根据前四轴关节的关节位置值求解后三轴关节位置 值。

相应地，本发明还提供一种双臂医疗辅助机器人软件平台系统，包 括以下模块：

运动学模块，用于进行运动学建模，求解关节位置值；

机器人作业模块，用于对示教过程进行解析，得到各个关节的关节 值，保证机器人示教再现；

双臂协调模块，用于根据双臂间的运动学及动力学约束关系，控制 双臂的协调与碰撞。

一些实施例中，所述运动学模块求解关节位置值的具体过程为：

通过建立正运动学方程、构建MP广义逆矩阵方程、建立牛顿迭代 方程组、迭代方程组计算得到双臂医疗辅助机器人双臂部分关节的关节 位置值；

根据所述部分关节位置值求解剩余的关节位置值；

对计算得到的关节多解进行挑选。

一些实施例中，所述运动学模块通过建立正运动学方程、构建MP广 义逆矩阵方程、建立牛顿迭代方程组、迭代方程组计算得到部分关节位 置值的具体步骤为：

根据MDH坐标关系表建立传递变换矩阵方程，

其中，α_i-1表示连杆的长度，是MDH坐标中z_i-1与z_i之间的公垂线， a_i-1表示连杆的转角，为MDH坐标中z_i-1与z_i之间的角度，d_i表示连杆的 偏距，为MDH坐标中z_i轴上两条相邻的公垂线之间的距离，θ_i表示关 节角，为MDH坐标中x_i-1与x_i相互平行绕z轴旋转的角度；i表示当前 坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i；T旋转矩 阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述。

运用正解相乘矩阵乘积得到运动学正解关系表达式，所述运动学正 解关系表达式为：其中表示机器人的本体转 移矩阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述。采用相 乘矩阵乘积，即可求出机器人本体矩阵。

牛顿法下山法的迭代公式为：

xⁿ⁺¹＝xⁿ-ω(F′(xⁿ))^-1F(xⁿ)

其中，x＝[θ₁ θ₂ θ₃ θ₄]表示当前时刻的四个关节值；n+1表示当 前时刻；n表示上一个时刻；ω表示下山因子，一般ω＝0.5；F(xⁿ)为雅 克比矩阵，表示当前时刻x的表达关系式；其中雅克比矩阵F(xⁿ)，即

对于七自由度的双臂医疗辅助机器人，根据正运动学方程、MP广义 逆矩阵方程、牛顿法下山法的迭代公式建立包含四个方程的牛顿迭代方 程组：

其中，表示机器人世界坐标系下的位置值；F₁、F₂、F₃表示关节值与位置点之间的方程关系；D表示机器人的连杆长度；i表 示当前坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i。

求解雅克比矩阵J，且保证其始终可逆：

其中θ₁、θ₂、θ₃和θ₄表示当前时刻的四个关节值；从而求得前四轴 关节的关节位置值，根据前四轴关节的关节位置值求解后三轴关节位置 值。

此外，本发明还提供一种双臂医疗辅助机器人，所述双臂医疗辅助 机器人包括上述的双臂医疗辅助机器人软件平台系统。

本发明的有益效果是：区别于现有技术的情况，本发明提供的一 种双臂医疗辅助机器人软件平台设计方法具有以下有益效果：

(1)、采用手臂分块化处理，将对传统的十四轴双手臂的处理， 简化为对七轴单手臂的处理，运用牛顿迭代进行解算七自由度机器 人，解决了双手臂十四轴无法解析的问题；

(3)、采用双臂协调技术，保证机器人在示教和执行的过程中避 免碰撞，保证机器人运动时的安全性，即不能发生碰撞。

附图说明

图1是本发明双臂医疗辅助机器人控制系统一个实施例的结构框 图。

具体实施方式

本发明的目的是设计一种双臂医疗辅助机器人软件平台。使用该平 台不仅可以实现机器人的药品调配的过程，而且可以实现工人和机器人 可以协同工作，将纯手工药品调配工作和纯自动化生产有机的结合起 来，解决医疗行业中操作中人工密集化的问题。本发明双臂医疗辅助机 器人软件平台的设计方法基于一种拟人双臂机器人，其核心功能有：(1) 机器人装有两个手臂(每个手臂有7个自由度)以及集成在机器人躯干 内的控制器，而且机械臂的抓手具有较大的工作半径；(2)此款机器人 安全性能非常高，在没有其它安全措施的条件下可以在工人旁边运行； (3)可以符合医疗标准，能够用在医疗场所。以现有技术为基础，重 新布局结构，完成机械本体设计；通过软件模拟及模型推导实现完成双 臂协调作业轨迹规划；对各关节电流功率重新校核，确认单控制器结构 形式，最后通过总体规划及结合功能要求完成总体控制布局。

拟人双臂机器人某种程度上可以比作两个单臂机器人在一起工作 的情况，与单臂机器人相比，目标是设计具有一体化关节的双臂机器人， 要求手臂具有以下特点：(1)具有一体化的关节：一体化的概念是指将 电机、减速器、制动器、编码器、驱动器集成为一个驱动单元中，以实 现机器人的紧凑化设计。前期设计中考虑驱动器开发的难度，驱动器暂时不集成到驱动单元中；(2)手臂具有7个自由度：手臂具有拟人化的 特点，与人的手臂一样具有7个自由度。前期设计中考虑通用性，7DOF 手臂可通过去除J3轴驱动单元的方法整体更改为6DOF手臂(3)手臂 的先进控制：要求手臂具有力控制功能，可与人协作。同时手臂要具备 双臂协调操作功能，保证协调的同步性和精确性以及防止碰撞。

本发明的双臂医疗辅助机器人软件平台设计方法，基于上述双臂机 器人，具体包括以下步骤：

进行运动学建模，求解关节位置值。具体地，首先通过建立正运动 学方程、构建MP广义逆矩阵方程、建立牛顿迭代方程组、迭代方程组 计算得到双臂医疗辅助机器人部分关节的关节位置值，然后根据所述部 分关节位置值求解剩余的关节位置值，最后对计算得到的关节多解进行 挑选。

机器人作业模块对示教过程进行解析，得到各个关节的关节值，保 证机器人示教再现。机器人作业模块是双臂医疗辅助机器人能够执行示 教过程的翻译解析模块。用户将示教的机器人位置、姿态和特征信息存 储到机器人作业中。在执行模式下，机器人作业模块再对示教过程进行 解析，最终得到各个关节的关节值，以保证机器人示教再现。作业信息 还包括其他的逻辑、全局变量和控制功能。

根据双臂间的运动学及动力学约束关系，控制双臂的协调与碰撞。 双臂医疗辅助机器人从某种操作程度上，可以看做是两个单臂机器人一 起协同工作。双臂协作技术就是指两条单手臂进行相互的协调操作技 术，不仅涉及到双臂间的运动学及动力学约束关系，而且涉及到双臂共 同作业时的控制方法。双臂机器人其理论核心是在考虑其他机器人作为 未知干扰来源的影响，其中一个机器人是独立于其他机器人，进而实现 相互工作不干扰。双臂医疗辅助机器人使用传感器数据，通过一个常见 的物理耦合臂之间的联系。两个手臂之间的通信控制器设置右手为主 手，左手为辅手。双臂可以相互进行行动轨迹规划和决策，即协调武器 之间的关系。

本发明双臂医疗辅助机器人软件平台设计方法的一个实施例中，提 供一个对七自由度机器人的关节求解过程：根据MDH坐标关系表建立传 递变换矩阵方程，

其中，α_i-1表示连杆的长度，是MDH坐标中z_i-1与z_i之间的公垂线， a_i-1表示连杆的转角，为MDH坐标中z_i-1与z_i之间的角度，d_i表示连杆的 偏距，为MDH坐标中z_i轴上两条相邻的公垂线之间的距离，θ_i表示关 节角，为MDH坐标中x_i-1与x_i相互平行绕z轴旋转的角度；i表示当前 坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i；T旋转矩 阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述。

运用正解相乘矩阵乘积得到运动学正解关系表达式，所述运动学正 解关系表达式为：其中表示机器人的本体转 移矩阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述。采用相 乘矩阵乘积，即可求出机器人本体矩阵。

牛顿法下山法的迭代公式为：

xⁿ⁺¹＝xⁿ-ω(F′(xⁿ))^-1F(xⁿ)

其中，x＝[θ₁ θ₂ θ₃ θ₄]表示当前时刻的四个关节值；n+1表示当 前时刻；n表示上一个时刻；ω表示下山因子，一般ω＝0.5；F(xⁿ)为雅 克比矩阵，表示当前时刻x的表达关系式；其中雅克比矩阵F(xⁿ)，即

对于七自由度的双臂医疗辅助机器人，根据正运动学方程、MP广义 逆矩阵方程、牛顿法下山法的迭代公式建立包含四个方程的牛顿迭代方 程组：

其中，表示机器人世界坐标系下的位置值；F₁、F₂、F₃表示关节值与位置点之间的方程关系；D表示机器人的连杆长度；i表 示当前坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i。

求解雅克比矩阵J，且保证其始终可逆：

其中θ₁、θ₂、θ₃和θ₄表示当前时刻的四个关节值；从而求得前四轴 关节的关节位置值，根据前四轴关节的关节位置值求解后三轴关节位置 值。

本发明还提供一种双臂医疗辅助机器人软件平台系统。包括运动学 模块，用于进行运动学建模，求解关节位置值。通过建立正运动学方程、 构建MP广义逆矩阵方程、建立牛顿迭代方程组、迭代方程组计算得到 双臂医疗辅助机器人双臂部分关节的关节位置值，根据所述部分关节位 置值求解剩余的关节位置值，对计算得到的关节多解进行挑选。求解关节位置值的具体过程如前文所述，在此不再赘述。

机器人作业模块，用于对示教过程进行解析，得到各个关节的关节 值，保证机器人示教再现。

双臂协调模块，用于根据双臂间的运动学及动力学约束关系，控制 双臂的协调与碰撞。

本发明的双臂医疗辅助机器人软件平台系统包括RSP底层平台和 RBP机器人平台。其中，上文所述的运动学模块位于算法层。请参阅图 1，为双臂医疗辅助机器人控制系统一个实施例的结构框图。如图中所 示，SRCP的上层也就是机器人任务调度层主要考虑的是根据不同机器人 的功能需求，设计满足需求的任务调度模块。任务调度层也是机器人控制系统的核心层，只是不同种类的机器人，任务调度层的设计不同而已。 任务调度层是基于SRCP之上开发的，通过接口库与SRCP进行联系，负 责机器人控制器软件的总体调度，可以是线程间的调度，也可以是进程 间的访问。任务调度层除了调用SRCP提供的接口服务，同时还需要有 各个子功能模块，供任务调用层调用，这样才构成一个完整的机器人控 制器软件，这些通用的子功能模块一般有通讯模块、监控模块、显示模 块、安全控制模块、感知控制模块、工艺控制模块以及应用模块等。其 中应用模块是需要终端用户进行二次开发的模块，用户需要开发哪些应 用的功能模块，只需通过系统注册方式，将应用模块挂载到任务调度层 即可。

本发明同时还提供一种双臂医疗辅助机器人，包括上述的双臂医疗 辅助机器人软件平台系统。采用本发明的双臂医疗辅助机器人进行开启 药瓶盖的具体过程为：

a、机器人左臂抓取药瓶，右臂爪取药瓶开启器；

b、左臂将药瓶放到指定位置，并通过视觉检查豁口位置，如果位置 不对需要用左臂旋转药瓶到特定位置，之后右臂抓住药瓶开启器迅速下 压，通过压力将药品中间的铝盖压掉；

c、右臂将药瓶开启器放回原位，左臂抬起药瓶将压下来的铝盖投放 的收纳盒内。此处需要引入视觉系统，确认药瓶盖是否开启成功，如果 还和药瓶或者开启器连在一起，需要有报错提示。

d、铝盖倒掉后，将药瓶放回到指定位置。

双臂医疗辅助机器人配药演示的具体过程为：

a、右臂抓取针管，左臂抓取推针器；

b、两手臂配合演示在安瓶内吸液，之后演示完成将药液注射到药瓶 内。

本发明的一种双臂医疗辅助机器人软件平台设计方法、双臂医疗辅 助机器人软件平台系统及双臂医疗辅助机器人。可以进行复杂的药品调 配过程，将双臂医疗辅助机器人软件平台应用到医疗辅助行业具有无法 比拟的优越性。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围， 凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或 直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保 护范围内。

Claims (7)

1.一种双臂医疗辅助机器人软件平台设计方法，其特征在于，包括以下步骤：

进行运动学建模，求解关节位置值；

机器人作业模块对示教过程进行解析，得到各个关节的关节值，保证机器人示教再现；

根据双臂间的运动学及动力学约束关系，控制双臂的协调与碰撞。

2.根据权利要求1所述的双臂医疗辅助机器人软件平台设计方法，其特征在于，进行运动学建模，求解关节位置值的具体步骤包括：

通过建立正运动学方程、构建MP广义逆矩阵方程、建立牛顿迭代方程组、迭代方程组计算得到双臂医疗辅助机器人部分关节的关节位置值；

根据所述部分关节位置值求解剩余的关节位置值；对计算得到的关节多解进行挑选。

3.根据权利要求1所述的双臂医疗辅助机器人软件平台设计方法，其特征在于，通过建立正运动学方程、建立牛顿迭代方程组、迭代方程组计算得到部分关节位置值的具体步骤为：

根据MDH坐标关系表建立传递变换矩阵方程，

其中，αi-1表示连杆的长度，是MDH坐标中zi-1与zi之间的公垂线，ai-1表示连杆的转角，为MDH坐标中zi-1与zi之间的角度，di表示连杆的偏距，为MDH坐标中zi轴上两条相邻的公垂线之间的距离，θ_i表示关节角，为MDH坐标中xi-1与xi相互平行绕z轴旋转的角度；i表示当前坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i；T旋转矩阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述；

运用正解相乘矩阵乘积得到运动学正解关系表达式，所述运动学正解关系表达式为：其中，表示机器人的本体转移矩阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述，采用相乘矩阵乘积，即可求出机器人本体矩阵；

牛顿法下山法的迭代公式为：

xⁿ⁺¹＝xⁿ-ω(F′(xⁿ))^-1F(xⁿ)

其中，x＝[θ₁ θ₂ θ₃ θ₄]表示当前时刻的四个关节值；n+1表示当前时刻；n表示上一个时刻；ω表示下山因子；F(xⁿ)为雅克比矩阵，表示当前时刻x的表达关系式；其中雅克比矩阵F(xⁿ)，即

对于七自由度的双臂医疗辅助机器人，根据正运动学方程、MP广义逆矩阵方程、牛顿法下山法的迭代公式建立包含四个方程的牛顿迭代方程组：

其中，表示机器人世界坐标系下的位置值；F₁、F₂、F₃表示关节值与位置点之间的方程关系；D表示机器人的连杆长度；i表示当前坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i；

求解雅克比矩阵J，且保证其始终可逆：

其中θ₁、θ₂、θ₃和θ₄表示当前时刻的四个关节值；从而求得前四轴关节的关节位置值，根据前四轴关节的关节位置值求解后三轴关节位置值。

4.一种双臂医疗辅助机器人软件平台系统，其特征在于，包括以下模块：

运动学模块，用于进行运动学建模，求解关节位置值；

机器人作业模块，用于对示教过程进行解析，得到各个关节的关节值，保证机器人示教再现；

双臂协调模块，用于根据双臂间的运动学及动力学约束关系，控制双臂的协调与碰撞。

5.根据权利要求4所述的双臂医疗辅助机器人软件平台系统，其特征在于，所述运动学模块求解关节位置值的具体过程为：

通过建立正运动学方程、构建MP广义逆矩阵方程、建立牛顿迭代方程组、迭代方程组计算得到双臂医疗辅助机器人双臂部分关节的关节位置值；

根据所述部分关节位置值求解剩余的关节位置值；

通过临近法对计算得到的关节多解进行挑选，对于多解采用临近求解公式求取最理性关节值，公式如下：

min(λ)＝|(θ_j-θ₀)|/θ₀

其中，θ_j表示求解出的可能关节值；θ₀表当前关节值。

6.根据权利要求4所述的双臂医疗辅助机器人软件平台系统，其特征在于，所述运动学模块通过建立正运动学方程、构建MP广义逆矩阵方程、建立牛顿迭代方程组、迭代方程组计算得到部分关节位置值的具体步骤为：

根据MDH坐标关系表建立传递变换矩阵方程，

其中，α_i-1表示连杆的长度，是MDH坐标中z_i-1与z_i之间的公垂线，a_i-1表示连杆的转角，为MDH坐标中z_i-1与z_i之间的角度，d_i表示连杆的偏距，为MDH坐标中z_i轴上两条相邻的公垂线之间的距离，θ_i表示关节角，为MDH坐标中x_i-1与x_i相互平行绕z轴旋转的角度；i表示当前坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i；T旋转矩阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述；

运用正解相乘矩阵乘积得到运动学正解关系表达式，所述运动学正解关系表达式为：

其中表示机器人的本体转移矩阵；旋转矩阵T是{i}坐标系相对于{i-1}坐标系的描述。采用相乘矩阵乘积，即可求出机器人本体矩阵；

牛顿法下山法的迭代公式为：

xⁿ⁺¹＝xⁿ-ω(F′(xⁿ))^-1F(xⁿ)

其中，x＝[θ₁ θ₂ θ₃ θ₄]表示当前时刻的四个关节值；n+1表示当前时刻；n表示上一个时刻；ω表示下山因子，一般ω＝0.5；F(xⁿ)为雅克比矩阵，表示当前时刻x的表达关系式；其中雅克比矩阵F(xⁿ)，即

对于七自由度的双臂医疗辅助机器人，根据正运动学方程、牛顿法下山法的迭代公式建立包含四个方程的牛顿迭代方程组：

其中，表示机器人世界坐标系下的位置值；F₁、F₂、F₃表示关节值与位置点之间的方程关系；D表示机器人的连杆长度；i表示当前坐标系号；i-1上一个坐标系号；cθ_i代表cosθ_i；sθ_i代表sinθ_i；

求解雅克比矩阵J，且保证其始终可逆：

其中θ₁、θ₂、θ₃和θ₄表示当前时刻的四个关节值；从而求得前四轴关节的关节位置值，根据前四轴关节的关节位置值求解后三轴关节位置值。

7.一种双臂医疗辅助机器人，其特征在于，所述双臂医疗辅助机器人包括如权利要求4-6任一项所述的双臂医疗辅助机器人软件平台系统。