CN108724213B - 一种移乘机器人 - Google Patents

Abstract

本发明涉及一种移乘机器人，其解决了现有靠人工对行动不便人士移乘搬运时，增加了护理人员的负担，并且安全性低的技术问题，其包括左机械臂、右机械臂和底盘，左机械臂设有左承重托板，右机械臂设有右承重托板。本发明广泛用于医疗器械技术领域。

Description

一种移乘机器人

技术领域

本发明涉及医疗器械技术领域，具体而言，涉及一种移乘机器人。

背景技术

众所周知，由于术后、瘫痪、休克、麻醉、昏迷以及失能的病人或 老年人身体机能的损伤、退化而失去或缺乏行动能力，护理工作难度很 大，针对该群体的护理工作，主要包括进食服务、移乘搬运以及个人卫 生清洁。目前，移乘搬运基本都是通过人工实现的，医院大多以传统的 转运方式为主，依靠多名医护人员抬、抱、扶来完成护理对象的转运工 作，这类转运方式由于工作模式很难标准化，不仅增加了护理人员的负 担，护理对象受到二次损伤或跌落的危险情况时有发生。

因此，研制出一种代替人工的机器人来实现移乘搬运是本领域技术 人员亟待解决的技术问题。

发明内容

本发明就是为了解决现有靠人工对行动不便人士移乘搬运时，增加 了护理人员的负担，并且安全性低的技术问题，提供了一种可代替人工 自动进行移乘搬运的移乘机器人。

本发明的技术方案是，提供一种移乘机器人，包括左机械臂、右机 械臂和底盘，左机械臂包括左底座、左小臂驱动电机、左小臂减速器、 左小臂传动机构、左小臂驱动连杆、左大臂驱动电机、左大臂减速器、 左大臂传动机构、左大臂、左小臂传动连杆、左小臂、旋转关节一、旋 转关节二、旋转关节三、旋转关节四、旋转关节五、旋转关节六、左三 角形保持架、左末端姿态保持连杆、左三角形保持架连杆、旋转关节七、 左承重托板、左末端连杆和旋转关节九，所述左箱体一和左箱体二连接 于左底座上，所述左小臂驱动电机和左小臂减速器连接于左箱体一上， 左大臂驱动电机和左大臂减速器连接于左箱体二上；所述左小臂驱动电 机的输出轴通过左小臂传动机构与左小臂减速器的输入端连接，左小臂 驱动连杆的一端与左小臂减速器的输出端连接；左大臂驱动电机的输出 轴通过左大臂传动机构与左大臂减速器的输入端连接，左大臂的一端与 左大臂减速器的输出端连接，左小臂传动连杆的一端通过旋转关节一与 左小臂驱动连杆的另一端连接；左小臂的一端通过旋转关节九与左末端 连杆的一端连接，左小臂的另一端设有左小臂旋转关节连接部，左小臂 旋转关节连接部通过旋转关节三与左三角形保持架连接，左小臂传动连 杆的另一端通过旋转关节二与左小臂旋转关节连接部连接；左大臂的另 一端通过旋转关节与左三角形保持架连接，左大臂和左小臂在左三角形 保持架上的转动轴线是重合的，左末端姿态保持连杆的一端通过旋转关 节四与左三角形保持架连接，左末端姿态保持连杆的另一端通过旋转关节六与左末端连杆的另一端连接；左三角形保持架连杆的一端通过旋转 关节五与左三角形保持架连接，左三角形保持架连杆的另一端通过旋转 关节七与左箱体二连接，左承重托板与左末端连杆连接；

右机械臂包括右底座、右箱体一、右箱体二、右小臂驱动电机、右 小臂减速器、右小臂传动机构、右小臂驱动连杆、右大臂驱动电机、右 大臂减速器、右大臂传动机构、右大臂、右小臂传动连杆、右小臂、旋 转关节八、旋转关节十一、旋转关节十三、右三角形保持架、右末端姿 态保持连杆、右三角形保持架连杆、右承重托板和右末端连杆，右箱体 一和右箱体二连接于右底座上，右小臂驱动电机和右小臂减速器连接于 右箱体一上，所述右大臂驱动电机和右大臂减速器连接于右箱体二上； 右小臂驱动电机的输出轴通过右小臂传动机构与右小臂减速器的输入端 连接，右小臂驱动连杆的一端与右小臂减速器的输出端连接；右大臂驱 动电机的输出轴通过右大臂传动机构与右大臂减速器的输入端连接，右大臂的一端与右大臂减速器的输出端连接，右小臂传动连杆的一端通过 旋转关节八与右小臂驱动连杆的另一端连接；右小臂的一端通过旋转关 节与右末端连杆的一端连接，右小臂的另一端设有右小臂旋转关节连接 部，右小臂旋转关节连接部通过旋转关节与右三角形保持架连接，右小 臂传动连杆的另一端通过旋转关节与右小臂旋转关节连接部连接；右大 臂的另一端通过一个旋转关节与右三角形保持架连接，右大臂和右小臂 在右三角形保持架上的转动轴线是重合的；右末端姿态保持连杆的一端 通过旋转关节十一与右三角形保持架连接，右末端姿态保持连杆的另一 端通过旋转关节十三与右末端连杆的另一端连接；右三角形保持架连杆 的一端通过旋转关节与右三角形保持架连接，右三角形保持架连杆的另 一端通过旋转关节与右箱体二连接，右承重托板与右末端连杆连接；

