CN113288427B - 一种悬挂定位机械臂及控制方法 - Google Patents

Abstract

本发明涉及机械臂领域，具体地说是一种悬挂定位机械臂及控制方法，包括底座、升降关节、第一旋转关节、伸缩关节、第二旋转关节、第三旋转关节、第四旋转关节和末端操作器，第一旋转关节通过升降关节驱动升降，伸缩关节通过第一旋转关节驱动转动，第二旋转关节通过伸缩关节驱动移动，第三旋转关节通过第二旋转关节驱动转动，第三旋转关节设有可转动的支撑架，第四旋转关节和末端操作器安装于所述支撑架上，且末端操作器通过第四旋转关节驱动转动，支撑架中部设有把手一和力传感器B，支撑架与第四旋转关节连接一端设有把手二和力传感器A。本发明可实现机械臂的三维位置拖动、远心定位拖动与单轴运动控制，结构简单、易于操作且控制精确。

Description

一种悬挂定位机械臂及控制方法

技术领域

本发明涉及机械臂领域，具体地说是一种悬挂定位机械臂及控制方法。

背景技术

微创手术具有创伤小、疼痛轻、康复快等优点，但传统微创手术由于设备缺陷使得医生很难实现手眼协调工作，而由于机器人技术的快速发展，将机器人技术与微创手术相结合越来越受到认可，并且被医学界广泛应用于改善手术环境，其中远心机构在微创手术中起到了至关重要的作用，其通过特定机构的协调配合，能够实现空间点的远心运动，进而实现改善手术环境的功能。

现有的远心机构形式主要有两种类型，第一种是通过复杂的机构形式实现远心运动，如平行四边形支链结构，其存在结构形式复杂、对加工精度的要求过高、装配难度大、含有冗余约束且体积庞大等缺点；第二种形式为通过多个转动副构成的串联型机械臂结构，该结构存在关节数多、存在空间奇异值点、成本高且控制难度大等问题。

发明内容

本发明的目的在于提供一种悬挂定位机械臂及控制方法，其包括六个关节串联构成的机械臂，并可通过运动学解算与控制相结合实现远心运动，并控制末端连接操作器进行腹腔手术，可实现机械臂的三维位置拖动、远心定位拖动与单轴运动控制，具有结构简单、易于操作、不存在奇异点且零部件不需要高精度加工等优点。

本发明的目的是通过以下技术方案来实现的：

一种悬挂定位机械臂，包括底座、升降关节、第一旋转关节、伸缩关节、第二旋转关节、第三旋转关节、第四旋转关节和末端操作器，升降关节安装于底座上，第一旋转关节通过升降关节驱动升降，伸缩关节通过第一旋转关节驱动转动，第二旋转关节通过伸缩关节伸缩驱动移动，第三旋转关节通过第二旋转关节驱动转动，第三旋转关节包括支撑架和驱动单元，且所述支撑架通过所述驱动单元驱动转动，第四旋转关节和末端操作器设于支撑架上，且末端操作器通过第四旋转关节驱动转动，所述支撑架中部设有把手一和力传感器B，所述支撑架与第四旋转关节连接一端设有把手二和力传感器A，所述支撑架上设有使能按钮、按钮A和按钮B。

所述升降关节包括电动推缸和第一关节支撑件，电动推缸安装在所述底座上，且第一关节支撑件通过所述电动推缸驱动升降，所述第一旋转关节包括第二关节电机、齿轮一、齿轮二、交叉滚子轴环和第二关节连接件，其中第二关节电机设于第一关节支撑件上端内的一侧，齿轮一安装于第二关节电机的输出端，齿轮二可转动地设于第一关节支撑件上端中部，且所述齿轮二与齿轮一啮合，所述齿轮二与交叉滚子轴环内圈一侧同轴连接，交叉滚子轴环内圈的另一侧与第二关节连接件同轴连接，伸缩关节固装于所述第二关节连接件上。

所述底座上侧设有包裹所述电动推缸的第一关节外壳，所述第一关节外壳上端与一个包裹所述第一关节支撑件的第二关节外壳相连，所述第一关节外壳和第二关节外壳横截面可以为方形或圆形。

所述伸缩关节包括第三关节支撑板、伸缩机构和第三关节连接件，其中第三关节支撑板与第一旋转关节连接，伸缩机构安装于第三关节支撑板上，所述伸缩机构包括电机模组、丝杠和滑块，且所述丝杠通过电机模组驱动转动，所述滑块内部设有丝母套装于所述丝杠上，所述第三关节连接件与所述滑块固连，第二旋转关节安装于所述第三关节连接件端部。

所述第二旋转关节包括端关节和第四关节连接件，其中端关节固装于伸缩关节自由移动端部，所述端关节内设有第四关节电机，且所述第四关节连接件通过所述第四关节电机驱动转动，所述第三旋转关节的驱动单元包括第五关节电机、第五关节轴和第五关节交叉滚子轴环，且所述第四关节连接件一端插入所述支撑架中，并且在所述第四关节连接件端部一侧设有第五关节电机、另一侧设有第五关节轴，其中第五关节电机输出端与支撑架中部位置的一侧连接，第五关节轴一端通过第五关节交叉滚子轴环与第四关节连接件相连、另一端与支撑架中部位置的另一侧连接。

所述支撑架整体呈C型且为两侧薄板结构，并且两侧薄板上端和下端分别对应汇聚到一起，末端操作器两端分别与支撑架两端相连，第四旋转关节设于支撑架一端。

所述底座下侧设有脚轮、上侧设有支撑柱，且所述支撑柱上设有推手和显示屏。

一种根据所述悬挂定位机械臂的控制方法，其特征在于：机械臂处于三维位置拖动控制模式时，将力传感器B检测到的在把手一上施加的拖动力

映射为把手一所在位置的三维位置移动速度

设机器臂位置运动量为Δs＝[Δs_x，Δs_y，Δs_z],若机器人控制周期为Δt，，则：

机械臂的初始位姿为：

若机器人末端位置点从初始位置T_c在拖动控制下经过一个控制周期运动到T_e,则经过拖动控制后的位姿矩阵为：

然后基于逆运动学求解算得出各个关节的运动量并实现拖动控制。

一种根据所述悬挂定位机械臂的控制方法，其特征在于：使能按钮关闭，机械臂处于远心运动控制模式，此时机械臂的末端位置点(机械臂与末端操作器连接位置)绕指定点做半径r的球面运动，力传感器A检测把手二获得给定的拖动力：

机械臂的末端位置点通过上述作用力绕指定点做圆弧运动，且圆弧运动的角速度

大小与作用力大小成比例，即：

若机器人控制周期为Δt，则机械臂的末端位置点运动的圆弧角度α大小为：

若机器人末端位置点从初始位置P_c＝[x_c，y_c，z_c]^T在拖动控制下经过一个控制周期运动到P_e＝[x_e，y_e，z_e]^T，则

与

的夹角为α，且

与

三线共面，令

为三线共面的法向矢量,则：

上式(1)中：

由此求得

设机器臂初始在P_c点时末端位姿矩阵为：

经过拖动控制运动后到达P_e点，此时末端位姿矩阵为：

由P_c到P_e的运动过程等效为

绕

旋转α角度后得到，则其姿态变化关系为：

上式(2)中，

表示绕轴

旋转α角度的旋转变换矩阵，通过式(1)和(2)获得机器臂需要运动控制的目标位姿矩阵T_e，再基于逆运动学求解算得出各个关节的运动量实现远心拖动控制。

