CN114700959B - 一种机械臂镜像阻抗控制方法及镜像机械臂设备 - Google Patents
一种机械臂镜像阻抗控制方法及镜像机械臂设备 Download PDFInfo
- Publication number
- CN114700959B CN114700959B CN202111504016.2A CN202111504016A CN114700959B CN 114700959 B CN114700959 B CN 114700959B CN 202111504016 A CN202111504016 A CN 202111504016A CN 114700959 B CN114700959 B CN 114700959B
- Authority
- CN
- China
- Prior art keywords
- arm
- displacement
- dynamics
- angle
- input
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000006073 displacement reaction Methods 0.000 claims abstract description 108
- 230000033001 locomotion Effects 0.000 claims abstract description 23
- 230000003044 adaptive effect Effects 0.000 claims abstract description 11
- 230000004044 response Effects 0.000 claims abstract description 7
- 210000001364 upper extremity Anatomy 0.000 claims description 22
- 210000000323 shoulder joint Anatomy 0.000 claims description 11
- 210000002310 elbow joint Anatomy 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 6
- 101100129500 Caenorhabditis elegans max-2 gene Proteins 0.000 claims description 5
- 101100083446 Danio rerio plekhh1 gene Proteins 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 5
- 238000004088 simulation Methods 0.000 claims description 3
- 238000012549 training Methods 0.000 description 21
- 230000001276 controlling effect Effects 0.000 description 12
- 230000000694 effects Effects 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 5
- 230000009466 transformation Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 230000003993 interaction Effects 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 238000005452 bending Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 210000000245 forearm Anatomy 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 210000005036 nerve Anatomy 0.000 description 2
- 208000007101 Muscle Cramp Diseases 0.000 description 1
- 208000005392 Spasm Diseases 0.000 description 1
- 208000027418 Wounds and injury Diseases 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 210000004556 brain Anatomy 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000006378 damage Effects 0.000 description 1
- 230000007850 degeneration Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 210000003414 extremity Anatomy 0.000 description 1
- 208000014674 injury Diseases 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 239000011664 nicotinic acid Substances 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Orthopedic Medicine & Surgery (AREA)
- General Health & Medical Sciences (AREA)
- Physical Education & Sports Medicine (AREA)
- Manipulator (AREA)
Abstract
本发明提供了一种机械臂镜像阻抗控制方法及镜像机械臂设备，包括：对主动臂或从动臂进行动力学建模，获得第一动力学模型，用于将手柄相对坐标系与世界坐标系相互转换；获取从动臂的第一输入位移；将第一输入位移输入自适应控制器进行响应和跟踪，获得期望数据，其中，期望数据包括期望位置，期望位置为主动臂所需要到达的位置；获取对主动臂的第二动力学数据，其中，第二动力学数据包括第二输入力；比对期望位置与第二输入位移的位移差；基于位移差、期望数据和第二动力学数据，控制主动臂输出阻抗辅助力，辅助主动臂相对于从动臂镜像运动，保证主动臂可以更平滑地输出阻抗辅助力。
Description
技术领域
本发明涉及机器人控制技术领域，具体而言，涉及一种机械臂镜像阻抗控制方法及镜像机械臂设备。
背景技术
随着近年来我国人口老龄化进程的加快，越来越多的老年人都会经历上肢功能的退化，为帮助老年人锻炼上肢功能，康复师会帮助老年人反复弯曲肢体，以减少肌肉痉挛并刺激神经可塑性，使大脑能够形成新的神经连接。但是这种传统的锻炼方法既费时又费力，成本也很高，且我国现阶段康复师数量严重不足，无法全面帮助老年人进行上肢锻炼。现有的镜像拉力器只能通过一侧上肢的输入力，对另一侧上肢进行完全的镜像位移，容易拉伤老年人的上肢。
发明内容
本发明解决的问题是：如何使机械臂更平滑地输出辅助力。
为解决上述问题，本发明提供一种机械臂镜像阻抗控制方法，
应用于镜像机械臂设备，所述镜像机械臂设备包括主动臂、从动臂、设置在所述从动臂上的第一手柄和设置在所述主动臂上的第二手柄，所述机械臂镜像阻抗控制方法包括：
对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型，用于将手柄相对坐标系与世界坐标系相互转换；获取对所述从动臂的第一动力学数据，其中，所述第一动力学数据包括第一输入位移；将所述第一输入位移输入自适应控制器进行响应和跟踪，获得期望数据，其中，所述期望数据包括期望位置，所述期望位置为所述主动臂所需要到达的与所述从动臂互为镜像的位置；获取对所述主动臂的第二动力学数据，其中，所述第二动力学数据包括第二输入位移；比对所述期望位置与所述第二输入位移的位移差；将所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动。
相对于现有技术，本发明通过将从动臂受到的第一动力学数据与主动臂受到的第二动力学数据共同作为控制因子，结合第一输入位移和第一输入力，实现对主动臂的阻抗控制，使用户可以根据自身的情况控制从动臂的位移，进而控制主动臂能够平滑地输出自适应的力，从而保证控制主动臂的手臂能够顺利移动到期望位置，以达到训练的效果，阻抗控制能保证主动臂不会输出过于强烈的力而拉伤用户手臂；通过对主动臂或从动臂进行动力学建模，可以保证对机械臂的控制更加精确，使自适应控制器可以精确控制主动臂的输出；通过对期望位置和第二输入位移计算位移差，可以保证主动臂能够输出向期望位置位移的力，实现对用户手臂的适应性训练，针对第二输入力、第二输入位移和第二输入方向制定适应性的输出，保证根据用户的实际情况输出辅助力，提升用户的使用体验，提升用户积极性。
可选地，所述将所述第一输入位移输入自适应控制器进行响应和跟踪，获得期望数据包括：
基于所述第一输入位移进行逆向动力学建模，获得第二动力学模型，逆向求解第一角度和第二角度，其中，所述第二动力学模型表示对人体手臂关节的模拟，所述第一角度表示人体肩关节的外展角度，所述第二角度表示人体肘关节的内收角度。
由此，通过对第一输入位移进行逆向动力学建模，求得第一角度和第二角度，可以更精确地模拟主动臂移动至期望位置所需要的力，使输出更加平滑，保证达到更好地训练效果，防止输出的力过大，使主动臂过度位移，降低用户的舒适度。
可选地，所述基于所述第一输入位移进行逆向动力学建模，获得第二动力学模型，逆向求解第一角度和第二角度包括：
对所述第一输入位移进行基于人体上肢的基于三个连杆的DH建模，获得所述第二动力学模型，其中，坐标系原点设置在肩关节上；
基于所述第二动力学模型，获得
其中，θ1表示所述第一角度，θ2表示所述第二角度，(x,y)表示所述第一手柄在笛卡尔坐标系中的位置坐标，d1表示第二连杆相对于第一连杆的偏置，d2表示第三连杆相对于所述第二连杆的偏置。
由此，通过第二动力学模型，获得第一角度和第二角度，可以使机械臂的控制更加精确，保证实现机械臂的阻抗控制。
可选地，所述对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型包括：
对所述主动臂或所述从动臂进行DH建模，其中，坐标系原点设置在第一关节臂上。
由此，通过对主动臂或从动臂进行DH建模，可以保证实现机械臂对于复杂的场景的控制，实现对机械臂的高精度控制、碰撞检测、机械臂的力的控制以及牵引控制。
可选地，所述第二动力学数据还包括第二输入力；所述基于所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动包括：
基于所述位移差、所述第一角度和所述第二角度获得所述主动臂的虚拟刚度，其中，所述虚拟刚度随所述位移差、所述第一角度和所述第二角度的变化而变化；基于所述虚拟刚度和所述第二输入力获得所述阻抗辅助力。
基于位移差、期望数据和第二动力学数据，帮助实现通过从动臂与主动臂的输入力、输入位移的差异来制定主动臂的阻抗大小，其中，虚拟刚度保证实现对主动臂的平滑控制。
可选地，所述虚拟刚度通过如下公式求得：
其中，当θ为θ1时，求出的K表示所述主动臂对所述人体肩关节输出的虚拟刚度、当θ为θ2时，求出的K表示所述主动臂对所述人体肘关节输出的虚拟刚度、Kmax1表示所述主动臂提供的最大虚拟刚度、Kmax2表示所述主动臂受到的最大虚拟刚度、Kmin表示所述虚拟刚度的最小值、emax和emin分别为虚拟误差范围的最大值和最小值、e为所述位移差、β为可调节参数。
由此，通过第一角度和第二角度以及位移差计算虚拟刚度，实现对主动臂的精确控制，以保证主动臂可以平滑地输出力。
可选地，在所述比对所述期望位置与所述第二输入位移的位移差之后，还包括：
判断所述位移差是否大于预设位移差，若是，则进行所述基于所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动的步骤。
由此，判断位移差是否大于预设位移差，可以保证主动臂可以根据需要而进行阻抗控制，在不需要干预时不进行阻抗控制。
可选地，所述第二动力学数据还包括第二输入方向；所述基于所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动包括：
若所述第二输入方向在所述主动臂与所述从动臂的连接线上的投影朝向所述从动臂，则通过如下公式增大所述阻抗辅助力的干预范围：
若所述第二输入方向在所述主动臂与所述从动臂的连接线上的投影远离所述从动臂，则通过如下公式减小所述阻抗辅助力的干预范围：
emax＝0.1ln(Fs)+0.1；
其中，Fs为所述第二输入力。
由此，根据所述第二输入方向在所述主动臂与所述从动臂的连接线上的投影朝向，制定不同大小的阻抗辅助力，保证根据用户的力量水平制定合适的辅助力。
可选地，所述判断所述位移差是否大于预设位移差，若是，则进行所述基于所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动的步骤之后，还包括：
在所述阻抗辅助力中加入重力补偿和摩擦力补偿。
由此，加入重力补偿和摩擦力补偿可以保证主动臂的输出更适应实际需要，减小控制误差。
另一方面，本发明还提出一种镜像机械臂设备，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上任一项所述的机械臂镜像阻抗控制方法。
所述镜像机械臂设备相对于现有技术与所述机械臂镜像阻抗控制方法所具有的优势相同，在此不再赘述。
附图说明
图1为本发明实施例的机械臂镜像阻抗控制方法的流程示意图；
图2为本发明实施例的机械臂镜像阻抗控制方法的控制流程图；
图3为本发明实施例的机械臂镜像阻抗控制方法步骤S600细化后的流程示意图。
具体实施方式
人体的上肢结构较为复杂，现有的机械臂只能进行肘关节弯曲/伸展和前臂伸/屈的二维平面运动，活动空间受到很大限制，且不能根据用户的需求而改变训练任务，训练方法较为单一。其中，有些训练机械臂是通过完全复制一侧机械臂的位移，从而控制另一侧机械臂的运动，迫使用户的一侧手臂跟踪另一侧手臂，没有考虑到每个用户的状态，缺乏人机交互性，容易对用户造成伤害；另一些机械臂是通过制定对一侧手臂的预设轨迹来达到训练的目的，该种机械臂控制方法在用户出现不适时无法对轨迹进行改变，容易造成伤害；没有考虑到用户的训练意愿，打击用户训练积极性，达不到最佳训练效果。
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
为了解决上述问题，本发明提供一种机械臂镜像阻抗控制方法，应用于上肢镜像训练器，其中，所述上肢镜像训练器包括主动臂和从动臂，从动臂用于接收用户一侧手臂的输入信号，主动臂用于接收用户另一侧手臂的输入信号，通过两侧手臂的输入信号的差异，对另一侧手臂进行自适应阻抗辅助力的输出，从而达到锻炼上肢的目的。所述从动臂包括第一从动杆、第二从动杆、第三从动杆第四从动杆，其中，所述第四从动杆还包括第一手柄；所述主动臂包括第一主动杆、第二主动杆、第三主动杆和第四主动杆，其中，第四主动杆还包括第二手柄。
在一实施例中，第一主动杆第一从动杆的长度为0，原点坐标系原点与第一主动杆或第一从动杆的坐标系原点重合，方向不同。
如图1、图2所示，机械臂镜像阻抗控制方法包括：
步骤S100，对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型，用于将手柄相对坐标系与世界坐标系相互转换。
本发明实施例中所称的机械臂包括主动臂和从动臂。
动力学建模主要是为了获取机械臂运动过程中的关节力矩，将运动过程中的关节力矩通过前馈的方式补偿到驱动器三环控制中的最内环电流环，从而提高驱动器的响应速度，进而提高机械臂的高精度控制；同时，可以进行机械臂力的控制，通过设定机械臂的末端输出力矩，可以让机械臂以某一力矩在被作用对象上；还可以让机械臂沿着被施加的外力运动，在本发明中，通过对主动臂或从动臂进行动力学建模，可以更好地控制主动臂的输出力矩，达到精确控制的目的，可以防止机械臂频繁地受到外接的扰动，保护使用者不被机械臂伤害。
可选地，主动臂和从动臂的连杆参数相同，是完全镜像的。
在一实施例中，因主动臂和从动臂的机械结构是镜像的，所以，只需要对主动臂和从动臂其中的一个进行建模即可，可以减小建模计算量。
在本发明的一个实施例中，对主动臂或从动臂进行建模，主要是研究机械臂末端的手柄所在的相对坐标系与世界坐标系之间的转换关系，其坐标原点设置在第一主动杆上或第一从动杆与其他从动杆连接的关节处。
可选地，通过DH建模方法，对机械臂进行动力学建模，获得第一动力学模型。
DH建模，是由Denavit和Hartenberg提出的一种建模方法，主要用在机器人运动学上，这种方法在每个连杆上建立一个坐标系，通过齐次坐标变换来实现两个连杆上坐标的变换，在多连杆串联的系统中，多次使用齐次坐标变换，以建立首末坐标系的关系。
以对主动臂进行动力学建模为例：设坐标原点所在的点为第一关节，第二主动杆与第三主动杆所连接的关节为第二关节，第三主动杆与第四主动杆所连接的关节为第三关节，第四主动杆与手柄所连接的关节为第四关节，则机械臂的DH参数如下表所示：
则世界坐标系中，手柄坐标系的变换矩阵方程如下所示：
其中，q是关节位置，[n，o，a]和p分别是在世界坐标系中，第二手柄的方向矩阵和位置向量，di表示第i+1连杆相对于第i连杆的偏置。
步骤S200，获取对所述从动臂的第一动力学数据，其中，所述第一动力学数据包括第一输入位移。
基于步骤S100建立的第一动力学模型可以帮助机械臂实现很多控制方法，如位置控制和力矩控制，其中，本发明使用阻抗控制的方法，利用阻抗控制实现对主动臂和从动臂的轨迹跟踪，保证根据训练过程以及使用者的左右臂力量差距情况和参与程度来控制主动臂的输出。
阻抗控制，表示不直接控制机械臂末端与环境接触力。通过机械臂末端与环境之间的动态关系，将力的控制和未知的控制综合起来考虑，用相同的策略实现力的控制和位置控制。
可选地，第一动力学数据还包括第一输入力。
所以，在一实施例中，获取从动臂的第一输入位移和受到的第一输入力，可以通过位移的大小来实现对主动臂的阻抗控制，也可以通过位移和力的大小共同实现对主动臂的阻抗控制，保证能实时性地对机械臂末端进行控制，也保证可以构建一个完整的系统，使之能同时控制力和位置，能更好地实现主动臂受到从动臂的变化而产生自适应变化。
步骤S300，将所述第一输入位移输入自适应控制器进行响应和跟踪，获得期望数据，其中，所述期望数据包括期望位置，所述期望位置为所述主动臂所需要到达的与所述从动臂互为镜像的位置。
自适应控制器包括可以按照预定顺序改变主电路或控制电路的接线和改变电路中阻值来控制电动机的启动、调速、制动和反向的主令装置。
通过使用者对从动臂的控制，获得第一输入位移，然后通过第一输入位移来获得主动臂的期望位置，即通过从动臂的输入来获得主动臂所需要的输出位置以及输出力的大小，当主动臂的位置未达到期望位置，或未达到主动臂所期望的力时，由自适应控制器通过阻抗控制的方式，对主动臂施加辅助控制。
步骤S400，获取对所述主动臂的第二动力学数据，其中，所述第二动力学数据包括第二输入位移。
在一实施例中，辅助控制并不是完全通过从动臂的第一输入位移控制的，而是与第二动力学数据相结合，从而控制主动臂进行自适应阻抗控制，所以，在步骤S300之后，还需要获取使用者对主动臂输入的力、位移或方向，即第二动力学数据。
步骤S500，比对所述期望位置与所述第二输入位移的位移差。
通过比对用户控制主动臂的实际位置与用户控制从动臂的实际位置的镜像位移差异，即通过比对期望位置与第二输入位移的位移差，使用自适应控制器辅助控制主动臂，以达到辅助训练的效果。其中，位移差为镜像位移差，下面通过举例说明镜像位移差，当用户的左臂控制从动臂向外进行位移时，用户右臂控制的主动臂的期望位置为原始位置向外进行相同的位移；当用户的左臂控制从动臂向内进行位移时，用户的右臂控制的主动臂的期望位置为原始位置向内进行相同的位移。
步骤S600，将所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动。
通过互为镜像的位移差与期望数据控制主动臂输出的阻抗辅助力，换言之，当用户左臂控制的从动臂朝一个方向位移一定距离时，若用户的右臂的力气不够，或用户右臂控制主动臂朝非镜像的位置位移时，自适应控制器通过位移差和期望数据控制主动臂进行辅助运动，帮助用户的右臂朝期望位移和期望方向移动，完成对用户的双臂训练，此时若用户感觉不适，则可以通过将左臂收回，使自适应控制器输出一个收回的力，帮助用户的右臂进行收回，防止左臂的位移、角度过大而损伤力气不足的右臂，避免左臂用力过猛，而对右臂带来的伤害，同时，通过阻抗控制，实现对主动臂的平滑控制，防止加速度过大而对拉伤用户的右臂。
可选地，如图2所示，在所述阻抗辅助力中加入重力补偿和摩擦力补偿。
在阻抗辅助力中加入重力补偿和摩擦力补偿，可以更精确地控制主动臂的输出，帮助主动臂能根据实际情况输出最适合辅助用户的力。
可选地，步骤S300包括：
基于所述第一输入位移进行逆向动力学建模，获得第二动力学模型，逆向求解第一角度和第二角度，其中，所述第二动力学模型表示对人体手臂关节的模拟，所述第一角度表示人体肩关节的外展角度，所述第二角度表示人体肘关节的内收角度。
可选地，通过DH建模方法建立第二动力学模型。
在一实施例中，对人体上肢和机械臂均进行DH建模，保证机械臂可以依据人体结构或规律而控制，更适于训练。人体上肢建模，还可以从仿生的角度，优化机械臂的控制，使机械臂的动力输出更精确。
上肢的训练过程中，因伸展角度不同，会有不同的张力阻碍其伸展，所以需要根据阻抗控制策略，来对用户的上肢进行自适应阻抗控制，即通过对主动臂的第二输入力进行自适应力的输出。
由于人体上肢功能复杂，为了更精确地控制机械臂以及更好地保护上肢，本发明对第一输入位移进行逆向动力学建模，从而使主动臂可以更精确地对用户进行训练。
人体手臂结构较为复杂，其中，肩关节具有三个自由度，较为灵活，而肘关节具有两个自由度，为了精确控制，将第二动力学模型简化为各有一个自由度的模型，在用户的手接触手柄时，得到手柄位置在世界坐标系的位置，从而通过第二动力学模型对关节角度进行逆向求解。
可选地，所述基于所述第一输入位移进行逆向动力学建模，获得第二动力学模型，逆向求解第一角度和第二角度包括：
对所述第一输入位移进行基于人体上肢的基于三个连杆的DH建模，获得所述第二动力学模型，其中，坐标系原点设置在肩关节上；
基于所述第二动力学模型，获得
其中，θ1表示所述第一角度，θ2表示所述第二角度，(x,y)表示所述第一手柄在笛卡尔坐标系中的位置坐标，d1表示第二连杆相对于第一连杆的偏置，d2表示第三连杆相对于所述第二连杆的偏置。
其中，三个连杆分别为第一连杆、第二连杆和第三连杆。在一实施例中，与手柄连接的连杆为第三连杆，与用户小臂对应；第三连杆另一侧连接的连杆为第二连杆，与用户的大臂对应；第二连杆另一侧连接的连杆为第一连杆；第一连杆与用户的肩膀对应，其长度为0；第一连杆与第二连杆的夹角为第一夹角，第二连杆与第三连杆的夹角为第二夹角。
可以由第二动力学模型计算出人体肩关节外展的角度，以及肘关节内收的角度。
可选地，步骤S600包括：
步骤S601，基于所述位移差、所述第一角度和所述第二角度获得所述主动臂的虚拟刚度，其中，所述虚拟刚度随所述位移差、所述第一角度和所述第二角度的变化而变化。
步骤S602，基于所述虚拟刚度和所述第二输入力获得所述阻抗辅助力。
在对机械臂进行阻抗控制时，需要建立阻抗模型，将使用者的一个手臂对从动臂的轨迹作为整个镜像机械臂系统的输入，再通过自适应控制器进行响应和跟踪，在此过程中，需要引入三个参数来实现阻抗控制，帮助主动臂的输出更为平滑，所述三个参数包括虚拟质量、虚拟阻尼和虚拟刚度，其中，镜像机械臂洗头工在笛卡尔空间的阻抗控制可以表示为其中，xdes为用户对从动臂的第一输入位移，即期望位移，xtar为用户对主动臂的第二输入位移，即实际位移，M、B、K分别是阻抗模型所需的虚拟质量、虚拟阻尼和虚拟刚度参数，F为主动臂施加的力，Fs为用户对主动臂施加的外力。为了在运动空间的不同方向上具有不同的阻抗特性，以更好地辅助训练，以上参数可以用矩阵代替，当用户对主动臂的第二输入位移偏离期望位移时，自适应控制器将对主动臂施加辅助力。
Mdes表示期望虚拟质量，其与期望加速度相乘，是基于加速度的参数；Bdes表示期望虚拟阻尼，其与期望速度相乘，是基于速度的虚拟参数。
研究表明，满训练对上肢的训练效果要好于快训练，所以在对用户的上肢镜像训练中，运动位置比运动速度更加重要，所以上述关系可以近似忽略加速度和速度的影响，简化为K(xdes-xtar)＝Fassist。
可选地，肘关节伸展期间呈现的阻抗力的具体角度大于等于0°且小于等于140°。
可选地，所述虚拟刚度通过如下公式求得：
其中，当θ为θ1时，求出的K表示所述主动臂对所述人体肩关节输出的虚拟刚度、当θ为θ2时，求出的K表示所述主动臂对所述人体肘关节输出的虚拟刚度、Kmax1表示所述主动臂提供的最大虚拟刚度、Kmax2表示所述主动臂受到的最大虚拟刚度、Kmin表示所述虚拟刚度的最小值、emax和emin分别为虚拟误差范围的最大值和最小值、e为所述位移差、β为可调节参数。
通过第一角度或第二角度、位移差求得主动臂对每个关节所需要输出的虚拟刚度，即通过虚拟刚度控制第一关节和第二关节所要输出的力，保证主动臂的输出更加平顺，防止输出的力变化过大而拉伤用户，其中，第一关节为第一主动杆与第二主动杆形成的关节，第二关节为第二主动杆与第三主动杆形成的关节。
Kmax1表示主动臂中的电机能提供的最大虚拟刚度，其中，最大虚拟刚度指不会伤害到用户的最大虚拟刚度，若超出此虚拟刚度，则表示主动臂的加速度或辅助力力量过大，拉伤用户。
Kmax2表示用户的胳膊施加给主动臂的最大虚拟刚度，其中，Kmax1与Kmax2的和表示主动臂运动过程中的最大总虚拟刚度，此虚拟刚度用于衡量主动臂运动过程中的平顺程度。
在一实施例中，第一关节的角度即为第一角度，第二关节的角度即为第二角度；第一关节与第二关节上具有电机，通过电机输出扭矩从而控制主动臂进行辅助训练。
由公式
可知，当e大于emax时，K会随之增大，当e小于emin时，K为α，即主动臂不输出力，主动臂由用户完全控制。
基于双侧的误差与人体关节角度推导出的自适应刚度，将两者相结合，从而使标准化单一度量作为自适应系数，以更新主动臂的自适应刚度，帮助主动臂的输出更加平顺。
优选地，当θ为第一角度时，m的取值为-0.0493。
可选地，步骤S500之后，还包括：
判断所述位移差是否大于预设位移差，若是，则进行所述基于所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动的步骤。
当位移差大于预设位移差时，表示用户另一只手臂对于主动臂的控制位移不到位，或角度发生了偏离,需要机械臂辅助干预，则进行步骤S600，主动臂主动进行阻抗控制，输出一个平滑的力，使连接用户另一只手臂的主动臂缓缓移动至期望位置。
当位移差小于预设位移差时，表示用户另一只手臂的力量和控制均满足要求，此时主动臂不进行主动干预，用户此时通过自己的力量控制主动臂。
可选地，如图3所示，步骤S600还包括：
第二动力学数据还包括第二输入方向。
步骤S601，
若所述第二输入方向在所述主动臂与所述从动臂的连接线上的投影朝向所述从动臂，则通过如下公式增大所述阻抗辅助力的干预范围：
步骤S602，
若所述第二输入方向在所述主动臂与所述从动臂的连接线上的投影远离所述从动臂，则通过如下公式减小所述阻抗辅助力的干预范围：
emax＝0.1ln(Fs)+0.1，
其中，Fs为所述第二输入力。
当用户具有对主动臂进行第二输入力的能力时，还需判断此时用户与主动臂的交互力的方向，进一步实现基于交互力改变虚拟误差范围的控制。
通过举例说明，当用户对主动臂施加的力具有一个向外的推力，即用户对主动臂施加的力超过一定数值，但又不能完全将主动臂送入与从动臂镜像的期望位置时，执行步骤S601,通过公式
减小阻抗辅助力的干预，减少对用户的辅助，尽量依靠用户自身的能力，对主动臂送入镜像的期望位置以略微增加将主动臂送入期望位置的难度。
当用户对主动臂施加的力具有一个向内收的拉力时。即用户对主动臂施加的小于一定数值，表示用户的力量不够，则通过公式
emax＝0.1ln(|Fs|)+0.1
增大阻抗辅助力的干预，增大对用户的辅助，以帮助用户更容易将主动臂送入期望位置，提升用户使用的积极性。
另一方面，本发明还提出一种镜像机械臂设备，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上任一项所述的机械臂镜像阻抗控制方法。
所述镜像机械臂设备相对于现有技术与所述机械臂镜像阻抗控制方法所具有的优势相同，在此不再赘述。
虽然本公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术人员在不脱离本公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。
Claims (10)
1.一种机械臂镜像阻抗控制方法，其特征在于，应用于镜像机械臂设备，所述镜像机械臂设备包括主动臂、从动臂、设置在所述从动臂上的第一手柄和设置在所述主动臂上的第二手柄，所述机械臂镜像阻抗控制方法包括：
对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型，用于将手柄相对坐标系与世界坐标系相互转换；
获取对所述从动臂的第一动力学数据，其中，所述第一动力学数据包括第一输入位移；
将所述第一输入位移输入自适应控制器进行响应和跟踪，获得期望数据，其中，所述期望数据包括期望位置，所述期望位置为所述主动臂所需要到达的与所述从动臂互为镜像的位置；
获取对所述主动臂的第二动力学数据，其中，所述第二动力学数据包括第二输入位移；
比对所述期望位置与所述第二输入位移的位移差；
将所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动。
2.根据权利要求1所述的机械臂镜像阻抗控制方法，其特征在于，所述将所述第一输入位移输入自适应控制器进行响应和跟踪，获得期望数据包括：
基于所述第一输入位移进行逆向动力学建模，获得第二动力学模型，逆向求解第一角度和第二角度，其中，所述第二动力学模型表示对人体手臂关节的模拟，所述第一角度表示人体肩关节的外展角度，所述第二角度表示人体肘关节的内收角度。
3.根据权利要求2所述的机械臂镜像阻抗控制方法，其特征在于，所述基于所述第一输入位移进行逆向动力学建模，获得第二动力学模型，逆向求解第一角度和第二角度包括：
对所述第一输入位移进行基于人体上肢的基于三个连杆的DH建模，获得所述第二动力学模型，其中，坐标系原点设置在肩关节上；
基于所述第二动力学模型，获得
其中，θ1表示所述第一角度，θ2表示所述第二角度，(x,y)表示所述第一手柄在笛卡尔坐标系中的位置坐标，d1表示第二连杆相对于第一连杆的偏置，d2表示第三连杆相对于所述第二连杆的偏置，a和b表示所述人体手臂关节经过逆向动力学建模后的模拟量。
4.根据权利要求1所述的机械臂镜像阻抗控制方法，其特征在于，所述对所述主动臂或所述从动臂进行动力学建模，获得第一动力学模型包括：
对所述主动臂或所述从动臂进行DH建模，其中，坐标系原点设置在第一关节臂上。
5.根据权利要求3所述的机械臂镜像阻抗控制方法，其特征在于，所述第二动力学数据还包括第二输入力；所述将所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动包括：
基于所述位移差、所述第一角度和所述第二角度获得所述主动臂的虚拟刚度，其中，所述虚拟刚度随所述位移差、所述第一角度和所述第二角度的变化而变化；
基于所述虚拟刚度和所述第二输入力获得所述阻抗辅助力。
6.根据权利要求5所述的机械臂镜像阻抗控制方法，其特征在于，所述虚拟刚度通过如下公式求得：
其中，当θ为θ1时，求出的K表示所述主动臂对所述人体肩关节输出的虚拟刚度、当θ为θ2时，求出的K表示所述主动臂对所述人体肘关节输出的虚拟刚度、Kmax1表示所述主动臂提供的最大虚拟刚度、Kmax2表示所述主动臂受到的最大虚拟刚度、Kmin表示所述虚拟刚度的最小值、emax和emin分别为虚拟误差范围的最大值和最小值、e为所述位移差、β为可调节参数、和α为中间变量。
7.根据权利要求1所述的机械臂镜像阻抗控制方法，其特征在于，在所述比对所述期望位置与所述第二输入位移的位移差之后，还包括：
判断所述位移差是否大于预设位移差，若是，则进行基于所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动的步骤。
8.根据权利要求6所述的机械臂镜像阻抗控制方法，其特征在于，所述第二动力学数据还包括第二输入方向；所述将所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动包括：
若所述第二输入方向在所述主动臂与所述从动臂的连接线上的投影朝向所述从动臂，则通过如下公式增大所述阻抗辅助力的干预范围：
若所述第二输入方向在所述主动臂与所述从动臂的连接线上的投影远离所述从动臂，则通过如下公式减小所述阻抗辅助力的干预范围：
emax＝0.1ln(|Fs|)+0.1；
其中，Fs为所述第二输入力。
9.根据权利要求7所述的机械臂镜像阻抗控制方法，其特征在于，所述判断所述位移差是否大于预设位移差，若是，则进行所述基于所述位移差、所述期望数据和所述第二动力学数据，结合所述第一动力学模型，控制所述主动臂输出阻抗辅助力，以辅助所述主动臂相对于所述从动臂镜像运动的步骤之后，还包括：
在所述阻抗辅助力中加入重力补偿和摩擦力补偿。
10.一种镜像机械臂设备，其特征在于，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如权利要求1-9任一项所述的机械臂镜像阻抗控制方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111504016.2A CN114700959B (zh) | 2021-12-01 | 2021-12-01 | 一种机械臂镜像阻抗控制方法及镜像机械臂设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111504016.2A CN114700959B (zh) | 2021-12-01 | 2021-12-01 | 一种机械臂镜像阻抗控制方法及镜像机械臂设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114700959A CN114700959A (zh) | 2022-07-05 |
| CN114700959B true CN114700959B (zh) | 2024-01-30 |
Family
ID=82166469
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111504016.2A Active CN114700959B (zh) | 2021-12-01 | 2021-12-01 | 一种机械臂镜像阻抗控制方法及镜像机械臂设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114700959B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108324503A (zh) * | 2018-03-16 | 2018-07-27 | 燕山大学 | 基于肌骨模型和阻抗控制的康复机器人自适应控制方法 |
| CN110355761A (zh) * | 2019-07-15 | 2019-10-22 | 武汉理工大学 | 一种基于关节刚度和肌肉疲劳的康复机器人控制方法 |
| CN110421547A (zh) * | 2019-07-12 | 2019-11-08 | 中南大学 | 一种基于估计动力学模型的双臂机器人协同阻抗控制方法 |
| CN111730599A (zh) * | 2020-07-08 | 2020-10-02 | 深圳市优必选科技股份有限公司 | 阻抗控制方法、装置、阻抗控制器和机器人 |
| CN112891127A (zh) * | 2021-01-14 | 2021-06-04 | 东南大学 | 一种基于自适应阻抗控制的镜像康复训练方法 |
| CN113341728A (zh) * | 2021-06-21 | 2021-09-03 | 长春工业大学 | 一种抗噪型归零神经网络的四轮移动机械臂轨迹跟踪控制方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080009771A1 (en) * | 2006-03-29 | 2008-01-10 | Joel Perry | Exoskeleton |
| JP2012081568A (ja) * | 2010-10-14 | 2012-04-26 | Sony Corp | ロボットの制御装置及び制御方法、並びにコンピューター・プログラム |
-
2021
- 2021-12-01 CN CN202111504016.2A patent/CN114700959B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108324503A (zh) * | 2018-03-16 | 2018-07-27 | 燕山大学 | 基于肌骨模型和阻抗控制的康复机器人自适应控制方法 |
| CN110421547A (zh) * | 2019-07-12 | 2019-11-08 | 中南大学 | 一种基于估计动力学模型的双臂机器人协同阻抗控制方法 |
| CN110355761A (zh) * | 2019-07-15 | 2019-10-22 | 武汉理工大学 | 一种基于关节刚度和肌肉疲劳的康复机器人控制方法 |
| CN111730599A (zh) * | 2020-07-08 | 2020-10-02 | 深圳市优必选科技股份有限公司 | 阻抗控制方法、装置、阻抗控制器和机器人 |
| CN112891127A (zh) * | 2021-01-14 | 2021-06-04 | 东南大学 | 一种基于自适应阻抗控制的镜像康复训练方法 |
| CN113341728A (zh) * | 2021-06-21 | 2021-09-03 | 长春工业大学 | 一种抗噪型归零神经网络的四轮移动机械臂轨迹跟踪控制方法 |
Non-Patent Citations (2)
| Title |
|---|
| 柔性机械臂辅助空间站舱段对接阻抗控制;危清清;刘志全;王耀兵;邵立民;;中国空间科学技术(06);全文 * |
| 面向空间机械臂操作任务的模仿学习策略;李重阳;蒋再男;刘宏;蔡鹤皋;;哈尔滨工业大学学报(06);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114700959A (zh) | 2022-07-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108309689B (zh) | 一种基于外骨骼机器人的渐进式康复训练方法 | |
| CN108430375B (zh) | 机器人系统以及反向驱动该系统的方法 | |
| JP3436320B2 (ja) | 非線形システムの出力軌道と動特性の制御方法および装置 | |
| Noda et al. | Development of an upper limb exoskeleton powered via pneumatic electric hybrid actuators with bowden cable | |
| CN108524187B (zh) | 一种六自由度的上肢康复机器人控制系统 | |
| Rosen et al. | Upper limb powered exoskeleton | |
| CN107050763B (zh) | 一种新型踝关节康复机器人其控制方法 | |
| CN108897220B (zh) | 一种自适应稳定平衡控制方法和系统以及双足仿人机器人 | |
| CN110597072A (zh) | 机器人导纳柔顺控制方法及系统 | |
| Wang et al. | Operator-based robust nonlinear tracking control for a human multi-joint arm-like manipulator with unknown time-varying delays | |
| CN110251898B (zh) | 一种基于力反馈的手腕康复外骨骼的闭环控制系统 | |
| CN109529274B (zh) | 基于冗余机械臂上肢关节主动康复系统及其训练方法 | |
| JP2023552991A (ja) | 複合ロボットの制御方法、装置及びシステム | |
| JP3369351B2 (ja) | 多関節マニピュレータの弾性設定方法および制御装置 | |
| CN112274865A (zh) | 下肢康复机器人的按需辅助自适应控制方法和系统 | |
| CN112237524A (zh) | 柔性上肢康复机器人被动康复训练的控制方法 | |
| CN114700959B (zh) | 一种机械臂镜像阻抗控制方法及镜像机械臂设备 | |
| CN113730178A (zh) | 上肢等速训练康复机器人系统及机器人控制方法 | |
| Wu et al. | Variable admittance time-delay control of an upper limb rehabilitation robot based on human stiffness estimation | |
| Lingampally et al. | A humanoid neck using parallel manipulators | |
| JP3105694B2 (ja) | マニピュレータ制御方法 | |
| Grecu et al. | Analysis of human arm joints and extension of the study to robot manipulator | |
| CN114434452B (zh) | 一种基于势能场的镜像机械臂控制方法及镜像机械臂设备 | |
| CN113967909B (zh) | 基于方向奖励的机械臂智能控制方法 | |
| CN109483551A (zh) | 一种控制机器人多轴运动的方法、装置及系统 |
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 |