左底座与底盘连接，右底座与底盘连接。

优选地，左底座和右底座与底盘之间设有丝杠副，丝杠副连接有丝 杠副驱动电机，丝杠副与底盘连接，丝杠副设有螺母座一和螺母座二， 左底座与螺母座一连接，右底座与螺母座二连接。

优选地，底盘连接有两个脚轮和两个麦克纳姆轮。

优选地，右承重托板和左承重托板之间连接有束带。

优选地，左大臂的转动角度范围是25～160°，所述左小臂驱动连杆 的转动角度范围是-80～70°，所述右大臂的转动角度范围是25～160°， 所述右小臂驱动连杆207的转动角度范围是-80～70°。

优选地，移乘机器人还包括控制***，所述控制***包括上位机、 控制器一、控制器二、驱动器一、驱动器二、编码器一、编码器二、驱 动器三、驱动器四、编码器三和编码器四，所述控制器一与上位机连接， 控制器二与上位机1连接；所述控制器一设有串口一，所述驱动器一的 输入端与串口一连接，驱动器一的输出端与左小臂驱动电机连接；所述 驱动器二的输入端与串口一连接，左大臂驱动电机与驱动器二的输出端 连接，所述编码器一与左小臂驱动电机连接，编码器一的信号输出端与 控制器一连接；编码器二与左大臂驱动电机连接，编码器二的信号输出 端与控制器一连接；所述控制器二设有串口二，驱动器三的输入端与串 口二连接，驱动器三的输出端与右小臂驱动电机连接；驱动器四的输入 端与串口二连接，右大臂驱动电机与驱动器四的输出端连接；编码器三 与右小臂驱动电机连接，编码器三的信号输出端与控制器二连接，编码 器四与右大臂驱动电机连接，编码器四的信号输出端与控制器二连接。

优选地，上位机设有ROS***和显示器，ROS***设有RViz仿真模 块、RViz显示模块、控制模块、数据处理模块，所述数据处理模块与RViz 仿真模块连接，控制模块与数据处理模块连接通信。

本发明还提供一种移乘方法，其特征在于，包括以下步骤：

先建立好三维模型，显示与实际左机械臂和右机械臂相同尺寸的三 维模型；

利用ROS中的RViz仿真模块调用三维模型，实现三维模型和实际机 械臂各关节实时关联；当上位机控制RViz仿真模块中三维模型的运动时， 左机械臂和右机械臂跟随运动并向RViz仿真模块实时反馈各关节状态信 息，当实际机械臂的反馈状态未达到三维模型状态时，上位机计算差值 后继续发送指令控制机械臂运动，直至左机械臂和右机械臂运动到达设 定的目标位置。

本发明的有益效果是：制造成本低，负载能力强，作业范围大，工 作速度快，工作效率高，稳定性高，安全性高。

能够在很大程度上避免护理对象再次受到损伤。使用移乘机器人转 运护理对象的过程，充分结合了人与机器各自的优势，医护人员能够准 确地辨识环境信息并做出操作判断，移乘机器人则能够快速高效地完成 转运工作，通过人机协作达到了人机***整体的最佳效率。

移乘机器人应用场景主要是医院、疗养院、养老院等狭窄工作环境， 能够协助医护人员安全可靠、舒适、灵活地完成不同护理场景下病人的 搬运工作。

机器人单臂具有3个自由度，结构上将驱动小臂转动的电机下移到 机械臂底座，这不仅可以降低机器人的重心，还能够减轻大臂的承重， 同时使得小臂更加轻巧，大大增加了机器人的稳定性。

本发明进一步的特征和方面，将在以下参考附图的具体实施方式的 描述中，得以清楚地记载。

附图说明

图1是移乘机器人的立体图；

图2是移乘机器人的主视图；

图3是移乘机器人的俯视图；

图4是移乘机器人的左视图；

图5图1所示结构去掉左箱体一、左箱体二、右箱体一和右箱体二 的示意图；

图6是图4所示结构去掉左箱体一、左箱体二的示意图；

图7是左机械臂的主要结构的立体图；

图8是平面并联机械臂机构原理图；

图9是控制***示意图；

图10是上位机设有ROS***的示意图；

图11是机械臂各连杆参数计算示意图；

图12是ROS***通过三维模型控制实际机械臂的流程图。

图中符号说明：

100.左机械臂，101.左底座，102.左箱体一，103.左箱体二，104.左小 臂驱动电机，105.左小臂减速器，106.左小臂皮带轮传动机构，107.左小 臂驱动连杆，108.左大臂驱动电机，109.左大臂减速器，110.左大臂皮带 轮传动机构，111.左大臂，112.左小臂传动连杆，113.左小臂，113-1.左小 臂旋转关节连接部，114.旋转关节一，115.旋转关节二，116.旋转关节三， 117.旋转关节四，118.旋转关节五，119.旋转关节六，120.左三角形保持 架，121.左末端姿态保持连杆，122.左三角形保持架连杆，123.旋转关节 七，124.左承重托板，125.左末端连杆，126.旋转关节九，

200.右机械臂，201.右底座，202.右箱体一，203.右箱体二，204.右 小臂驱动电机，205.右小臂减速器，206.右小臂皮带轮传动机构，207.右 小臂驱动连杆，208.右大臂驱动电机，211.右大臂，212.右小臂传动连杆， 213.右小臂，213-1.右小臂旋转关节连接部，214.旋转关节八，217.旋转关 节十一，219.旋转关节十三，220.右三角形保持架，221.右末端姿态保持 连杆，222.右三角形保持架连杆，224.右承重托板，225.右末端连杆；

