CN114434452B - 一种基于势能场的镜像机械臂控制方法及镜像机械臂设备 - Google Patents

一种基于势能场的镜像机械臂控制方法及镜像机械臂设备 Download PDF

Info

Publication number
CN114434452B
CN114434452B CN202111529951.4A CN202111529951A CN114434452B CN 114434452 B CN114434452 B CN 114434452B CN 202111529951 A CN202111529951 A CN 202111529951A CN 114434452 B CN114434452 B CN 114434452B
Authority
CN
China
Prior art keywords
potential energy
energy field
arm
force
track
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111529951.4A
Other languages
English (en)
Other versions
CN114434452A (zh
Inventor
张佳楫
冯光
左国坤
胡静艳
施长城
宋涛
徐佳琳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ningbo Institute of Material Technology and Engineering of CAS
Cixi Institute of Biomedical Engineering CIBE of CAS
Original Assignee
Ningbo Institute of Material Technology and Engineering of CAS
Cixi Institute of Biomedical Engineering CIBE of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ningbo Institute of Material Technology and Engineering of CAS, Cixi Institute of Biomedical Engineering CIBE of CAS filed Critical Ningbo Institute of Material Technology and Engineering of CAS
Priority to CN202111529951.4A priority Critical patent/CN114434452B/zh
Publication of CN114434452A publication Critical patent/CN114434452A/zh
Application granted granted Critical
Publication of CN114434452B publication Critical patent/CN114434452B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J11/00—Manipulators not otherwise provided for
    • A—HUMAN NECESSITIES
    • A63—SPORTS; GAMES; AMUSEMENTS
    • A63B—APPARATUS FOR PHYSICAL TRAINING, GYMNASTICS, SWIMMING, CLIMBING, OR FENCING; BALL GAMES; TRAINING EQUIPMENT
    • A63B23/00—Exercising apparatus specially adapted for particular parts of the body
    • A63B23/035—Exercising apparatus specially adapted for particular parts of the body for limbs, i.e. upper or lower limbs, e.g. simultaneously
    • A63B23/12—Exercising apparatus specially adapted for particular parts of the body for limbs, i.e. upper or lower limbs, e.g. simultaneously for upper limbs or related muscles, e.g. chest, upper back or shoulder muscles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • General Health & Medical Sciences (AREA)
  • Physical Education & Sports Medicine (AREA)
  • Rehabilitation Tools (AREA)

Abstract

本发明提供了一种基于势能场的镜像机械臂控制方法，应用于镜像机械臂设备，镜像机械臂设备包括主动臂、从动臂、设置在从动臂上的第一手柄和设置在主动臂上的第二手柄，基于势能场的镜像机械臂控制方法包括：获得对所述从动臂的第一运动数据，基于所述第一手柄的第一起始位置和所述第一运动数据建立第一势能场；将所述第一运动轨迹输入自适应控制器进行响应和跟踪，获得期望数据；基于所述第二手柄的第二起始位置和所述期望轨迹建立与所述第一势能场互为镜像的第二势能场；获得对所述主动臂的第二运动数据，基于所述第二运动数据与所述期望轨迹的轨迹差异，控制所述主动臂施加辅助力，保证机械臂始终处于安全位置。

Description

一种基于势能场的镜像机械臂控制方法及镜像机械臂设备
技术领域
本发明涉及机器人控制技术领域，具体而言，涉及一种基于势能场的镜像机械臂控制方法及镜像机械臂设备。
背景技术
脑卒中是导致人们死亡的第二大原因，其发病率和致死率较高，大多数患有脑卒中的患者在经过治疗后仍会伴有不同程度的残疾，偏瘫是脑卒中常见的后遗症，肢体偏瘫严重影响了患者的日常生活活动，其主要表现为患侧肌力减、肢体痉挛以及活动受限。患者存在运动障碍的原因是其中枢神经受到了损伤，在神经***受损后，通过改变剩余神经的活动以优化神经资源，可以恢复运动功能，基于此，上肢康复机器人应运而生，通过模拟康复师的康复手法，反复弯曲患侧臂肢体，但是此种镜像拉力器只能通过一侧的上肢输入力镜像控制另一侧上肢输入力，仅将镜像的力的复制作为其训练的目标，对输入力不加以控制，当一侧输入力过大时，另一侧的拉力器会输出一个较大的加速度或移动到一个较远的距离，容易拉伤使用者的上肢。
发明内容
本发明解决的问题是：如何控制机械臂输出纠偏力，使机械臂始终处于安全的位置。
为解决上述问题，本发明提供一种基于势能场的镜像机械臂控制方法，应用于镜像机械臂设备，所述镜像机械臂设备包括主动臂、从动臂、设置在所述从动臂上的第一手柄和设置在所述主动臂上的第二手柄，所述基于势能场的镜像机械臂控制方法包括：
获得对所述从动臂的第一运动数据，基于所述第一手柄的第一起始位置和所述第一运动数据建立第一势能场，其中，所述第一运动数据包括第一运动轨迹；将所述第一运动轨迹输入自适应控制器进行响应和跟踪，获得期望数据，其中，所述期望数据包括期望轨迹，所述期望轨迹为所述主动臂所需要到达的与所述从动臂互为镜像的位置；基于所述第二手柄的第二起始位置和所述期望轨迹建立与所述第一势能场互为镜像的第二势能场；获得对所述主动臂的第二运动数据，基于所述第二运动数据与所述期望轨迹的轨迹差异，控制所述主动臂施加辅助力，其中，所述第二运动数据包括第二运动轨迹，所述辅助力包括切向力和法向力，所述法向力由所述第二势能场提供。
相对于现有技术，本发明基于第一运动轨迹和第一运动数据建立第一势能场，再将第二势能场镜像建立在以第二手柄的第二初始位置为中心的主动臂的周围，可以使主动臂在偏离期望轨迹时，主动施加一个朝期望轨迹缓慢靠近的法向力，帮助主动臂的实际运动轨迹尽量与期望轨迹拟合，更好地进行辅助训练，也保证用户在控制主动臂进行跟随训练时，主动臂的位置始终处于一个安全的位置；通过比较期望轨迹与第二运动数据的轨迹差异，对主动臂施加切向力和法向力，更准确地控制主动臂进行力的输出，防止输出的力过大而拉伤用户。
可选地，所述获得对所述主动臂的第二运动数据，基于所述第二运动数据与所述期望轨迹的轨迹差异，控制所述主动臂施加辅助力包括：
计算所述第二运动数据相对于所述期望轨迹的所述轨迹差异，基于所述轨迹差异与预设的初始权重获得刚度系数参数；基于所述刚度系数参数调整所述第二势能场的强度，所述第二势能场用于控制主动臂施加向所述期望轨迹的法线方向的所述法向力。
由此，通过轨迹差异获得刚度系数参数，通过刚度系数参数调整势能场强度，从而控制主动臂施加的法向力大小，保证法向力是基于轨迹差异而得出的，法向力不会过大而导致伤害用户，也不会过小而导致训练强度过差。
可选地，在所述基于所述刚度系数参数调整所述第二势能场的强度之后，还包括：
再次获取对所述主动臂的所述第二运动数据；基于再次获取的所述第二运动数据与所述期望轨迹计算所述轨迹差异，基于所述轨迹差异更新所述刚度系数参数；基于更新后的所述刚度系数参数进一步调整所述第二势能场的强度。
由此，通过多次获取第二运动数据而获得轨迹差异，优化势能场的强度，保证获得最适于用户训练的势能场强度。
可选地，所述第一运动数据还包括第一输入力，所述第二运动数据还包括第二输入力；所述计算所述第二运动数据相对于所述期望轨迹的所述轨迹差异，基于所述轨迹差异与预设的初始权重获得刚度系数参数包括：
对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型；基于所述第一动力学模型、所述第二运动数据与所述轨迹差异，控制所述主动臂输出阻抗辅助力，所述阻抗辅助力用于对所述主动臂施加向所述期望轨迹的切线方向的所述切向力。
由此，通过动力学建模，可以实现对机械臂的精确控制，进而控制机械臂输出较为平滑的相对于期望轨迹切线方向的阻抗辅助力，配合势能场，保证对主动臂进行法向和切向的完全控制。
可选地，所述对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型包括：
对所述主动臂或所述从动臂进行DH建模。
由此，通过DH建模实现对机械臂的精确控制。
可选地，所述基于所述第二手柄的第二起始位置和所述期望轨迹建立与所述第一势能场互为镜像的第二势能场之后，还包括：
基于所述第二手柄的实际位置与所述期望轨迹之间的距离，建立阻尼场，其中，所述阻尼场中阻尼力的大小与所述实际位置和所述期望轨迹的最小距离呈正相关。
由此，在主动臂偏离期望轨迹时，通过阻尼力防止主动臂偏离目标，使训练更加安全。
可选地，基于所述第二手柄的实际位置与所述期望轨迹之间的距离，建立阻尼场，其中，所述阻尼场中阻尼力的大小与所述实际位置和所述期望轨迹的最小距离呈正相关包括：
所述阻尼力通过如下公式设置：
其中，α表示距离系数，FB表示所述阻尼力，B0表示预设的最小阻尼力， L表示所述第二手柄的所述位置与所述期望轨迹的最小距离，表示所述第二手柄在所述实际位置的的速度。
由此，阻尼力与距离呈正相关关系，第二手柄的位置偏离期望轨迹的程度越大，阻尼力就越大，防止主动臂过于偏离期望轨迹，保证训练的安全性。
可选地，所述轨迹差异为所述第二运动轨迹与所述期望轨迹上预设的每个采样点之间的平均距离。
由此，可以更加精准地获取轨迹差异。
可选地，所述辅助力还包括重力补偿力和摩擦力补偿力。
由此，在辅助力中加入重力补偿和摩擦力补偿可以保证主动臂的输出更适应实际需要，减小控制误差。
另一方面，本发明还提出一种镜像机械臂设备，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上任一项所述的机械臂镜像阻抗控制方法。
所述镜像机械臂设备相对于现有技术与所述机械臂镜像阻抗控制方法所具有的优势相同，在此不再赘述。
附图说明
图1为本发明实施例的机械臂镜像阻抗控制方法的流程示意图；
图2为本发明实施例的机械臂镜像阻抗控制方法步骤S400细化后的流程示意图；
图3为本发明实施例的机械臂镜像阻抗控制方法步骤S402细化后的流程示意图；
图4为本发明实施例的机械臂镜像阻抗控制方法步骤S404之后的流程示意图；
图5为本发明实施例的机械臂镜像阻抗控制方法的示意图。
具体实施方式
在常见的主从机械臂控制策略中，通过一侧的机械臂引导另一侧机械臂运动，主要包括对机械臂设置一个特定的训练轨迹，然后引导需要辅助力的另一侧机械臂进行轨迹跟踪，其具有时变的特性，即根据时间的推移，控制机械臂的运动。
在实际使用场景中，很多使用者不能完全主动控制机械臂的运动，会出现使用者的上肢跟不上机械臂的移动的情况，此时机械臂只基于预设的时间与预设的轨迹进行运动，不会考虑到使用者的上肢能力，当机械臂运动过快或运动的幅度过大时，会导致使用者的上肢出现过度拉伸的情况，导致上肢出现痉挛；另外，由于位移或者角度的不断增加，机械臂的电机产生一个较大的扭矩，此时机械臂与使用者之间将会产生一个很大的接触力，容易对使用者造成二次伤害。
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
为了解决上述问题，本发明提供一种基于势能场的镜像机械臂控制方法，其特征在于，应用于镜像机械臂设备，所述镜像机械臂设备包括主动臂、从动臂、设置在所述从动臂上的第一手柄和设置在所述主动臂上的第二手柄。
在本发明实施例中，机械臂包括主动臂和从动臂。
从动臂用于接收用户较为有力量的一侧的上肢的输入信号，用于制定期望轨迹，也用于指导主动臂的运动轨迹；主动臂用于接收用户另一侧较为无力的上肢的输入信号，通过两侧信号的差异，对接触主动臂的一侧上肢进行辅助力的输出，从而达到训练上肢的目的。其中，从动臂上设置有识别位置的编码器和识别力的传感器，主动臂上设置有电机，用于辅助上肢向预设预设轨迹移动。
如图1和图5所示，所述基于势能场的镜像机械臂控制方法包括：
步骤S100，获得对所述从动臂的第一运动数据，基于所述第一手柄的第一起始位置和所述第一运动数据建立第一势能场，其中，所述第一运动数据包括第一运动轨迹。
在一实施例中，主动臂与从动臂的关节参数是完全相同的，用于实现主动臂与从动臂的完全镜像轨迹的模拟，换言之，当使用者的双臂的力量完全相同，且控制能力相同时，可以通过一侧的上肢控制从动臂完成一段位移轨迹，然后通过另一侧上肢完成镜像的位移轨迹。
通过获取用户对从动臂的第一运动数据，获得第一运动轨迹，然后通过第一运动轨迹来规定主动臂所需要完成的轨迹，以实现对用户的上肢力量训练。
由于用户的上肢与第一手柄和第二手柄直接接触，所以，需要基于所述第一手柄的第一起始位置和所述第一运动数据建立第一势能场，可以通过第一手柄的位置实现对用户上肢位置的跟踪，然后根据上肢位置来规定另一侧上肢的期望轨迹。将用户控制第一手柄位移划出的轨迹作为第一运动轨迹，将用户控制第二手柄位移划出的实际轨迹作为第二运动轨迹。
在步骤S100中，因从动臂上未设置电机，所以从动臂的势能场不具有纠偏作用，即从动臂不会施加一个对用户上肢的辅助作用，只对运动轨迹进行势能场的建立，用于将势能场镜像施加到主动臂周围。
在一实施例中，对第一手柄的运动轨迹进行分解，获得多个位置点，通过位置点共同建立一个势能场。基于运动轨迹建立势能场，可以保证主动臂在进行跟随运动时，每个运动点都会受到势能场的干预。
步骤S200，将所述第一运动轨迹输入自适应控制器进行响应和跟踪，获得期望数据，其中，所述期望数据包括期望轨迹，所述期望轨迹为所述主动臂所需要到达的与所述从动臂互为镜像的位置。
自适应控制器包括可以按照预定顺序改变主电路或控制电路的接线和改变电路中阻值来控制电动机的启动、调速、制动和反向的主令装置。
因本发明涉及的机械臂的训练过程包括：使用者通过力量较强的第一上肢控制从动臂做位移运动，通过自适应控制器进行响应和跟踪，获得第一输入位移，然后使用者通过另一侧力量较弱的第二上肢尽量跟随从动臂做镜像的位移运动，此时使用者需要让第二上肢模拟第一上肢的运动。所以，将第一上肢做的位移运动设定为期望轨迹，所述期望轨迹为主动臂所需要到达的与从动臂互为镜像的位置，以达到对使用者较为虚弱的另一侧上肢进行训练的目的。
步骤S300，基于所述第二手柄的第二起始位置和所述期望轨迹建立与所述第一势能场互为镜像的第二势能场。
因需要训练的上肢是直接与主动臂接触，通过主动臂来辅助用户进行上肢力量训练，所以在以第一运动轨迹为中心建立势能场之后，需要以第二运动轨迹为中心建立势能场。
用户使用第二上肢通过第二手柄做跟随第一上肢的镜像运动，将第二手柄的实际位移轨迹作为第二运动轨迹。
当用户的第二上肢跟随第一上肢做镜像运动时，如果第二运动轨迹偏离了期望的轨迹，则会受到势能场的纠偏力的作用，辅助主动臂往轨迹的线的方向移动，确保训练轨迹的精准度，实现对第二上肢的辅助力量训练。
步骤S400，获得对所述主动臂的第二运动数据，基于所述第二运动数据与所述期望轨迹的轨迹差异，控制所述主动臂施加辅助力，其中，所述第二运动数据包括第二运动轨迹，所述辅助力包括切向力和法向力，所述法向力由所述第二势能场提供。
获取到用户对主动臂的第二运动数据，通过第二运动数据与期望轨迹的差异的大小来对主动臂施加一个向期望轨迹靠拢的辅助力，实现对用户上肢的训练。主动臂通过输出一个切向力来辅助用户的上肢朝轨迹的正向方向进行移动；当用户的第二上肢的运动轨迹发生偏移时，通过输出一个法向力，辅助用户的上肢朝靠拢轨迹的方向移动。
下面通过举例的方式说明切向力和法向力，在一实施例中，用户通过第一上肢控制从动臂进行顺时针的圆周运动，此时将从动臂的逆时针的圆周运动的运动轨迹作为期望轨迹，基于此轨迹建立第一势能场，然后将第二势能场镜像夹建立在主动臂周围，以干预主动臂的运动轨迹。在此同时，用户通过第二上肢控制主动臂进行互为镜像的逆时针的圆周运动，因用户的第二上肢的力量较弱，所以可能会出现偏离期望轨迹的情况，在第二上肢偏离期望轨迹时，主动臂的电机输出辅助力，干预第二上肢向期望轨迹靠拢，其中，辅助力包括切向力和法向力，切向力保证主动臂往逆时针的方向缓慢行进，而法向力保证主动臂缓慢地往最近的期望轨迹上靠拢。
在另一实施例中，用户控制从动臂进行顺时针的圆周运动，则主动臂输出与从动臂的运动轨迹方向相同的切向力，即顺时针的圆周运动。
因控制互为镜像的运动更容易出现轨迹上的错误，故在一些实施例中，主动臂与从动臂的运动可以为相同方向，以降低用户控制主动臂位移的难度，增加用户训练的积极性。
可选地，如图2所示，步骤S400包括：
步骤S401，计算所述第二运动数据相对于所述期望轨迹的所述轨迹差异，基于所述轨迹差异与预设的初始权重获得刚度系数参数。
步骤S402，基于所述刚度系数参数调整所述第二势能场的强度，所述第二势能场用于控制主动臂施加向所述期望轨迹的法线方向的所述法向力。
通过第二上肢控制第二手柄跟随期望轨迹的重合程度，设定刚度系数参数，即通过第二上肢对于期望轨迹的完成情况，来判断第二上肢的能力是否达到了预设能力，如果第二上肢移动的轨迹与预设轨迹相差过大，则表示第二上肢的力量较弱，无法主动完成训练目标，需要较大的辅助力来辅助完成训练，则此时通过轨迹差异的大小来设置刚度系数参数，通过刚度系数来干预主动臂中电机的输出平滑程度和力量大小。当第二上肢力量较差时，电机输出的力会更大，反之，电机的力量输出会更小，鼓励用户通过自己的力量完成训练。
刚度系数是用以描述材料在外力作用下弹性变形形态的基本物理量。更通俗的讲是使杆端产生单位位移时所需施加的杆端力。
在一实施例中，刚度系数用于调整第二势能场的强度，主动臂受到的第二势能场的强度可以看作其在第二势能场中受到的弹性势能的强度。此势能用于将主动臂向期望轨迹的法向方向施加辅助力，在第二势能场的设计中，期望轨迹上点的势能相等且全局最低，期望轨迹之外的点势能随着距离的增加而不断增加，第二势能场在期望轨迹上不产生法向的辅助力，因此在势能场的引导下，主动臂可以在期望轨迹上***而不受力，在期望轨迹之外，将会受到法向力的干预。
在一实施例中，同一刚度系数产生的力与第二上肢的位置和期望轨迹的距离有关，第二上肢的位置与期望轨迹的距离越大，则力越大，反之，力越小。
基于以上准则，第二势能场的设计如下所示：
其中，p为运动空间的任意一点，是所要求解的最优值，是一个标量，φi(p)表示在空间中点p处的势能，K表示刚度系数，K越大，φi(p)越大，β表示刚度系数参数。
根据上述公式可知，p距离pi越远，弹性势能越大。
利用高斯核函数，计算p点的势能相对于N个采样点的权重，对单点势能φi(p)做回归处理，得到势能场函数Φ(p)，具体计算过程如下所示：
在第二势能场中，各个采样点的势能是相同的，势能场的负梯度方向就是主动臂末端受力的方向。我们希望在采样点p上，势能场产生的力为0，因此对于势能场参数/>的选择可以转化为求解凸优化问题，如下所示：
其中，
势能场产生的力是根据空间位置变化产生的一个定值。如果在运动过程中，第二上肢偏离了指定的运动轨迹，在势能场的作用下，将会产生一个法向力，通过修改刚度系数K来调节法向力的大小，同时也调节了第二上肢在运动轨迹法线方向上的***的能力。
进而获得如下公式：
实现对势能场的建立。
可选地，如图3所示，在步骤S402之后，还包括：
步骤S403，再次获取对所述主动臂的所述第二运动数据。
进行一次训练后，用户再控制从动臂进行运动，使用第二上肢控制主动臂进行跟随运动时，训练结果会稍有变化，此时需要再次获取第二运动数据，对用户后续训练数据进行采集，以获得更适合用户的训练辅助力。
步骤S404，基于再次获取的所述第二运动数据与所述期望轨迹计算所述轨迹差异，基于所述轨迹差异更新所述刚度系数参数。
在某些情况下，用户根据训练次数的增加，其上肢力量会有所变化；在另一些情况下，只根据一次的第二运动数据建立第二势能场，对用户的真实水平不具有较高参考性，所以通过再次获取的轨迹差异更新刚度系数参数，以获得更适于用户训练的第二势能场，保证获得更好的训练效果。另一方面，用户在训练的过程中，上肢力量可能会发生变化，此时通过再次计算轨迹差异更新刚度系数参数，可以保证主动臂输出的力始终与用户的上肢力量相适应。
通过以下公式获得刚度系数参数：
其中，β表示影响弹性系数K的刚度系数参数，Lmax表示规定的最大偏移距离，Ln表示第n个点的偏移距离，n∈1...M，Score表示运动周期的得分，表示第二运动轨迹与期望轨迹的偏离程度，Error表示在一个训练周期中，主动臂偏离期望轨迹的平均值。
步骤S405，基于更新后的所述刚度系数参数进一步调整所述第二势能场的强度。
可选地，如图4所示在步骤S404之后，还包括：
步骤S4041，对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型。
可选地，所述第一动力学模型通过DH建模建立。
DH建模，是由Denavit和Hartenberg提出的一种建模方法，主要用在机器人运动学上，这种方法在每个连杆上建立一个坐标系，通过齐次坐标变换来实现两个连杆上坐标的变换，在多连杆串联的***中，多次使用齐次坐标变换，以建立首末坐标系的关系。
在本发明实施例中，第一动力学模型用于控制机械臂实现阻抗控制。
阻抗控制，表示不直接控制机械臂末端与环境接触力。通过机械臂末端与环境之间的动态关系，将力的控制和未知的控制综合起来考虑，用相同的策略实现力的控制和位置控制。
步骤S4042，基于所述第一动力学模型、所述第二运动数据与所述轨迹差异，控制所述主动臂输出阻抗辅助力，所述阻抗辅助力用于对所述主动臂施加向所述期望轨迹的切线方向的所述切向力。
基于步骤S4041建立的第一动力学模型可以帮助机械臂实现很多控制方法，如位置控制和力矩控制，其中，本发明使用阻抗控制的方法，利用阻抗控制实现对主动臂和从动臂的轨迹跟踪，保证根据训练过程以及使用者的左右臂力量差距情况和参与程度来控制主动臂的输出。
可选地，在步骤S300之后，还包括：
基于所述第二手柄的实际位置与所述期望轨迹之间的距离，建立阻尼场，其中，所述阻尼场中阻尼力的大小与所述实际位置和所述期望轨迹的最小距离呈正相关。
由于建模的不确定性(例如摩擦力模型不准确，机械结构参数测量不精确，以及机械臂在与使用者交互过程中，使用者施力的大小不确定)，将会导致下列两种情况：①在工作空间中第二势能场产生的法向力并不足以纠正患者的偏离程度；②缺少辅助力使得患者不足以完成任务，导致患者参与训练的积极性降低。
为解决上述两问题，在一实施例中设计一个基于第二手柄位置预期望轨迹的距离变化的阻尼场，既可以保证第二手柄的位置过偏时防止第二手柄继续偏移，也通过改变阻尼值大小而调节第二手柄的运动速度，防止第二上肢移动过快或主动臂的输出速度过快，保证了***的安全性，防止第二上肢移动速度过快，也尽可能减小使用者在训练过程中第二上肢位置上的安全性和速度上的安全性。
可选地，步骤S301包括：
所述阻尼力通过如下公式设置：
其中，α表示距离系数，FB表示所述阻尼力，B0表示预设的最小阻尼力， L表示所述第二手柄的所述位置与所述期望轨迹的最小距离，表示所述第二手柄在所述实际位置的的速度。
在距离期望轨迹5cm范围内，阻尼为定值，当L大于或等于5cm时，阻尼值线性增加，阻尼场函数是关于空间位置变化的函数。
可选地，所述轨迹差异为所述第二运动轨迹与期望轨迹预设的每个采样点之间的平均距离。
在一实施例中，为了准确获得轨迹差异，在第一运动轨迹上，每隔一段距离设置一个采样点，在主动臂所要达到的期望轨迹上也设置有相同个数、相同位置的采样点，通过计算主动臂的实际位置与最近的采样点的平均距离来计算轨迹差异。
可选地，所述辅助力还包括重力补偿力和摩擦力补偿力。
控制主动臂的电机输出辅助力的同时，还输出与主动臂受到的重力相平衡的重力补偿力、与主动臂运动时，机械关节之间的内部摩擦相平衡的摩擦力补偿力。在辅助力中加入重力补偿和摩擦力补偿，可以使用户在训练时避免被机械臂的重力、机械臂自身运动的摩擦力影响，提升训练效果。
另一方面，本发明还提出一种镜像机械臂设备，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上任一项所述的机械臂镜像阻抗控制方法。
所述镜像机械臂设备相对于现有技术与所述机械臂镜像阻抗控制方法所具有的优势相同，在此不再赘述。
虽然本公开披露如上，但本公开的保护范围并非仅限于此。本领域技术人员在不脱离本公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (10)

1.一种基于势能场的镜像机械臂控制方法，其特征在于，应用于镜像机械臂设备，所述镜像机械臂设备包括主动臂、从动臂、设置在所述从动臂上的第一手柄和设置在所述主动臂上的第二手柄，所述基于势能场的镜像机械臂控制方法包括：
获得对所述从动臂的第一运动数据，基于所述第一手柄的第一起始位置和所述第一运动数据建立第一势能场，其中，所述第一运动数据包括第一运动轨迹；
将所述第一运动轨迹输入自适应控制器进行响应和跟踪，获得期望数据，其中，所述期望数据包括期望轨迹，所述期望轨迹为所述主动臂所需要到达的与所述从动臂互为镜像的位置；
基于所述第二手柄的第二起始位置和所述期望轨迹建立与所述第一势能场互为镜像的第二势能场，所述第二势能场用于使所述第二手柄的位置与所述期望轨迹的距离越大，产生的力越大；距离越小，力越小；
所述第二势能场表示为：其中，p表示运动空间的任意一点，/>表示所要求解的最优值，φi(p)表示在空间中点p处的势能，K表示刚度系数，β表示刚度系数参数，i表示采样点序号，T表示矩阵的转置，p距离pi越远，所述第二势能场提供的弹性势能越大；
获得对所述主动臂的第二运动数据，基于所述第二运动数据与所述期望轨迹的轨迹差异，控制所述主动臂施加辅助力，其中，所述第二运动数据包括第二运动轨迹，所述辅助力包括切向力和法向力，所述切向力由所述主动臂提供，所述切向力用于辅助所述主动臂朝所述期望轨迹的正向方向移动，所述法向力由所述第二势能场提供。
2.根据权利要求1所述的基于势能场的镜像机械臂控制方法，其特征在于，所述获得对所述主动臂的第二运动数据，基于所述第二运动数据与所述期望轨迹的轨迹差异，控制所述主动臂施加辅助力包括：
计算所述第二运动数据相对于所述期望轨迹的所述轨迹差异，基于所述轨迹差异与预设的初始权重获得所述刚度系数参数；
基于所述刚度系数参数调整所述第二势能场的强度，其中，所述第二势能场用于控制所述主动臂施加向所述期望轨迹的法线方向的所述法向力。
3.根据权利要求2所述的基于势能场的镜像机械臂控制方法，其特征在于，在所述基于所述刚度系数参数调整所述第二势能场的强度之后，还包括：
再次获取对所述主动臂的所述第二运动数据；
基于再次获取的所述第二运动数据与所述期望轨迹计算所述轨迹差异，基于所述轨迹差异更新所述刚度系数参数；
基于更新后的所述刚度系数参数进一步调整所述第二势能场的强度。
4.根据权利要求2所述的基于势能场的镜像机械臂控制方法，其特征在于，所述第一运动数据还包括第一输入力，所述第二运动数据还包括第二输入力；所述计算所述第二运动数据相对于所述期望轨迹的所述轨迹差异，基于所述轨迹差异与预设的初始权重获得刚度系数参数包括：
对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型；
基于所述第一动力学模型、所述第二运动数据与所述轨迹差异，控制所述主动臂输出阻抗辅助力，其中，所述阻抗辅助力用于对所述主动臂施加向所述期望轨迹的切线方向的所述切向力。
5.根据权利要求4所述的基于势能场的镜像机械臂控制方法，其特征在于，所述对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型包括：
对所述主动臂或所述从动臂进行DH建模。
6.根据权利要求1-5任一所述的基于势能场的镜像机械臂控制方法，其特征在于，所述基于所述第二手柄的第二起始位置和所述期望轨迹建立与所述第一势能场互为镜像的第二势能场之后，还包括：
基于所述第二手柄的实际位置与所述期望轨迹之间的距离，建立阻尼场，其中，所述阻尼场中阻尼力的大小与所述实际位置和所述期望轨迹的最小距离呈正相关。
7.根据权利要求6所述的基于势能场的镜像机械臂控制方法，其特征在于，所述基于所述第二手柄的实际位置与所述期望轨迹之间的距离，建立阻尼场包括：
所述阻尼力通过如下公式设置：
其中，α表示距离系数，FB表示所述阻尼力，B0表示预设的最小阻尼力，L表示所述第二手柄的所述位置与所述期望轨迹的最小距离，表示所述第二手柄在所述实际位置的速度。
8.根据权利要求6所述的基于势能场的镜像机械臂控制方法，其特征在于，所述轨迹差异为所述第二运动轨迹与所述期望轨迹上预设的多个采样点之间的平均距离。
9.根据权利要求6所述的基于势能场的镜像机械臂控制方法，其特征在于，所述辅助力还包括重力补偿力和摩擦力补偿力。
10.一种镜像机械臂设备，其特征在于，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如权利要求1-9任一项所述的基于势能场的镜像机械臂控制方法。
CN202111529951.4A 2021-12-07 2021-12-07 一种基于势能场的镜像机械臂控制方法及镜像机械臂设备 Active CN114434452B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111529951.4A CN114434452B (zh) 2021-12-07 2021-12-07 一种基于势能场的镜像机械臂控制方法及镜像机械臂设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111529951.4A CN114434452B (zh) 2021-12-07 2021-12-07 一种基于势能场的镜像机械臂控制方法及镜像机械臂设备

Publications (2)

Publication Number Publication Date
CN114434452A CN114434452A (zh) 2022-05-06
CN114434452B true CN114434452B (zh) 2024-06-04

Family

ID=81364025

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111529951.4A Active CN114434452B (zh) 2021-12-07 2021-12-07 一种基于势能场的镜像机械臂控制方法及镜像机械臂设备

Country Status (1)

Country Link
CN (1) CN114434452B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106475999A (zh) * 2016-12-23 2017-03-08 东南大学 刚性条件下基于阻抗模型的双臂协调的加速度控制方法
CN207202880U (zh) * 2017-03-10 2018-04-10 芜湖市第二人民医院 放射科用辅助患者站立支撑装置
CN112891127A (zh) * 2021-01-14 2021-06-04 东南大学 一种基于自适应阻抗控制的镜像康复训练方法
CN114700959A (zh) * 2021-12-01 2022-07-05 宁波慈溪生物医学工程研究所 一种机械臂镜像阻抗控制方法及镜像机械臂设备

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2010059066A1 (en) * 2008-11-19 2010-05-27 Industrial Research Limited Exercise device and system
JP6418652B2 (ja) * 2016-04-22 2018-11-07 トヨタ自動車株式会社 上肢リハビリ支援装置及びその作動方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106475999A (zh) * 2016-12-23 2017-03-08 东南大学 刚性条件下基于阻抗模型的双臂协调的加速度控制方法
CN207202880U (zh) * 2017-03-10 2018-04-10 芜湖市第二人民医院 放射科用辅助患者站立支撑装置
CN112891127A (zh) * 2021-01-14 2021-06-04 东南大学 一种基于自适应阻抗控制的镜像康复训练方法
CN114700959A (zh) * 2021-12-01 2022-07-05 宁波慈溪生物医学工程研究所 一种机械臂镜像阻抗控制方法及镜像机械臂设备

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于迭代学习控制的上肢康复机器人轨迹跟踪研究;李亚锋;《中国优秀硕士学位论文全文数据库 信息科技辑》;20201215;全文 *
带躯干变刚度双足机器人的周期行走控制;王珊;周亚丽;张奇志;;工程力学;20191225(第12期);全文 *

Also Published As

Publication number Publication date
CN114434452A (zh) 2022-05-06

Similar Documents

Publication Publication Date Title
Li et al. Adaptive human–robot interaction control for robots driven by series elastic actuators
Miyamoto et al. A kendama learning robot based on bi-directional theory
Pehlivan et al. A subject-adaptive controller for wrist robotic rehabilitation
CN111956452B (zh) 一种上肢康复机器人控制方法及装置
CN111888194B (zh) 一种上肢康复训练方法、***、装置及存储介质
CN110597072B (zh) 机器人导纳柔顺控制方法及***
Tsoi et al. Impedance control of ankle rehabilitation robot
JP7054130B2 (ja) 運動学習支援装置および運動学習支援方法
CN112274865B (zh) 下肢康复机器人的按需辅助自适应控制方法和***
WO2020118730A1 (zh) 机器人柔顺性控制方法、装置、设备及存储介质
Perrier Gesture planning integrating knowledge of the motor plant's dynamics: A literature review from motor control and speech motor control
Li et al. An integrated approach for robotic Sit-To-Stand assistance: Control framework design and human intention recognition
Almusawi et al. Online teaching of robotic arm by human–robot interaction: end effector force/torque sensing
Brahmi et al. Impedance learning control for physical human-robot cooperative interaction
CN113568422B (zh) 基于模型预测控制优化强化学习的四足机器人控制方法
Zhang et al. A learning control scheme for upper-limb exoskeleton via adaptive sliding mode technique
Huo et al. Adaptive intention-driven variable impedance control for wearable robots with compliant actuators
CN114434452B (zh) 一种基于势能场的镜像机械臂控制方法及镜像机械臂设备
CN117982855A (zh) 基于主动康复训练的上肢康复机器人运动控制方法
Khan et al. Muscle circumference sensor and model reference-based adaptive impedance control for upper limb assist exoskeleton robot
Jamšek et al. Predictive exoskeleton control for arm-motion augmentation based on probabilistic movement primitives combined with a flow controller
CN114700959B (zh) 一种机械臂镜像阻抗控制方法及镜像机械臂设备
Huang et al. Motion Regulation Solutions to Holding and Moving an Object for Single-Leader–Dual-Follower Teleoperation
CN116671941A (zh) 一种用于柔性下肢外骨骼的镜像自适应阻抗控制方法
de Azevedo Fernandes et al. A Real Framework to Apply Collaborative Robots in Upper Limb Rehabilitation.

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant