机器人三维空间重力平衡补偿装置及方法
【技术领域】
[0001] 本发明涉及一种机器人重力补偿装置及方法，具体涉及一种机器人三维空间重力 平衡补偿装置及方法。
【背景技术】
[0002] 重力补偿装置广泛应用于工业医疗等领域，例如在工业机器人的机械手中，往往 需要将其自身重力平衡以达到更精确的控制目标；在医疗康复等领域中，重力平衡更能带 给复健病人以福音，重力平衡装置可以减少甚至省去电驱动的环节，增加了机构的可靠性， 使得使用安全性得到了极大的保障。现有的重力补偿装置大多局限于二维空间，而现有的 三维空间补偿装置的结构是利用差动机构或者添加质量块等方式实现，这种补偿方式结构 复杂或者笨重而失去实用性。
【发明内容】
[0003] 本发明为解决现有三维空间补偿装置结构复杂的问题，而提出一种机器人三维空 间重力平衡补偿装置及方法。
[0004] 装置：本发明的机器人三维空间重力补偿平衡装置包括第一平衡杆、第二平衡杆、 固定套、大臂杆、小臂杆、肘关节、第一肩关节、第二肩关节、第三肩关节、连接头、第一转动 关节、第二转动关节、第三转动关节、第一滑轮、第一弹簧、第二滑轮、第二弹簧、平衡杆钢丝 绳、小臂钢丝绳和固定架，第一平衡杆的一端与第一转动关节固定连接，第一转动关节与第 二转动关节铰接，第二转动关节与第三转动关节铰接，第一平衡杆的另一端与连接头固定 连接，第二平衡杆的一端与连接头铰接，第二平衡杆的另一端与固定套铰接，大臂杆的一端 穿过固定套与肘关节固定连接，固定套绕大臂杆转动，大臂杆的另一端与第一肩关节通过 轴承连接，第一肩关节与第二肩关节铰接，第二肩关节与第三肩关节铰接，小臂杆的一端与 肘关节铰接，第一滑轮和第一弹簧均固定在第一平衡杆上，且第一滑轮位于第一转动关节 的一侧，第一弹簧位于连接头的一侧，第二滑轮和第二弹簧均固定在小臂杆上，且第二滑轮 位于肘关节的一侧，第二弹簧位于小臂杆的外侧，第三肩关节和第三转动关节均与固定架 固定连接，平衡杆钢丝绳的一端与固定架固定连接，平衡杆钢丝绳的另一端绕过第一滑轮 与第一弹簧固定连接，小臂钢丝绳的一端与第二平衡杆固定连接，小臂钢丝绳的另一端绕 过第二滑轮与第二弹簧固定连接。
[0005] 方法一：所述方法是实现平面重力平衡补偿的方法，其步骤如下：
[0006] 步骤一：计算大臂杆的重力势能W1:
[0007] 公式一 =W1= m 汨（1' 心+h)
[0008] 其中，Hi1为大臂杆的质量，g为重力加速度，Γ 1为大臂杆的质量中心点至第一肩 关节与第二肩关节铰接点的长度，C1Scos Θ i，Q1为平行四边形的外侧大臂杆与固定架之 间的锐角，h为第一肩关节的铰接点至第一转动关节的铰接点之间的距离；
[0009] 步骤二：计算小臂杆的重力势能W2:
[0010] 公式二：W2= m Sgd1C^l ' 2c1+2+h) =m2g(l1c1+l/2(^2-1' pA+h)
[0011] 其中，m2为小臂杆的质量，I1为大臂杆的长度，I' 2为小臂杆的质量中心点至肘 关节铰接点的长度，c1+2为cos( Θ ^ Θ 2)，(：2为cos Θ 2, 4为sin Θ ^ &为sin Θ 2，Θ 2为小 臂杆的外侧小臂杆与大臂杆之间的锐角；
[0012] 步骤三：计算第一平衡杆的重力势能W3:
[0013] 公式三：W3= m 3gl' W1
[0014] 其中，!113为第一平衡杆的质量，1' 3为第一平衡杆的质量中心点至第一转动关节 铰接点的长度；
[0015] 步骤四：计算第二平衡杆的重力势能W4:
[0016] 公式四：W4= m IgG1C1+]/ 4)
[0017] 其中，m4为第二平衡杆的质量，1' 4为第二平衡杆的质量中心点至连接头铰接点 的长度；
[0018] 步骤五：计算大臂杆、小臂杆、第一平衡杆和第二平衡杆的总势能Vg:
[0019] 公式五：Vg= W !+WjWfW4= m igh+n^gh+n^gl ' 4+(ηι^1 ' 1+m2gl1+m3gl ' 3+m4gl1) Ci+m2gl' ' P1S2
[0020] 步骤六：计算第一弹簧的伸长量X1:
[0021] 公式六：xf = h2 + df + Zhd1C1
[0022] 其中，X12为第一弹簧的伸长量的平方，d i为平衡杆钢丝绳的长度，d /为平衡杆钢 丝绳长度的平方；
[0023] 步骤七：计算第二弹簧的伸长量X2:
[0024] 公式七：x| = h2 + df + 2hd2c1+2
[0025] 其中，X22为第二弹簧的伸长量的平方，d2为小臂钢丝绳的长度，d 22为小臂钢丝绳 长度平方；
[0026] 步骤八：计算第一弹簧和第二弹簧的弹性势能和Vs:
[0027] 公式八：Vs = |kA2 + |k2x!= ^k1Oi2 + df) + 吾k2(h2 + df) + Ic1Iid1C1 + Ic2Iid2C1C2 _ It2Iid2S1S2
[0028] 其中，kl为第一弹簧的劲度系数，k2为第二弹簧的劲度系数；
[0029] 步骤九：为使Vs+Vg =常量，对公式八消除系数，由于重力势能结算结果为负值，故 有：
[0030] 公式九：πι$Γ i+n^gli+n^gr fn^gli= k ^d1 Vs和 V g对应项系数相消
[0031] 公式十：m2gr 2= k 2hd2 Vs和V g对应项系数相消
[0032] 步骤十：当系统关系满足以上公式九和公式十时，系统达到平面平衡；
[0033] 步骤十一：根据实际物体质量m，调节第一肩关节的铰接点至第一转动关节的铰 接点之间的距离h，即可实现平衡补偿。
[0034] 方法二：所述方法是实现空间重力平衡补偿方法，其步骤如下：
[0035] 步骤一：计算大臂杆的重力势能W1:
[0036] 公式一' A+h)
[0037] 其中，其中，Hi1为大臂杆的质量，g为重力加速度，Γ 1为大臂杆的质量中心点至 第一肩关节与第二肩关节铰接点的长度，C1Scos Θ i，Q1为平行四边形的外侧大臂杆与固 定架之间的锐角，h为第一肩关节的铰接点至第一转动关节的铰接点之间的距离；
[0038] 步骤二：计算小臂杆的重力势能W2:
[0039] 公式二' ：W2= m Wd1C^l ' JiC1C2-I^ JiC0S1S2+!!)
[0040] 其中，m2为小臂杆的质量，I i为大臂杆的长度，1' 2为小臂杆的质量中心点至肘关 节铰接点的长度，Cci为cos Θ ^ C2为cos Θ 17 4为sin Θ 17 &为sin Θ 2，Θ 2为小臂杆的外侧 小臂杆与大臂杆之间的锐角；Θ ^为大臂杆轴向自转角；
[0041] 步骤三：计算第一平衡杆的重力势能W3:
[0042] 公式三' ：W3=m3gl' W1
[0043] 其中，m3为第一平衡杆的质量，1' 3为第一平衡杆的质量中心点至第一转动关节 铰接点的长度；
[0044] 步骤四：计算第二平衡杆的重力势能W4:
[0045] 公式四' 4)
[0046] 其中，m4为第二平衡杆的质量，1' 4为第二平衡杆的质量中心点至连接头铰接点 的长度；
[0047] 步骤五：计算大臂杆、小臂杆、第一平衡杆和第二平衡杆的总势能Vg:
[0048] 公式五' ：Vg= W !+WfWfW4= m igh+n^gh+n^gr 4+(ηι^Γ 1+m2gl1+m3gr 3+m4gl1) Ci+m2gl' ' AS1S2
[0049] 步骤六：计算第一弹簧的伸长量x1:
[0050] 公式六' ：x| = h2 + df + Zhd1C1
[0051] 其中，X12为第一弹簧的伸长量的平方，Cl1为平衡杆钢丝绳的长度，Cl 12为平衡杆钢 丝绳长度的平方；
[0052] 步骤七：计算第二弹簧的伸长量X2:
[0053] 公式七，：X·二 h2 + di + 211(12(?? - C0S1S2)
[0054] 其中，X22为第二弹簧的伸长量的平方，d2为小臂钢丝绳的长度，d 22为小臂钢丝绳 长度平方；
[0055] 步骤八：计算第一弹簧和第二弹簧的弹性势能和Vs: 1 1
[0056] 公式八 ' ：Vs = Jk1X! + jk2xf = ^k1Ch2 + d|) + -k2(h2 + d|) + ^hd1C1 + k2hd2c1c2 - k2hd2c〇s1S2
[0057] 其中，kl为第一弹簧的劲度系数，k2为第二弹簧的劲度系数；
[0058] 步骤九：为使Vs+Vg =常量，对公式八消除系数，由于重力势能结算结果为负值，故 有：
[0059] 公式九'：πι$Γ i+n^gli+n^gr fn^gli= k Ad1 Vs和 V g对应项系数相消
[0060] 公式十'：m2gr 2= k 2hd2 Vs和V g对应项系数相消
[0061] 步骤十：当系统关系满足以上公式九'和公式十'时，系统达到空间平衡；
[0062] 步骤十一：根据实际物体质量m，调节第一肩关节的铰接点至第一转动关节的铰 接点之间的距离h，即可实现空间补偿。
[0063] 本发明与现有技术相比具有以下有益效果：
[0064] 一、本发明的装置为模仿手臂自由度的结构，由于固定套与大臂杆可以相对转动， 这样就不会受到大臂杆自转的影响，从而保证了在三维空间中平行四边形仍然存在。
[0065] 二、本发明的平行四边形结构和零位弹簧的引入，使得各个质点的位置解析式和 弹簧伸长量的解析式项类型相同，即可以进行抵销，通过计算即可获得质量与弹簧位置的 关系。本发明的方法有效解决了三维重力平