300.底盘，400.脚轮，500.麦克纳姆轮，600.丝杠副，700.皮带轮 传动机构，800.丝杠副驱动电机；

1.上位机，2.控制器一，3.控制器二，4.驱动器一，5.驱动器二， 6.编码器一，7.编码器二，8.驱动器三，9.启动器四，10.编码器三，11. 编码器四。

具体实施方式

以下参照附图，以具体实施例对本发明作进一步详细说明。

如图1-7所示，移乘机器人包括左机械臂100、右机械臂200、底盘 300、脚轮400、麦克纳姆轮500、丝杠副600、皮带轮传动机构700、丝 杠副驱动电机800，两个脚轮400与底盘300的后端连接；两个麦克纳姆 轮500与底盘300的前端连接，丝杠副600安装在底盘300上，丝杠副 驱动电机800安装在底盘300上，丝杠副驱动电机800的输出轴通过皮 带轮传动机构700与丝杠副600的丝杠连接。麦克纳姆轮500能够方便 机器人整体前行、横移、斜行或旋转。

丝杠副600设有螺母座一和螺母座二，丝杠副600的丝杠转动时， 螺母座一和螺母座二相互远离平移或相互靠拢平移。左机械臂100上的 左底座101与螺母座一连接，右机械臂200上的右底座201与螺母座二 连接。

左机械臂100包括左底座101、左箱体一102、左箱体二103、左小 臂驱动电机104、左小臂减速器105、左小臂皮带轮传动机构106、左小 臂驱动连杆107、左大臂驱动电机108、左大臂减速器109、左大臂皮带 轮传动机构110、左大臂111、左小臂传动连杆112、左小臂113、旋转关 节一114、旋转关节二115、旋转关节三116、旋转关节四117、旋转关节 五118、旋转关节六119、左三角形保持架120、左末端姿态保持连杆121、 左三角形保持架连杆122、旋转关节七123、左承重托板124、左末端连 杆125、旋转关节九126，左箱体一102和左箱体二103固定连接在左底 座101上，左小臂驱动电机104、左小臂减速器105安装在左箱体一102 上，左小臂驱动电机104的输出轴通过左小臂皮带轮传动机构106与左 小臂减速器105的输入端连接，左小臂驱动连杆107的一端与左小臂减 速器105的输出端连接；左大臂驱动电机108、左大臂减速器109安装在 左箱体二103上，左大臂驱动电机108的输出轴通过左大臂皮带轮传动 机构110与左大臂减速器109的输入端连接，左大臂111的一端与左大 臂减速器109的输出端连接，左小臂传动连杆112的一端通过旋转关节 一114与左小臂驱动连杆107的另一端连接；左小臂113的一端通过旋 转关节九126与左末端连杆125的一端连接，左小臂113的另一端设有 左小臂旋转关节连接部113-1，左小臂旋转关节连接部113-1通过旋转关 节三116与左三角形保持架120连接，左小臂传动连杆112的另一端通 过旋转关节二115与左小臂旋转关节连接部113-1连接；左大臂111的另 一端通过一个旋转关节与左三角形保持架120连接，左大臂111和左小 臂113在左三角形保持架120上的转动轴线是重合的。左末端姿态保持 连杆121的一端通过旋转关节四117与左三角形保持架120连接，左末 端姿态保持连杆121的另一端通过旋转关节六119与左末端连杆125的 另一端连接；左三角形保持架连杆122的一端通过旋转关节五118与左 三角形保持架120连接，左三角形保持架连杆122的另一端通过旋转关 节七123与左箱体二103连接，左承重托板124与左末端连杆125连接。

右机械臂200与左机械臂100的结构相同。右机械臂200包括右底 座201、右箱体一202、右箱体二203、右小臂驱动电机204、右小臂减 速器205、右小臂皮带轮传动机构206、右小臂驱动连杆207、右大臂驱 动电机208、右大臂减速器、右大臂皮带轮传动机构、右大臂211、右小 臂传动连杆212、右小臂213、旋转关节八214、旋转关节十一217、旋 转关节十三219、右三角形保持架220、右末端姿态保持连杆221、右三 角形保持架连杆222、右承重托板224、右末端连杆225，右箱体一202、 右箱体二203固定安装在右底座201上，右小臂驱动电机204、右小臂减 速器205安装在右箱体一202上，右大臂驱动电机208和右大臂减速器 安装在右箱体二203上。

右小臂驱动电机204的输出轴通过右小臂皮带轮传动机构206与右 小臂减速器205的输入端连接，右小臂驱动连杆207的一端与右小臂减 速器205的输出端连接；右大臂驱动电机208的输出轴通过右大臂皮带 轮传动机构与右大臂减速器的输入端连接，右大臂211的一端与右大臂 减速器的输出端连接，右小臂传动连杆212的一端通过旋转关节八214与右小臂驱动连杆207的另一端连接；右小臂213的一端通过旋转关节 与右末端连杆225的一端连接，右小臂213的另一端设有右小臂旋转关 节连接部213-1，右小臂旋转关节连接部213-1通过旋转关节与右三角形 保持架220连接，右小臂传动连杆212的另一端通过旋转关节与右小臂 旋转关节连接部213-1连接；右大臂211的另一端通过一个旋转关节与右 三角形保持架220连接，右大臂211和右小臂213在右三角形保持架220 上的转动轴线是重合的。右末端姿态保持连杆221的一端通过旋转关节 十一217与右三角形保持架220连接，右末端姿态保持连杆221的另一 端通过旋转关节十三219与右末端连杆225的另一端连接；右三角形保 持架连杆222的一端通过旋转关节与右三角形保持架220连接，右三角 形保持架连杆222的另一端通过旋转关节与右箱体二203连接，右承重 托板224与右末端连杆225连接。

