本发明属于外骨骼机器人的技术领域，具体涉及一种具有动态负载补偿的上肢外骨骼协同控制方法。
背景技术：
机器人外骨骼系统是一种人机协作系统，可增强佩戴者在各种环境中的力量。人类操作者仅负责机器人的位置控制，力控制和运动信号生成。该系统是机械结构的形式，其与人体的外部结合以改善佩戴者的肌肉力量。目前正在研究各种行业的应用，包括工业应用，医疗康复等领域。
由于现有的上肢外骨骼控制器直接通过人机间的相互作用力来进行直接力控制，针对负载重量过大、人机之间作用力随负载重量增加而增加等问题存在着严重的不足。
技术实现要素：
有鉴于此，本发明提供了一种具有动态负载补偿的上肢外骨骼协同控制方法，能够实时补偿负载导致的人机作用力，使穿戴者忽略负载重量以相同的力对不同重量的负载进行操作，实现了人机作用力最小化，达到人机协同的控制目的，为全身外骨骼的平衡控制奠定基础。
实现本发明的技术方案如下：
一种具有动态负载补偿的上肢外骨骼协同控制方法，利用负载对外骨骼的广义力和其对应的雅各比矩阵得到补偿负载影响所需的关节力矩τload，通过逆动力学得到上肢外骨骼自重引起的关节力矩τg，利用人与上肢外骨骼间的相互作用力和其对应的雅各比矩阵得到人机作用力映射到关节空间的力矩τh，利用最终关节力矩τ对上肢外骨骼各关节进行控制，其中，τ＝τg+τload+τh，然后对人与外骨骼之间的相互作用力fhr进行测量，当fhr不为0时，通过直接力控制器和pid控制器对τ进行反馈补偿。
进一步地，上肢外骨骼对负载的承载方式为固定承载。
进一步地，负载对上肢外骨骼的广义力通过六维力/力矩传感器测得。
进一步地，分别在上肢的握把处、上肢外骨骼的肘关节与末端执行器之间设置两个六维力/力矩传感器。
进一步地，各关节的角度信息通过设置在各关节处的角度传感器测得。
进一步地，
其中，jl是六维力/力矩传感器坐标系中心位置的雅可比矩阵，flr是负载对外骨骼的广义力，flr＝[flxflyflzmlxmlymlz]t，它们分别表示负载自身重量产生的力flr＝[flxflyflz]t和力矩tlr＝[mlxmlymlz]t；此处的力flr包含负载重力和惯性力，力矩tlr包含负载自重产生的力矩和惯性力矩。
有益效果：
1、本发明提供了一种具有动态负载补偿的上肢外骨骼协同控制方法，该方法通过实时补偿动态负载对上肢外骨骼的影响，使穿戴者能够忽略负载重量以相同的力对不同重量的负载进行操作，实现了人机作用力最小化，达到人机协同的控制目的。
2、本发明不局限于单一的承载方式，负载补偿器可对上肢外骨骼以固定和活动两种承载方式连接的负载进行补偿，因此，适应范围更加广泛。
3、本发明通过补偿器对负载产生的影响进行了实时补偿，并且在实际控制中设置了一个放大系数kf，因此控制时人机作用力极小。而由于一般的搬举过程中都是低速、平缓动作，此时的人机作用力近似等于0，在实际中常常将人机作用力引起的关节力矩忽略掉。
附图说明
图1为本发明六维力/力矩传感器的安装示意图。
图2(a)为活动承载方式示意图，图2(b)为固定承载方式示意图。
图3为本发明方法示意图。
图4为无补偿的直接力系统控制框图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明提供了一种具有动态负载补偿的上肢外骨骼协同控制方法，具体包括以下步骤：
步骤1：在上肢各关节和指定位置之间分别安装角度传感器和六维力/力矩传感器，用于测量各关节的角度和三维空间的广义力。
具体安装方式如图1所示：分别在上肢的握把处、上肢外骨骼的肘关节与末端执行器之间安装两个六维力/力矩传感器，在上肢肩关节、肘关节安装若干角度传感器(具体数量根据上肢结构的自由度来确定)。
步骤2：操作者穿戴上肢外骨骼进行搬举测试，实时记录测试过程中六维力/力矩传感器与各个角度传感器采集的各项数据。其中，外骨骼对负载的承载方式为固定承载方式，如图2(a)和(b)所示，与活动承载方式相比，固定承载方式更具一般性和普遍性，如表1所示，负载的重心位置和体积变化对活动承载方式没有影响，但对固定承载方式有较大影响。
需要说明的是，外骨骼与末端执行器之间的六维力/力矩传感器可以测量力和力矩，而活动承载方式可以看作是负载自重产生的力矩和惯性力矩均为0时的特殊固定承载方式，因此提出的负载补偿器可对上肢外骨骼以固定和活动两种承载方式连接的负载进行补偿。
表1活动和固定承载方式对比
其中，负载与外骨骼之间的六维力/力矩传感器用于测量负载对外骨骼的作用力flr(物机作用力)，flr为一个6×1的广义力矢量。
flr＝flxflyflzmlxmlymlz]t，它们分别表示负载对外骨骼末端执行器产生的三维空间的全力信息，即三个力分量和三个力矩分量。
握把处的六维力/力矩传感器用于测量人机之间的作用力fhr(人机作用力)，fhr为一个6×1的广义力矢量。
fhr＝[fhxfhyfhzmhxmhymhz]t它们分别表示人推动握把时产生的三维空间的全力信息，即三个力分量和三个力矩分量。
步骤3：根据关节传感器测得的关节角度，使用拉格朗日方程法对上肢外骨骼进行动力学建模，计算上肢外骨骼自重引起的关节力矩。
方程式中忽略摩擦力及空气阻力等因素，h(q)是惯性矩阵，
步骤4：根据上肢外骨骼的肘关节与末端执行器之间的六维力/力矩传感器测量的广义力数据及各关节的角度数据，通过雅各比矩阵可以计算补偿负载影响所需的关节力矩。
其中，jl是六维力/力矩传感器坐标系中心位置的雅可比矩，此雅各比矩阵可以直接通过关节角度信息及已知的杆长等信息推导。负载与外骨骼之间的作用力flr是一个广义力，flr＝[flxflyflzmlxmlymlz]t，它们分别表示负载自身重量产生的力flr＝[flxflyflz]和力矩tlr＝[mlxmlymlz]。此时测量的力包含负载重力和惯性力，而测量的力矩包含负载自重产生的力矩和惯性力矩。
等式左边τload表示因为补偿负载影响所需的关节力矩。这一部分即为本文中提到的负载补偿部分，用于补偿负载重量对上肢外骨骼的影响，使得后期进行直接力控制时自动补偿关节力矩，这样便可以使得上肢外骨骼在搬举重物时可以忽视负载的重量，以一个相同的力去举起不同重量的重物。
步骤5：根据握把处的六维力/力矩传感器测量的广义力数据及各关节的角度数据，通过雅各比矩阵可以计算人机作用力映射到关节空间的力矩。
τh＝jhtfhr
τh表示人机作用力映射到关节空间的力矩。
jh是握把处六维力/力矩传感器坐标系中心位置的雅可比矩阵。此雅各比矩阵可以直接通过关节角度及已知的杆长等信息推导。
fhr表示人机之间的相互作用力，即人推动握把时产生的三维空间的广义力，它包含负载导致的人机作用力和改变上肢外骨骼运动状态的人机作用力。一般直接力控制器中会加入一个放大系数kf，因此人机作用力会比它们小，但是若放大系数过大，则会导致控制太过于灵敏，鲁棒性不好，而且实际控制中会出现剧烈抖动等现象。当没有负载补偿时，人机作用力fhr会随着负载重量的增加而增大；当有负载补偿时，补偿器对负载导致的人机作用力进行实时补偿，此时人机作用力fhr仅仅用于改变上肢外骨骼的运动状态，因此人可以以相同的力去操作外骨骼搬举不同重量的重物，实现人机作用力最小，达到人体协同控制的目的。而且由于搬举过程中大部分都是低速、平缓的动作，此时的人机作用力极小，近似等于0，因此使用负载补偿时通常可以将人机作用力引起的关节力矩忽略掉。
步骤6：具体的系统控制框图如图3所示。根据系统控制框图可以直接制定相关的控制控制策略：
fd是控制目标量。为了使操作者可以用最小的作用力操作外骨骼运动，将fd设置为0，很好地跟踪穿戴者的运动。
τh是人机作用力fhr映射到关节空间的力矩，jh是六维力/力矩传感器坐标系中心位置的雅可比矩阵。
δf＝fd-fhr
τhy是经过力控制器计算得到的需要动力系统提供的关节力矩。khy是直接力控制器的放大倍数，该系数可由操作者改变。
τhy＝khyτh
其中，kp，ti，td分别是力控制器的比例参数、积分参数和微分参数。
外骨骼的关节角度信息θ是一个广义变量，包括θ、
τ＝τp+τg+τload+τh
最终上肢外骨骼关节力矩的表达式为：
与常规的直接力系统控制框图(如图4)相比，本发明的控制方法，增加了对负载引起的关节力矩的补偿，使穿戴者忽略负载重量以相同的人机作用力对不同重量的负载进行操作，实现了人机作用力最小化。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。