使能按钮开启，机械臂切换为单轴运动控制模式，将力传感器A检测把手二获得拖动力F_A直接映射为伸缩关节、第二旋转关节(4、第三旋转关节的单轴运动，其映射关系为：

且第四旋转关节通过按钮A、按钮B单独控制的正转与反转。

本发明的优点与积极效果为：

1、本发明利用六个模块化的关节构成PRPRRR型机械臂，通过新颖的结构形式达到相同的功能要求，其中按钮、把手与传感器安装在机械臂上特定的位置，通过其相互配合并结合算法解算，实现机械臂的三维位置拖动、远心定位拖动与单轴运动控制，控制精确、操作方便，功能稳定可靠、增强人机交互的安全性且降低设备成本。

2、本发明的支撑架整体呈C型且为两侧薄板结构，两侧薄板之间空隙便于折叠过程中将支撑架及末端操作器收拢在第四关节连接件周围，当处于工作状态时，支撑架处于展开状态，而其非工作状态下通过其特殊结构形式处于收拢状态，减少了设备整体的占用空间。相比现有的机构形式，本发明具有工作空间大、结构简单、安装拆卸方便、不存在空间奇异点等优点。

附图说明

图1为本发明的立体示意图一，

图2为本发明的立体示意图二，

图3为图1中底盘的结构示意图，

图4为图2中的A-A剖视图，

图5为图4中的I处放大图，

图6为图1中伸缩关节及第二旋转关节的结构示意图，

图7为图6中伸缩机构的结构示意图，

图8为图1中第三旋转关节、第四旋转关节及末端操作器的结构示意图，

图9为图8中第三旋转关节的结构示意图，

图10为图9中第三旋转关节的主视图

图11为图10中的B-B剖视图，

图12为图8中第三旋转关节、第四旋转关节及末端操作器的爆炸示意图，

图13为图8中支撑架的结构示意图，

图14为本发明算法中需要的各个关节坐标系及其运动形式图，

图15为本发明的机器人运动坐标系示意图，

图16为本发明的远心定位运动球坐标系示意图。

其中，1为升降关节，101为电动推缸，102为第一关节支撑件，103为第一关节外壳；2为第一旋转关节，201为第二关节外壳，202为第二关节电机，203为第二关节电机连接件，204为齿轮一，205为第二关节电机安装板，206为交叉滚子轴环，207为第二关节连接件，208为齿轮二；3为伸缩关节，301为第三关节支撑板，302为伸缩机构，3021为电机模组，3022为联轴器，3023为安装座，3024为滑块，3025为丝杠，3026为限位开关安装板，3027为限位开关，303为第三关节连接件，304为第三关节外壳；4为第二旋转关节，401为端关节，402为第四关节连接件；5为第三旋转关节，501为支撑架，502为力传感器B，503为把手一，504为使能按钮，505为按钮A，506为按钮B，507为力传感器A，508为把手二，509为第五关节电机，510为第五关节交叉滚子轴环，511为第二关节轴，512为挡圈；6为第四旋转关节，601为第六关节连接板，602为第六关节交叉滚子轴环，603为第六关节电机转接轴，604为第六关节电机，605为末端固定盘；7为末端操作器；8为底座，801为脚轮，802为底座称重板，803为推手，804为显示屏，805为支撑柱，806为座体。

具体实施方式

下面结合附图对本发明作进一步详述。

如图1～16所示，本发明包括升降关节1、第一旋转关节2、伸缩关节3、第二旋转关节4、第三旋转关节5、第四旋转关节6、末端操作器7以及底座8，升降关节1安装在底座8上且输出端与第一旋转关节2连接，所述第一旋转关节2通过所述升降关节1驱动升降，第一旋转关节2的输出端与伸缩关节3连接，所述伸缩关节3通过所述第一旋转关节2驱动转动，伸缩关节3的输出端与第二旋转关节4连接，所述第二旋转关节4通过所述伸缩关节3伸缩驱动移动，第二旋转关节4的输出端与第三旋转关节5连接，所述第三旋转关节5通过所述第二旋转关节4驱动转动，第三旋转关节5包括支撑架501和驱动单元，且所述支撑架501通过所述驱动单元驱动转动，第四旋转关节6和末端操作器7均安装于所述支撑架501上，且末端操作器7通过第四旋转关节6驱动转动。所述升降关节1、第一旋转关节2、伸缩关节3、第二旋转关节4、第三旋转关节5和第四旋转关节6依次串联形成六自由度机械臂，并通过第四旋转关节6输出端连接末端操作器7，将所有关节的合成运动传递给末端操作器7，并通过运动学分析与控制算法相结合，实现末端操作器7端点的远心运动。

如图3所示，所述底座8包括座体806、脚轮801和支撑柱805，升降关节1垂直安装于所述座体806上，所述座体806下侧设有底座承重板802，四个脚轮801分别固定于所述底座承重板802的四个角端，支撑柱805设于座体806上侧，在所述支撑柱805顶端设有推手803和显示屏804，使用者通过手扶推手803，利用四个脚轮801的滚动特性，可将设备整体进行空间位置的移动，方便搬运与使用，为使用者节约时间和节省体力。

如图4所示，所述升降关节1包括电动推缸101和呈柱状的第一关节支撑件102，其中电动推缸101的底端安装在所述座体806下侧的底座承重板802上，电动推缸101的输出端与第一关节支撑件102的一端相连，所述第一关节支撑件102通过所述电动推缸101驱动升降，且第一旋转关节2安装于所述第一关节支撑件102上，另外如图5所示，所述底座8上侧设有第一关节外壳103将电动推缸101包裹，所述第一关节外壳103上端与一个第二关节外壳201相连，且所述第二关节外壳201将所述第一关节支撑件102包裹，所述第一关节外壳103和第二关节外壳201横截面可以为方形或圆形。

如图4～5所示，所述第一旋转关节2包括第二关节电机202、齿轮一204、齿轮二208、交叉滚子轴环206和第二关节连接件207，其中齿轮一204安装于第二关节电机202的输出端，齿轮二208与齿轮一204啮合，且如图5所示，齿轮二208与交叉滚子轴环206内圈一侧同轴连接，交叉滚子轴环206内圈的另一侧与第二关节连接件207同轴连接，伸缩关节3固装于所述第二关节连接件207上。机构工作时，第二关节电机202驱动齿轮一204转动，齿轮一204带动齿轮二208转动，齿轮二208通过交叉滚子轴环206驱动第二关节连接件207转动，进而驱动伸缩关节3转动。所述交叉滚子轴环206为本领域公知技术且为市购产品。

如图4～5所示，所述第一关节支撑件102上端固设有第二关节电机安装板205，第二关节电机202设于第一关节支撑件102上端内的一侧且与所述第二关节电机安装板205固连，并且所述第二关节电机202的输出轴通过一个第二关节电机连接件203与齿轮一204相连，齿轮二208可转动地设于所述第二关节电机安装板205中部，并通过所述交叉滚子轴环206与第二关节连接件207连接。

如图6～7所示，所述伸缩关节3包括第三关节支撑板301、伸缩机构302和呈板状的第三关节连接件303，其中第三关节支撑板301中部与所述第二关节连接件207连接，伸缩机构302安装于第三关节支撑板301上，所述伸缩机构302设有可移动的滑块3024，且所述第三关节连接件303与所述滑块3024固连，第二旋转关节4安装于所述第三关节连接件303端部。

如图7所示，所述伸缩机构302包括电机模组3021、安装座3023、滑块3024和丝杠3025，其中安装座3023固装于所述第三关节支撑板301上，电机模组3021和丝杠3025均设于所述安装座3023上，且所述丝杠3025通过电机模组3021驱动转动，滑块3024与所述安装座3023滑动连接，在所述安装座3023上设有与所述滑块3024配合的滑轨，所述滑块3024内部设有丝母套装于所述丝杠3025上。机构工作时，电机模组3021驱动丝杠3025转动，丝杠3025驱动滑块3024移动，进而带动所述第三关节连接件303直线移动。

如图7所示，所述电机模组3021的输出轴通过一个联轴器3022与所述丝杠3025的一端连接，所述安装座3023两侧设有与所述滑块3024配合的滑轨，中间设有所述丝杠3025，所述安装座3023一侧设有限位开关安装板3026，且所述限位开关安装板3026两端分别设有限位开关3027用于实现滑块3024的零位寻找以及电气限位，所述限位开关3027为本领域公知技术且为市购产品。

如图6所示，所述第三关节支撑板301上设有第三关节外壳304将所述伸缩结构302包裹。

如图6和图8所示，所述第二旋转关节4包括端关节401和呈柱状的第四关节连接件402，其中端关节401固装于所述第三关节连接件303端部，所述端关节401内设有第四关节电机，且所述第四关节连接件402一端通过所述第四关节电机驱动转动，另一端与所述第三旋转关节5固连。

如图8～13所示，所述第三旋转关节5包括支撑架501和驱动单元，其中所述驱动单元包括第五关节电机509、第五关节轴511和第五关节交叉滚子轴环510，其中如图11所示，所述第四关节连接件402端部插入所述支撑架501中部，且所述第四关节连接件402端部一侧设有第五关节电机509，另一侧设有第五关节轴511，且第五关节电机509和第五关节轴511同轴设置，其中第五关节电机509固设于第四关节连接件402上且输出端与支撑架501中部位置的一侧连接，第五关节轴511一端通过第五关节交叉滚子轴环510安装于第四关节连接件402上，第五关节轴511另一端与支撑架501中部位置的另一侧连接并通过挡圈512限位固定。机构工作时，所述第五关节电机509驱动所述支撑架501转动。所述第五关节交叉滚子轴环510为本领域公知技术且为市购产品。

如图13所示，所述支撑架501整体呈C型且为两侧薄板结构，两侧薄板之间空隙便于折叠过程中将支撑架501及末端操作器7收拢在第四关节连接件402周围，并且所述两侧薄板上端和下端分别对应汇聚到一起，可用于固定末端操作器7。如图1～2所示，当处于工作状态时，支撑架501处于展开状态，而其非工作状态下通过其特殊结构形式处于收拢状态，减少了设备整体的占用空间。

如图8～9和图12所示，所述支撑架501中部与第五关节电机509同轴位置的A处设有力传感器B502和把手一503，所述支撑架501与第四旋转关节6连接一端的B处位置设有力传感器A507和把手二508，所述力传感器B502和力传感器A507均为多维力传感器，此为本领域公知技术且为市购产品。另外如图9和图12所示，所述支撑架501与第四旋转关节6连接一端还设有使能按钮504、按钮A505和按钮B506。

本发明包括拖动模式与单轴运动模式两种工作状态，其中拖动模式还包括远心定位拖动与三维位置拖动两种工作状态，如图14所示，机械臂的远心定位拖动方案主要是通过力传感器A507检测把手二508上的拖动力变化情况，例如检测到拖动力沿着图中俯仰方向时，通过控制算法求解出各个运动关节转动量实现该方向的远心拖动功能，其余方向的拖动实现方式与此相同，进而实现远心运动(俯仰、偏转、远心移动)所需要的三个方向的拖动功能。而三维位置拖动主要是通过力传感器B502检测把手一503上拖动力的变化情况，当检测到把手一503的拖动力沿着X_O-Y_O-Z_O中的一个时，通过控制算法求解出升降关节1、第一旋转关节2、伸缩关节3的转动量，进而实现末端操作器7的三维位置拖动，在此过程中末端操作器7相对于把手一503不运动，从而实现末端操作器7在三维空间中的位置拖动定位。

另外本发明还可以通过力传感器A507检测把手二508实现单轴运动功能，其实现过程通过按下使能按钮504，然后通过力传感器A507(其为多维力传感器)检测把手二508上拖动力的变化，实现第二旋转关节4、第三旋转关节5和第四旋转关节6的单轴运动。另外由于远心定位拖动与单轴运动均使用了把手二508与力传感器A507，所以本发明通过按钮A505和按钮B506单独控制第四旋转关节6的正转与反转，第四旋转关节6通过两个按钮实现单轴运动，主要为了实现在远心运动过程中，末端操作器7仍然具有自转的功能。所述使能按钮504、按钮A505和按钮B506的具体控制原理为本领域公知技术。

在力拖动方案与单轴运动过程中，使用者还可以通过拖拽把手一503和把手二508，实现其运动功能，更方便且更有效。

如图12所示，所述第四旋转关节6包括第六关节电机604和第六关节连接板601，第六关节电机604固定在支撑架501上，第六关节连接板601通过第六关节电机604驱动转动，且第六关节连接板601与末端操作器7连接。

如图12所示，所述第四旋转关节6还包括第六关节交叉滚子轴环602、第六关节电机转接轴603和末端固定盘605，其中末端固定盘605和第六关节电机604都固定在支撑架501上，且第六关节电机604输出轴与所述第六关节电机转接轴603连接，所述第六关节电机转接轴603与第六关节交叉滚子轴环602一侧内圈连接，第六关节交叉滚子轴环602另一侧内圈与第六关节连接板601连接，而第六关节交叉滚子轴环602外圈固定在末端固定盘605上。所述第六关节交叉滚子轴环602为本领域公知技术且为市购产品。

本发明机械臂整体的走线形式采用内部走线，电源线与信号线通过中空关节及内部走线孔连接到底座部分，再与外部电源连接，减少机械臂运动过程中线缆的缠绕问题，同时也增强美观性。

本发明的工作原理为：

本发明工作时，第一旋转关节2通过所述升降关节1驱动升降，伸缩关节3通过所述第一旋转关节2驱动转动，第二旋转关节4通过所述伸缩关节3伸缩驱动移动，第三旋转关节5通过所述第二旋转关节4驱动转动，第三旋转关节5包括支撑架501和驱动单元，且所述支撑架501通过所述驱动单元驱动转动，第四旋转关节6和末端操作器7均安装于所述支撑架501上，且末端操作器7通过第四旋转关节驱动转动。所述升降关节1、第一旋转关节2、伸缩关节3、第二旋转关节4、第三旋转关节5和第四旋转关节6依次串联形成六自由度机械臂，并通过第四旋转关节6输出端连接末端操作器7，将所有关节的合成运动传递给末端操作器7，并通过运动学分析与控制算法相结合，实现末端操作器7端点的远心运动。

另外本发明控制方法如下：

一、关于机器人正运动学及逆运动学控制方法，本发明对于机械臂的运动控制可以建立如图15所示的机器人运动坐标系，建立机器人D-H参数表如下：

机器人D-H参数表

<u>i</u>	<u>a<sub>i-1</sub></u>	<u>α<sub>i-1</sub></u>	<u>d<sub>i</sub></u>	<u>θ<sub>i</sub></u>
					<u>1</u>	<u>0</u>	<u>0</u>	<u>a<sub>0</sub>+d<sub>1</sub></u>	<u>0</u>
<u>2</u>	<u>0</u>	<u>0</u>	<u>0</u>	<u>θ<sub>2</sub></u>
					<u>3</u>	<u>0</u>	<u>90°</u>	<u>a<sub>3</sub>+d<sub>3</sub></u>	<u>0</u>
<u>4</u>	<u>0</u>	<u>90°</u>	<u>a<sub>4</sub></u>	<u>θ<sub>4</sub></u>
					<u>5</u>	<u>0</u>	<u>90°</u>	<u>0</u>	<u>θ<sub>5</sub></u>
<u>6</u>	<u>a<sub>6</sub></u>	<u>-90°</u>	<u>a<sub>5</sub></u>	<u>θ<sub>6</sub></u>

采用机器人常规D-H参数法即可得到机器人的正运动学表达式。

基于正运动学的表达式，当我们给定一个机器人的末端位姿阵：

可以求解机器人的各个关节运动量为：

θ₅＝arccos(-t₃₃)＝π-arccos(t₃₃)；

d₁＝t₃₄-a₀+a₄+a₅*cθ₅+a₆*sθ₅；

θ₄＝θ₂-arctan(t₂₃/t₁₃)。

上述求解过程为常规数学运算，可通过现有的数学控制软件实现。

二、关于机器人位置运动控制方法，在位置控制模式下，末端操作器7在三维空间进行位置拖动定位，此时末端操作器7相对于把手一503不运动，从而可通过把手一503实现末端操作器7在三维空间中的位置拖动定位。

将力传感器B502上检测到的在把手一503上施加的拖动力

映射为把手一503所在位置需要的三维位置移动速度

即:

上式为本领域公知技术，可参见机器人学或其他相关文献。

设机器臂位置运动量为Δs＝[Δs_x，Δs_y，Δs_z],若机器人控制周期为Δt，，则：

机械臂的初始位姿为，

若机器人末端位置点从初始位置T_c在拖动控制下经过一个控制周期运动到T_e,则经过拖动控制后的位姿矩阵为：

基于逆运动学求解算法可以得出各个关节的运动量，进而实现对机器人的位置拖动控制。

上述过程可通过现有的数学控制软件实现。

三、关于本发明机器人远心运动控制方法，在远心运动控制模式下，也即使能按钮504关闭，如图16所示，末端操作器7将绕指定点进行远心运动，机械臂的末端位置点(即机械臂与末端操作器7连接位置)将绕指定点做半径r的球面运动，其中r为机械臂末端位置点到指定远心点的距离，θ决定了在球面上的具体位置。

力传感器A507检测把手二508获得给定的拖动力：

需要控制末端操作器7将沿作用力方向上绕指定点做圆弧运动，圆弧半径为r，圆弧运动的角速度

大小与作用力大小成比例，即：

若机器人控制周期为Δt，则需要运动的圆弧角度α大小为：

如图16所示，若机器人末端位置点从初始位置P_c＝[x_c，y_c，z_c]^T在拖动控制下经过一个控制周期运动到P_e＝[x_e，y_e，z_e]^T，则根据前面分析可知，

与

的夹角为α，且

与

三线共面，令

为三线共面的法向矢量,则可以得到：

上式(1)中：

由此可求得

设机器人初始在P_c点时末端位姿矩阵为：

经过拖动控制运动后到达P_e点，此时末端位姿矩阵为：

由P_c到P_e的运动过程可以等效为

绕

旋转α角度后得到，则其姿态变化关系为：

上式(2)中，

表示绕轴

旋转α角度的旋转变换矩阵，其具体表达式可以从机器人学及其他公开文献中获得。

通过式(1)和(2)最终可以得到远心拖动控制时机器人需要运动控制的目标位姿矩阵T_e，再通过机器人逆运动学即可实现远心拖动控制。

上述过程可通过现有的数学控制软件实现。

四、使能按钮504按下开启，上述远心拖动控制模式切换为单轴运动控制模式。

在此模式下，将力传感器A507检测把手二508获得拖动力F_A直接映射为伸缩关节3、第二旋转关节4和第三旋转关节5的单轴运动：

上式为本领域公知技术，可参加机器人学等相关文献获得。

在此模式下，本发明可通过按钮A505、按钮B506单独控制第四旋转关节6的正转与反转，实现机器人的单轴运动控制。

上述过程可通过现有的数学控制软件实现。

Claims (9)

1.一种悬挂定位机械臂，其特征在于：包括底座(8)、升降关节(1)、第一旋转关节(2)、伸缩关节(3)、第二旋转关节(4)、第三旋转关节(5)、第四旋转关节(6)和末端操作器(7)，升降关节(1)安装于底座(8)上，第一旋转关节(2)通过升降关节(1)驱动升降，伸缩关节(3)通过第一旋转关节(2)驱动转动，第二旋转关节(4)通过伸缩关节(3)伸缩驱动移动，第三旋转关节(5)通过第二旋转关节(4)驱动转动，第三旋转关节(5)包括支撑架(501)和驱动单元，且所述支撑架(501)通过所述驱动单元驱动转动，第四旋转关节(6)和末端操作器(7)设于支撑架(501)上，且末端操作器(7)通过第四旋转关节(6)驱动转动，所述支撑架(501)中部设有把手一(503)和力传感器B(502)，所述支撑架(501)与第四旋转关节(6)连接一端设有把手二(508)和力传感器A(507)，所述支撑架(501)上设有使能按钮(504)、按钮A(505)和按钮B(506)；

所述第二旋转关节(4)包括端关节(401)和第四关节连接件(402)，其中端关节(401)固装于伸缩关节(3)自由移动端部，所述端关节(401)内设有第四关节电机，且所述第四关节连接件(402)通过所述第四关节电机驱动转动，所述第三旋转关节(5)的驱动单元包括第五关节电机(509)、第五关节轴(511)和第五关节交叉滚子轴环(510)，且所述第四关节连接件(402)一端插入所述支撑架(501)中，并且在所述第四关节连接件(402)端部一侧设有第五关节电机(509)、另一侧设有第五关节轴(511)，其中第五关节电机(509)输出端与支撑架(501)中部位置的一侧连接，第五关节轴(511)一端通过第五关节交叉滚子轴环(510)与第四关节连接件(402)相连、另一端与支撑架(501)中部位置的另一侧连接。

2.根据权利要求1所述的悬挂定位机械臂，其特征在于：所述升降关节(1)包括电动推缸(101)和第一关节支撑件(102)，电动推缸(101)安装在所述底座(8)上，且第一关节支撑件(102)通过所述电动推缸(101)驱动升降，所述第一旋转关节(2)包括第二关节电机(202)、齿轮一(204)、齿轮二(208)、交叉滚子轴环(206)和第二关节连接件(207)，其中第二关节电机(202)设于第一关节支撑件(102)上端内的一侧，齿轮一(204)安装于第二关节电机(202) 的输出端，齿轮二(208)可转动地设于第一关节支撑件(102)上端中部，且所述齿轮二(208)与齿轮一(204)啮合，所述齿轮二(208)与交叉滚子轴环(206)内圈一侧同轴连接，交叉滚子轴环(206)内圈的另一侧与第二关节连接件(207)同轴连接，伸缩关节(3)固装于所述第二关节连接件(207)上。

3.根据权利要求2所述的悬挂定位机械臂，其特征在于：所述底座(8)上侧设有包裹所述电动推缸(101)的第一关节外壳(103)，所述第一关节外壳(103)上端与一个包裹所述第一关节支撑件(102)的第二关节外壳(201)相连，所述第一关节外壳(103)和第二关节外壳(201)横截面可以为方形或圆形。

4.根据权利要求1所述的悬挂定位机械臂，其特征在于：所述伸缩关节(3)包括第三关节支撑板(301)、伸缩机构(302)和第三关节连接件(303)，其中第三关节支撑板(301)与第一旋转关节(2)连接，伸缩机构(302)安装于第三关节支撑板(301)上，所述伸缩机构(302)包括电机模组(3021)、丝杠(3025)和滑块(3024)，且所述丝杠(3025)通过电机模组(3021)驱动转动，所述滑块(3024)内部设有丝母套装于所述丝杠(3025)上，所述第三关节连接件(303)与所述滑块(3024)固连，第二旋转关节(4)安装于所述第三关节连接件(303)端部。

5.根据权利要求1所述的悬挂定位机械臂，其特征在于：所述支撑架(501)整体呈C型且为两侧薄板结构，并且两侧薄板上端和下端分别对应汇聚到一起，末端操作器(7)两端分别与支撑架(501)两端相连，第四旋转关节(6)设于支撑架(501)一端。

6.根据权利要求1所述的悬挂定位机械臂，其特征在于：所述底座(8)下侧设有脚轮(801)、上侧设有支撑柱(805)，且所述支撑柱(805)上设有推手(803)和显示屏(804)。

7.一种根据权利要求1所述的悬挂定位机械臂的控制方法，其特征在于：机械臂处于三维位置拖动控制模式时，将力传感器B(502)检测到的在把手一(503)上施加的拖动力

映射为把手一(503)所在位置的三维位置移动速度

设机器臂位置运动量为Δs＝[Δs_x，Δs_y，Δs_z],若机器人控制周期为Δt，，则：

机械臂的初始位姿为：

若机器人末端位置点从初始位置T_c在拖动控制下经过一个控制周期运动到T_e,则经过拖动控制后的位姿矩阵为：

然后基于逆运动学求解算得出各个关节的运动量并实现拖动控制。

8.一种根据权利要求1所述的悬挂定位机械臂的控制方法，其特征在于：使能按钮(504)关闭，机械臂处于远心运动控制模式，此时机械臂的末端位置点(机械臂与末端操作器(7)连接位置)绕指定点做半径r的球面运动，力传感器A(507)检测把手二(508)获得给定的拖动力：

机械臂的末端位置点通过上述拖动力 绕指定点做圆弧运动，且圆弧运动的角速度

大小与作用力大小成比例，即：

若机器人控制周期为Δt，则机械臂的末端位置点运动的圆弧角度α大小为：

若机器人末端位置点从初始位置P_c＝[x_c，y_c，z_c]^T在拖动控制下经过一个控制周期运动到P_e＝[x_e，y_e，z_e]^T，则

与

的夹角为α，且

与

三线共面，令

为三线共面的法向矢量,则：

上式(1)中：

由此求得

设机器臂初始在P_c点时末端位姿矩阵为：

经过拖动控制运动后到达P_e点，此时末端位姿矩阵为：

由P_c到P_e的运动过程等效为

绕

旋转α角度后得到，则其姿态变化关系为：

上式(2)中，

表示绕轴

旋转α角度的旋转变换矩阵，通过式(1)和(2)获得机器臂需要运动控制的目标位姿矩阵T_e，再基于逆运动学求解算得出各个关节的运动量实现远心拖动控制。

9.根据权利要求8所述的悬挂定位机械臂的控制方法，其特征在于：使能按钮(504)开启，机械臂切换为单轴运动控制模式，将力传感器A(507)检测把手二(508)获得拖动力F_A直接映射为伸缩关节(3)、第二旋转关节(4)、第三旋转关节(5)的单轴运动，其映射关系为：

且第四旋转关节(6)通过按钮A(505)、按钮B(506)单独控制的正转与反转。

Images