如图8所示，单个机械臂的平面运动机构原理图中，包括三个平行 四边形和一个三角形，其中：

1)

ADIJ为第一个平行四边形结构，AD边相当于左大臂111，IJ 边相当于左三角形保持架连杆122，DI边相当于左三角形保持架120的 底边。因为A和J两点相对于底座构件是固定的，当主动构件AD转动 时，通过平行四边形结构的约束，ΔHDI在平面的姿态是保持不变的。也 就是说，构件AD决定ΔHDI在竖直平面的位置；

2)

GHDE为第二个平行四边形结构，GE边相当于左末端连杆125， GH边相当于左末端姿态保持连杆121，HD边相当于左三角形保持架120 的一个侧边，DE边相当于左小臂113的前半部分，EC边相当于整个左 小臂113。由于ΔHDI在竖直平面的姿态保持不变，因而构件GE的平面 姿态保持不变，从而使得构件FM始终与水平面保持平行。构件CE决定 构件FM在平面中的位置。

3)

ABCD为第三个平行四边形结构，AB边相当于左小臂驱动连杆 107，BC边相当于左小臂传动连杆112，CD边相当于左小臂旋转关节连 接部113-1。构件AB决定构件CE在平面上的俯仰姿态，而构件AD决 定构件CE在平面上的位置，并且构件AB与构件AD都是主动件。

通过

ADIJ与

GHDE和ΔHDI实现构件FM(左承重托板124) 始终与水平面保持平行。

因此，左小臂驱动电机104和左大臂驱动电机108工作，从而驱动 左承重托板124在一个平面内前、后、上、下移动。右小臂驱动电机204 和右大臂驱动电机208工作，从而驱动右承重托板224在一个平面内前、 后、上、下移动。

左大臂111为驱动部件，其转动角度不超过360°。左小臂驱动连杆 107为驱动部件，其转动角度不超过360°。右大臂和右小臂驱动连杆其 转动角度不超过360°。优选地方案是，左大臂111的转动角度范围是 25～160°，左小臂驱动连杆107的转动角度范围是-80～70°。右大臂 211的转动角度范围是25～160°，右小臂驱动连杆207的转动角度范围 是-80～70°。

在使用移乘机器人，通常使用左承重托板124和右承重托板224以 双臂支撑抱人的方式实施移动、换乘、转运工作。当遇到处于无意识或 者极其虚弱的状态的处理对象，他们并不能主动配合医护人员或移乘机 器人完成转运工作，其身体会不自主地倾斜或堆萎，导致其实际姿态与 理想姿态有较大差异，因而极有可能发生掉落的危险。因此，在左承重托板124和右承重托板224连接一个束带，让病人躺在束带上，防止跌 落。

在使用移乘机器人对某些特殊伤病员进行搬运操作时，在左承重托 板124和右承重托板224上连接一个平板，让病员躺在该平板上。

启动丝杠副驱动电机800，调整左承重托板124和右承重托板224之 间的距离，以满足不同的病人的需要。

为了控制左机械臂100和右机械臂200同步运动，如图9所示，机 器人的控制***包括上位机1、控制器一2、控制器二3、驱动器一4、 驱动器二5、编码器一6、编码器二7、驱动器三8、驱动器四9、编码器 三10、编码器四11，控制器一2与上位机1连接，控制器二3与上位机 1连接，上位机1统筹控制器一2和控制器二3工作。

控制器一2设有串口一，驱动器一4的输入端与串口一连接，驱动 器一4的输出端与左小臂驱动电机104连接。驱动器二5的输入端与串 口一连接，左大臂驱动电机108与驱动器二5的输出端连接。编码器一6 与左小臂驱动电机104连接，编码器一6的信号输出端与控制器一2连 接。编码器二7与左大臂驱动电机108连接，编码器二7的信号输出端 与控制器一2连接。编码器一6实时反馈左小臂驱动电机104的位置信 号为控制器一，编码器二7实时反馈左大臂驱动电机108的位置信号为 控制器一。控制器一2的串口一发送的控制指令同时传输给左小臂驱动 电机104和左大臂驱动电机108，实现左小臂驱动电机104和左大臂驱动 电机108的同步工作，从而使左小臂驱动连杆107和左大臂111同步动 作协调配合。最终使左承重托板124运动至xyz三维空间的目标位置。

控制器二3设有串口二，驱动器三8的输入端与串口二连接，驱动 器三8的输出端与右小臂驱动电机204连接。驱动器四9的输入端与串 口二连接，右大臂驱动电机208与驱动器四9的输出端连接。编码器三 10与右小臂驱动电机204连接，编码器三10的信号输出端与控制器二3 连接。编码器四11与右大臂驱动电机208连接，编码器四11的信号输 出端与控制器二3连接。编码器三10实时反馈右小臂驱动电机204的位 置信号给控制器二，编码器四11实时反馈右大臂驱动电机208的位置信 号给控制器二。控制器二3的串口二发送的控制指令同时传输给右小臂 驱动电机204和右大臂驱动电机208，实现右小臂驱动电机204和右大臂 驱动电机208的同步工作，从而使右小臂驱动连杆207和右大臂211同 步动作协调配合，最终让右承重托板224运动至xyz三维空间的目标位 置。

如图10所示，上位机1设有ROS(Robot Operating System)***和显示器， ROS***设有RViz仿真模块1-1、控制模块1-2、数据处理模块1-3，数 据处理模块1-3与RViz仿真模块1-1连接通信，控制模块1-2与数据处 理模块1-3连接通信。

上位机进行三维建模，显示与真实的左机械臂100和右机械臂200 相同尺寸的三维模型，设置模型关节和机械臂关节一一对应，实现机械 臂模型和真实机械臂各关节实时关联。RViz仿真模块1-1调用建立好的 三维模型。当上位机1控制RViz仿真模块1-1中机械模型的运动时，左 机械臂100和右机械臂200跟随运动并向RViz仿真模块1-1实时反馈各关节状态，当真实机械臂的反馈状态未达到机械模型状态时，上位机计 算差值后继续发送指令控制机械臂运动，直至左机械臂100和右机械臂 200运动到达设定的目标位置。

上位机1：当上位机1得到外部输入的运动到指定位置(坐标xyz， 速度)的运动指令，经过数据处理模块1-3进行逆解得到各关节运动参 数(运动角度、速度)，并将运动参数数据发给RViz仿真模块1-1。

参考12，正逆解的具体过程是：机械臂的正运动学问题是已知关节 变量θ₁₁、θ₂₁，求解末端执行器左承重托板124或右承重托板224相对于 基坐标系的位姿。当移乘机器人底盘固定不动时，左机械臂或右机械臂 仅在平面中运动，可以将正运动学问题进行简化，以左机械臂为例，先 确定左末端连杆125的下端在平面中的位置E(x₀,y₀)，然后结合底盘的运 动便可以确定末端执行器左承重托板124在空间中的位姿。采用向量法 进行计算求解：

在固定参考系A-xy下构造闭环方程：

公式(2-1)中

k₁₁＝L₁,k₁₂＝L₄,k₂₁＝L₂,k₂₂＝L₃。

为C点的位矢，k₁₁、k₁₂、k₂₁、k₂₂分别代表AD杆、CD杆、AB杆、 BC杆的长度，

分别表示原动件AD杆、AB杆的单位矢量，

分别表示从动件CD杆、BC杆的单位矢量。

将公式(2-1)两边取模的平方得：

i＝1,2时，将(2-2)、(2-3)相加后分别得到：

即：

公式(2-4)和(2-5)相减后得到

公式(2-7)展开得：

y₂＝Dx₂+E (2-8)

公式(2-8)中，

将式(2-8)代入式(2-6)得

公式(2-9)中，F＝D²+1，G_i＝DE-k_i1cosθ₂₁-Dk_i1sinθ₂₁，

由此得出两组正解，根据装配模式，取

将(2-10)代入(2-8)便可确定y₂值，得到位矢C(x₂,y₂)^T。

为保证机械臂的稳定工作，∠GED、∠HGE、∠ABC角度值不能低于

所以需要将无效解剔除。首先验证∠GED、∠HGE的有效性。

x₁＝l₁cosθ₁₁ (2-11)

y₁＝l₁sinθ₁₁ (2-12)

x₃＝l₁cosθ₁₁+l₇cos∠HDI (2-13)

y₃＝l₁sinθ₁₁+l₇sin∠HDI (2-14)

求得D(x₁,y₁)、H(x₃,y₃)。

由于∠GED+∠HGE＝π，∠GED＝∠CDH，所以剔除不满足

的 C(x₂,y₂)坐标。

对于∠ABC，已知A(0,0)，C(x₂,y₂)，B点坐标可求

x₄＝l₂cosθ₂₁ (2-15)

y₄＝l₂sinθ₂₁ (2-16) 剔除不满足

的C(x₂,y₂)坐标。

至此，有效的C(x₂,y₂)坐标便可唯一确定。

已知C点、D点、E点共线，则必存在不与直线CD共线的点Q(x₅,y₅)， 以及实数u，使得：

则有

x₀＝ux₂+(1-u)x₁ (2-17)

y₀＝uy₂+(1-u)y₁ (2-18)

由DE杆为定长l₅可得

将式(2-17)、式(2-18)代入式(2-19)中，可得

取

式(2-20)代入式(2-17)、式(2-18)便可求得E点位矢(x₀,y₀)^T。

机械臂的逆运动学问题是已知末端执行器的位姿，来求解该位姿对 应的关节转角θ₁₁、θ₂₁，这往往是实际工程上更关心的问题。求解机器人 运动学反解是为了得到各运动关节的变化参数，更好地实现机器人的控 制。只有各关节变量运动参数确定后，机器人的末端执行器才能到达所 期望的位姿。

已知A(0,0)、E(x₀,y₀)，且杆DE、AD分别为定长l₅、l₁，则有

式(2-21)、式(2-22)联立后可得

y₁＝ax₁+b (2-23)

其中，

式(2-21)、式(2-23)联立后可得

式(2-24)代入式(2-23)后便可求得y₁，取较大的y₁值及其所对应的x₁。

为保证机械臂的稳定工作，∠GED、∠EDH角度值不能低于

所以需要将无效解剔除。由于∠GED+∠EDH＝π，所以剔除不满足

的D(x₁,y₁)。

左大臂111的转角

x₁≠0；

x₁＝0时，大臂与水平面垂直，此时有唯一解

已知C点、D点、E点共线，则必存在不与直线ED共线的点P(x₆,y₆)， 以及实数λ，使得

则有:

x₂＝λx₁+(1-λ)x₀ (2_-25)

y₂＝λy₁+(1-λ)y₀ (2_-26)

由CD杆为定长l₄可得

将式(2-25)、式(2-26)和式(2-27)联立可得

取

将式(2-28)代入式(2-25)、式(2-26)即可求得C点位矢(x₂,y₂)^T。

在固定参考系A-xy下构造闭环方程

两边取模的平方后相加得

得到三角方程

Asinθ₂₁+Bcosθ₂₁＝C (2-31)

其中，A＝2y₂k₂₁，B＝2x₂k₂₁，

利用三角代换A＝ρ·cosφ，B＝ρ·sinφ，则

φ＝atan2(B,A)。

式(2-31)可化简为

则

所以

小臂转角

为避免机械臂工作过程中发生紊乱，还需要剔除不满足

的θ₂₁。

由此便求得移乘机器人机械臂的运动学逆解。

上位机通过solidwork软件生成三维模型，生成三维模型的各个连 杆(GE、DE、CD、HG、DHI、BC、AB、AD、IJ)的参数模型(坐标系、转轴、 材质、颜色、外形尺寸、转动惯量等)。上位机完成各个关节参数信息的定义(包 括初始位姿、旋转类型、关节转动角度范围、转动速度、碰撞检测等)。

将关节变量θ₁₁、θ₂₁数据发给RViz仿真模块1-1。

RViz仿真模块1-1调用已经建立好的三维模型。

RViz显示模块读取三维模型的各个连杆的参数信息。

RViz显示模块将各个连杆的参数信息发送给结构转换树生成模块。

结构转换树生成模块建立RViz基坐标和机械臂基坐标之间的联系。

结构转换树生成模块建立机械臂各关节坐标之间联系。

结构转换树生成模块生成结构转换树。

RViz显示模块调用结构转换树显示理想的虚拟机械臂。

编码器一6和编码器二7发送的运动角度数据给数据处理模块，数 据处理模块计算出各个连杆(GE、DE、CD、HG、DHI、BC、AB、AD、IJ) 可旋转的关节的运动角度数据，RViz仿真模块调用各个连杆可旋转的关 节的运动角度数据，RViz显示模块显示实际的虚拟机械臂。

控制模块1-2根据θ₁₁、θ₂₁等数据生成脉冲信号数(目标脉冲值)发 送给驱动器，驱动器给驱动电机(步进电机)一个一个发送脉冲，实现 驱动电机运动，同时改变脉冲频率可以改变运动速度，编码器读取电机 转动圈数，每隔固定的圈数产生一个脉冲(实际脉冲值)，经过数据处理 模块1-3将实际脉冲值进行正解得到机械臂所到的位置，再调用RViz显示模块将实际的机械臂位置显示出来，与期望的目标位置进行比较。上 位机判断各关节差值是否在误差允许范围内，不符合要求则控制模块1-2 继续发送指令给驱动器，直至承重托板运动到达设定的目标位置。

以上所述仅对本发明的优选实施例而已，并不用于限制本发明，对 于本领域的技术人员来说，本发明可以有各种更改和变化。

Claims (3)

1.一种使用移乘机器人的移乘方法，其特征在于，所述移乘机器人包括左机械臂、右机械臂和底盘，所述左机械臂包括左底座、左箱体一、左箱体二、左小臂驱动电机、左小臂减速器、左小臂传动机构、左小臂驱动连杆、左大臂驱动电机、左大臂减速器、左大臂传动机构、左大臂、左小臂传动连杆、左小臂、旋转关节一、旋转关节二、旋转关节三、旋转关节四、旋转关节五、旋转关节六、左三角形保持架、左末端姿态保持连杆、左三角形保持架连杆、旋转关节七、左承重托板、左末端连杆和旋转关节九，所述左箱体一和左箱体二连接于左底座上，所述左小臂驱动电机和左小臂减速器连接于左箱体一上，所述左大臂驱动电机和左大臂减速器连接于左箱体二上；所述左小臂驱动电机的输出轴通过左小臂传动机构与左小臂减速器的输入端连接，左小臂驱动连杆的一端与左小臂减速器的输出端连接；所述左大臂驱动电机的输出轴通过左大臂传动机构与左大臂减速器的输入端连接，左大臂的一端与左大臂减速器的输出端连接，左小臂传动连杆的一端通过旋转关节一与左小臂驱动连杆的另一端连接；左小臂的一端通过旋转关节九与左末端连杆的一端连接，左小臂的另一端设有左小臂旋转关节连接部，左小臂旋转关节连接部通过旋转关节三与左三角形保持架连接，左小臂传动连杆的另一端通过旋转关节二与左小臂旋转关节连接部连接；左大臂的另一端通过旋转关节与左三角形保持架连接，左大臂和左小臂在左三角形保持架上的转动轴线是重合的，左末端姿态保持连杆的一端通过旋转关节四与左三角形保持架连接，左末端姿态保持连杆的另一端通过旋转关节六与左末端连杆的另一端连接；左三角形保持架连杆的一端通过旋转关节五与左三角形保持架连接，左三角形保持架连杆的另一端通过旋转关节七与左箱体二连接，所述左承重托板与左末端连杆连接；

所述右机械臂包括右底座、右箱体一、右箱体二、右小臂驱动电机、右小臂减速器、右小臂传动机构、右小臂驱动连杆、右大臂驱动电机、右大臂减速器、右大臂传动机构、右大臂、右小臂传动连杆、右小臂、旋转关节八、旋转关节十一、旋转关节十三、右三角形保持架、右末端姿态保持连杆、右三角形保持架连杆、右承重托板和右末端连杆，所述右箱体一和右箱体二连接于右底座上，所述右小臂驱动电机和右小臂减速器连接于右箱体一上，所述右大臂驱动电机和右大臂减速器连接于右箱体二上；所述右小臂驱动电机的输出轴通过右小臂传动机构与右小臂减速器的输入端连接，右小臂驱动连杆的一端与右小臂减速器的输出端连接；右大臂驱动电机的输出轴通过右大臂传动机构与右大臂减速器的输入端连接，右大臂的一端与右大臂减速器的输出端连接，右小臂传动连杆的一端通过旋转关节八与右小臂驱动连杆的另一端连接；右小臂的一端通过旋转关节与右末端连杆的一端连接，右小臂的另一端设有右小臂旋转关节连接部，右小臂旋转关节连接部通过旋转关节与右三角形保持架连接，右小臂传动连杆的另一端通过旋转关节与右小臂旋转关节连接部连接；右大臂的另一端通过一个旋转关节与右三角形保持架连接，右大臂和右小臂在右三角形保持架上的转动轴线是重合的；右末端姿态保持连杆的一端通过旋转关节十一与右三角形保持架连接，右末端姿态保持连杆的另一端通过旋转关节十三与右末端连杆的另一端连接；右三角形保持架连杆的一端通过旋转关节与右三角形保持架连接，右三角形保持架连杆的另一端通过旋转关节与右箱体二连接，右承重托板与右末端连杆连接；

所述左底座与底盘连接，所述右底座与底盘连接；

所述左底座和右底座与底盘之间设有丝杠副，丝杠副连接有丝杠副驱动电机，丝杠副与底盘连接，所述丝杠副设有螺母座一和螺母座二，所述左底座与螺母座一连接，所述右底座与螺母座二连接；所述底盘连接有两个脚轮和两个麦克纳姆轮；

所述移乘机器人还包括控制***，所述控制***包括上位机、控制器一、控制器二、驱动器一、驱动器二、编码器一、编码器二、驱动器三、驱动器四、编码器三和编码器四，所述控制器一与上位机连接，控制器二与上位机1连接；所述控制器一设有串口一，所述驱动器一的输入端与串口一连接，驱动器一的输出端与左小臂驱动电机连接；所述驱动器二的输入端与串口一连接，左大臂驱动电机与驱动器二的输出端连接，所述编码器一与左小臂驱动电机连接，编码器一的信号输出端与控制器一连接；编码器二与左大臂驱动电机连接，编码器二的信号输出端与控制器一连接；所述控制器二设有串口二，驱动器三的输入端与串口二连接，驱动器三的输出端与右小臂驱动电机连接；驱动器四的输入端与串口二连接，右大臂驱动电机与驱动器四的输出端连接；编码器三与右小臂驱动电机连接，编码器三的信号输出端与控制器二连接，编码器四与右大臂驱动电机连接，编码器四的信号输出端与控制器二连接；所述上位机设有ROS***和显示器，ROS***设有RViz仿真模块、RViz显示模块、控制模块、数据处理模块，所述数据处理模块与RViz仿真模块连接，控制模块与数据处理模块连接通信；

所述移乘方法包括以下步骤：

先建立好三维模型，显示与实际左机械臂和右机械臂相同尺寸的三维模型；

利用ROS中的RViz仿真模块调用三维模型，实现三维模型和实际机械臂各关节实时关联；当上位机控制RViz仿真模块中三维模型的运动时，左机械臂和右机械臂跟随运动并向RViz仿真模块实时反馈各关节状态信息，当实际机械臂的反馈状态未达到三维模型状态时，上位机计算差值后继续发送指令控制机械臂运动，直至左机械臂和右机械臂运动到达设定的目标位置；

当上位机得到外部输入的运动到指定位置的运动指令，经过数据处理模块进行逆解得到各关节运动参数，并将运动参数数据发给RViz仿真模块；

逆解的具体过程是：机械臂的正运动学问题是已知关节变量θ₁₁、θ₂₁，求解末端执行器左承重托板或右承重托板相对于基坐标系的位姿；当移乘机器人的底盘固定不动时，左机械臂或右机械臂仅在平面中运动，可以将正运动学问题进行简化，以左机械臂为例，先确定左末端连杆的下端在平面中的位置E(x₀,y₀)，然后结合底盘的运动便可以确定左承重托板在空间中的位姿，采用向量法进行计算求解：

在固定参考系A-xy下构造闭环方程：

公式(2-1)中

为C点的位矢，k₁₁、k₁₂、k₂₁、k₂₂分别代表AD杆、CD杆、AB杆、BC杆的长度，

分别表示原动件AD杆、AB杆的单位矢量，

分别表示从动件CD杆、BC杆的单位矢量；

将公式(2-1)两边取模的平方得：

i＝1,2时，将(2-2)、(2-3)相加后分别得到：

即：

公式(2-4)和(2-5)相减后得到：

公式(2-7)展开得：

y₂＝Dx₂+E (2-8)

公式(2-8)中，

将式(2-8)代入式(2-6)得：

公式(2-9)中，F＝D²+1，G_i＝DE-k_i1cosθ₂₁-Dk_i1sinθ₂₁，

由此得出两组正解，根据装配模式，取

将(2-10)代入(2-8)便可确定y₂值，得到位矢C(x₂,y₂)^T；

为保证机械臂的稳定工作，∠GED、∠HGE、∠ABC角度值不能低于

所以需要将无效解剔除，首先验证∠GED、∠HGE的有效性，

x₁＝l₁cosθ₁₁ (2-11)

y₁＝l₁sinθ₁₁ (2-12)

x₃＝l₁cosθ₁₁+l₇cos∠HDI (2-13)

y₃＝l₁sinθ₁₁+l₇sin∠HDI (2-14)

求得D(x₁,y₁)、H(x₃,y₃)；

由于∠GED+∠HGE＝π，∠GED＝∠CDH，所以剔除不满足

的C(x₂,y₂)坐标；

对于∠ABC，已知A(0,0)，C(x₂,y₂)，B点坐标可求

x₄＝l₂cosθ₂₁ (2-15)

y₄＝l₂sinθ₂₁ (2-16)

剔除不满足

的C(x₂,y₂)坐标；

至此，有效的C(x₂,y₂)坐标便可唯一确定；

已知C点、D点、E点共线，则必存在不与直线CD共线的点Q(x₅,y₅)，以及实数u，使得：

则有

x₀＝ux₂+(1-u)x₁ (2-17)

y₀＝uy₂+(1-u)y₁ (2-18)

由DE杆为定长l₅可得

将式(2-17)、式(2-18)代入式(2-19)中，可得

式(2-20)代入式(2-17)、式(2-18)便可求得E点位矢(x₀,y₀)^T；

机械臂的逆运动学问题是已知末端执行器的位姿，来求解该位姿对应的关节转角θ₁₁、θ₂₁，求解机器人运动学反解是为了得到各运动关节的变化参数；

已知A(0,0)、E(x₀,y₀)，且杆DE、AD分别为定长l₅、l₁，则有

式(2-21)、式(2-22)联立后可得

y₁＝ax₁+b (2-23)

其中，

式(2-21)、式(2-23)联立后可得

式(2-24)代入式(2-23)后便可求得y₁，取较大的y₁值及其所对应的x₁；为保证机械臂的稳定工作，∠GED、∠EDH角度值不能低于

所以需要将无效解剔除。由于∠GED+∠EDH＝π，所以剔除不满足

的D(x₁,y₁)；

左大臂的转角

x₁≠0；

x₁＝0时，大臂与水平面垂直，此时有唯一解

已知C点、D点、E点共线，则必存在不与直线ED共线的点P(x₆,y₆)，以及实数λ，使得

则有:

x₂＝λx₁+(1-λ)x₀ (2-25)

y₂＝λy₁+(1-λ)y₀ (2-26)

由CD杆为定长l₄可得

将式(2-25)、式(2-26)和式(2-27)联立可得

取

将式(2-28)代入式(2-25)、式(2-26)即可求得C点位矢(x₂,y₂)^T；

在固定参考系A-xy下构造闭环方程

两边取模的平方后相加得

得到三角方程

Asinθ₂₁+Bcosθ₂₁＝C (2-31)

其中，A＝2y₂k₂₁，B＝2x₂k₂₁，

利用三角代换A＝ρ·cosφ，B＝ρ·sinφ，则

φ＝atan2(B,A)。

式(2-31)可化简为

则

所以

小臂转角

为避免机械臂工作过程中发生紊乱，还需要剔除不满足

的θ₂₁；

由此便求得移乘机器人机械臂的运动学逆解；

上位机通过solidwork软件生成三维模型，生成三维模型的各个连杆的参数模型，上位机完成各个关节参数信息的定义；

将关节变量θ₁₁、θ₂₁数据发给RViz仿真模块；

RViz仿真模块调用已经建立好的三维模型；

RViz显示模块读取三维模型的各个连杆的参数信息；

RViz显示模块将各个连杆的参数信息发送给结构转换树生成模块；

结构转换树生成模块建立RViz基坐标和机械臂基坐标之间的联系；

结构转换树生成模块建立机械臂各关节坐标之间联系；

结构转换树生成模块生成结构转换树；

RViz显示模块调用结构转换树显示理想的虚拟机械臂；

编码器一和编码器二发送的运动角度数据给数据处理模块，数据处理模块计算出各个连杆可旋转的关节的运动角度数据，RViz仿真模块调用各个连杆可旋转的关节的运动角度数据，RViz显示模块显示实际的虚拟机械臂。

2.根据权利要求1所述的使用移乘机器人的移乘方法，其特征在于，所述右承重托板和左承重托板之间连接有束带。

3.根据权利要求1所述的使用移乘机器人的移乘方法，其特征在于，所述左大臂的转动角度范围是25～160°，所述左小臂驱动连杆的转动角度范围是-80～70°，所述右大臂的转动角度范围是25～160°，所述右小臂驱动连杆207的转动角度范围是-80～70°。

Images