CN118143954B - 一种人形机器人上肢双机械臂的柔顺控制方法及装置 - Google Patents

Abstract

本发明涉及一种人形机器人上肢双机械臂的柔顺控制方法及装置，柔顺控制方法，包括：S1.基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对所述上肢双机械臂构建联合动力学模型；S2.对所述上肢双机械臂的双臂协同控制器和协同流程进行设计；S3.以所述上肢双机械臂运行过程中的内力最小为控制目标，基于所述闭链运动学模型、所述联合动力学模型、所述双臂协同控制器和所述协同流程对所述上肢双机械臂进行运动控制。本发明有效地提高了机械臂对方向盘操作的稳定性、鲁棒性和高效性。

Description

一种人形机器人上肢双机械臂的柔顺控制方法及装置

技术领域

本发明涉及机械臂控制方法，尤其涉及一种人形机器人上肢双机械臂的柔顺控制方法及装置。

背景技术

当前绝大多数类型的载具在操纵器的安装设计上依然保留着结实耐用的机械传动机构，并与轮胎保持物理连接。这种做法将轮胎与路面的接触情况实时通过方向盘传递给驾驶员，有效提升了驾驶体验，同时辅助***提供的缓冲裕量与回转力矩也提高了汽车操作性能。然而，这些设计将方向盘控制变得非线性，同时增加了操作任务的复杂度，给机器人实现拟人操作能力带来了很大的挑战。

在方向盘操作任务环境中存在回转力矩以及颠簸带来的不确定扰动，这种情况下需要机器人与环境进行柔***互，避免出现“脱手”状况使得控制不稳定，甚至直接损伤机械臂的情形。然而经典导纳控制在固定操作空间下，往往外力偏差值对应的位置控制变化量极小，在方向盘操作场景中极大地限制了其应用范围。与此同时，拟人机械臂在操作方向盘上是一个动态过程，在保证速度、位置跟踪精度的情况下，还需要维持稳定接触的施力跟踪，这要求对机械臂末端力控与位控完全解耦。

综合来看，双臂操作方向盘要比单臂在稳定性、可靠性上强上许多，一是由于方向盘回正力矩特性的存在，单机械臂操作打满方向盘需要腕部能够灵活翻转的同时持续施力，目前大部分机械臂加灵巧手的协同配合难以做到这一点，二是单机械臂转动方向盘时会出现较为“脆弱”的位形，此时方向盘的扰动及回正力矩可能会对机械臂造成损伤。因此，面向驾驶操作去研究双臂操作方向盘中的协同控制问题是十分必要的。

发明内容

本发明的目的在于提供一种人形机器人上肢双机械臂的柔顺控制方法及装置。

为实现上述发明目的，本发明提供一种人形机器人上肢双机械臂的柔顺控制方法，包括：

S1.基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对所述上肢双机械臂构建联合动力学模型；

S2.对所述上肢双机械臂的双臂协同控制器和协同流程进行设计，其中，所述双臂协同控制器为基于内力一致性的协同控制器，其表示为：

；

；

其中，为控制器输出，为左臂和右臂角速度的修正量，分别为比例、积分系数，为惯性系数，为2×2选择矩阵，代表控制器输出的值控制左臂或者右臂，表示在对操作对象操作的过程中左右臂之间的内力，当时，以减少左臂速率为主，当时，以减少右臂速率为主，表示左臂传感器坐标系下轴向的力，表示右臂传感器坐标系下轴向的力；

所述协同流程包括稳定型协同流程和快速型协同流程；

S3.以所述上肢双机械臂运行过程中的内力最小为控制目标，基于所述闭链运动学模型、所述联合动力学模型、所述双臂协同控制器和所述协同流程对所述上肢双机械臂进行运动控制。

根据本发明的一个方面，步骤S1中，基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型的步骤中，包括：

S11.基于操作对象的外形结构获取所述上肢双机械臂与操作对象的几何关系；

S12.基于所述几何关系构建所述上肢双机械臂的位置约束和速度约束，以完成所述闭链运动学模型的构建。

根据本发明的一个方面，步骤S11中，所述操作对象为方向盘；

步骤S12中，所构建的位置约束表示为：

；

式中，表示所述上肢双机械臂的左臂在基座标系下的位姿矩阵，表示所述上肢双机械臂的右臂在基座标系下的位姿矩阵，表示相对位姿矩阵，且表示为，表示在左臂末端的第一灵巧手坐标系下右臂末端的第二灵巧手的姿态矩阵，表示在第一灵巧手坐标系下第二灵巧手的位置向量，表示误差精度；

所构建的速度约束表示为：

；

其中，表示左臂末端的第一灵巧手的平动速度，表示右臂末端的第二灵巧手的平动速度，表示左臂末端的第一灵巧手的转动速度，表示右臂末端的第二灵巧手的转动速度，表示左臂末端的第一灵巧手与操作对象轴心的转动半径， 表示右臂末端的第二灵巧手与操作对象轴心的转动半径，，分别为操作对象转动半径与转动速度。

根据本发明的一个方面，步骤S1中，对所述上肢双机械臂构建联合动力学模型的步骤中，所述联合动力学模型表示为：

；

；

；

；

；

；

其中，表示左臂左关节力矩， 表示右臂右关节力矩，表示关节电机的加速度矩阵，表示右臂关节的惯性矩阵，表示左臂关节的惯性矩阵，表示右臂关节的科氏力矩阵，表示左臂关节的科氏力矩阵，表示关节电机的速度矩阵，表示右臂关节的重力矩阵，表示左臂关节的重力矩阵，表示右臂雅可比矩阵的逆，表示左臂雅可比矩阵的逆，表示右臂操作矩阵的逆，表示左臂操作矩阵的逆，表示操作对象的回转力矩，表示左臂末端和操作对象接触点的广义力，表示右臂末端和操作对象接触点的广义力，为操作对象的质量，为单位矩阵，为零矩阵，分别为操作对象的质心的速度向量与角速度向量，为目标坐标系下操作对象的转动惯量矩阵，为操作对象的姿态矩阵，为操作对象姿态矩阵的逆，表示基座标系下操作对象的转动惯量矩阵。

根据本发明的一个方面，对所述上肢双机械臂的协同流程进行设计的步骤中，所述稳定型协同流程的设计的步骤，包括：

S021.分别设置左臂和右臂的三种状态，其中，所述左臂的三种状态包括：左臂操作方向盘运动状态、左臂换手运动状态、左臂等待同步状态，所述右臂的三种状态包括：右臂操作方向盘运动状态、右臂等待同步状态、右臂换手运动状态；

S022.基于所述左臂的三种状态构建封闭的左臂运动循环，其中，所述左臂操作方向盘运动状态、所述左臂换手运动状态、所述左臂等待同步状态依次循环的建立连接，以构成所述左臂运动循环；

S023.基于所述右臂的三种状态构建封闭的右臂运动循环，其中，所述右臂操作方向盘运动状态、所述右臂等待同步状态、所述右臂换手运动状态依次循环的建立连接，以构成所述右臂运动循环；

S024.将所述左臂运动循环和所述右臂运动循环进行连接，以对所述左臂运动循环和所述右臂运动循环建立同步流程，其中，将所述左臂等待同步状态和所述右臂等待同步状态建立连接，将所述右臂换手运动状态与所述左臂等待同步状态建立连接，以构建所述左臂运动循环和所述右臂运动循环的左右同步等待循环。

根据本发明的一个方面，所述快速型协同流程的设计的步骤，包括：

S21.分别设置左臂和右臂的四种状态，其中，所述左臂的四种状态包括：第一等待同步状态、操作方向盘运动状态、第二等待同步状态、换手运动状态，所述右臂的四种状态包括：松开方向盘向后运动状态、第三等待同步状态、向前运动抓握方向盘运动状态、第四等待同步状态；

S22.基于所述左臂的四种状态构建封闭的第一运动循环，其中，所述第一等待同步状态、所述操作方向盘运动状态、所述第二等待同步状态、所述换手运动状态依次建立连接，以构成所述第一运动循环；

S23.基于所述右臂的四种状态构建封闭的第二运动循环，其中，所述松开方向盘向后运动状态、所述第三等待同步状态、所述向前运动抓握方向盘运动状态、所述第四等待同步状态依次循环的建立连接，以构成所述第二运动循环；

S24.将所述第一运动循环和所述第二运动循环进行连接，以对所述第一运动循环和所述第二运动循环建立同步流程，其中，将所述第一等待同步状态、所述第四等待同步状态、所述第二等待同步状态和所述第三等待同步状态依次建立连接，以构建所述第一运动循环和所述第二运动循环的同步等待循环。

实现上述发明目的，本发明提供一种采用前述的柔顺控制方法的柔顺控制装置，包括：

模型构建模块，所述模型构建模块基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对所述上肢双机械臂构建联合动力学模型；

控制器构建模块，用于生成所述上肢双机械臂的双臂协同控制器和协同流程，其中，所述双臂协同控制器为基于内力一致性的协同控制器，其表示为：

；

；

其中，为控制器输出，为左臂和右臂角速度的修正量，分别为比例、积分系数，为惯性系数，为2×2选择矩阵，代表控制器输出的值控制左臂或者右臂，表示在对操作对象操作的过程中左右臂之间的内力，当时，以减少左臂速率为主，当时，以减少右臂速率为主，表示左臂传感器坐标系下轴向的力，表示右臂传感器坐标系下轴向的力；

所述协同流程包括稳定型协同流程和快速型协同流程；

输出控制模块，所述输出控制模块以所述上肢双机械臂运行过程中的内力最小为控制目标，基于所述闭链运动学模型、所述联合动力学模型、所述双臂协同控制器和所述协同流程对所述上肢双机械臂进行运动控制。

根据本发明的一种方案，本发明针对双臂协同操作方向盘这一任务进行了探索，提出的方案在实验环境中证实了有效性与鲁棒性。由于协同流程的拟人化操作（操作方向盘外圈），可以适应多种机械臂末端执行器，无需专门定制。首次在方向盘操作任务中引入导纳控制这一概念，具有创新性，结合视觉定位、模式识别等智能算法，能够应用于多种不同驾驶场景。

根据本发明的一种方案，本发明通过对双臂协同问题进行建模，分析了双臂同时操作方向盘的运动学约束，并从动力学角度分析方向盘回转特性在闭链***中对双臂造成的影响；此外，为了减少双臂操作过程中的额外能量损耗，结合选择矩阵与单积分动力学控制器的思想，设计了调节左右臂运动角速率的协同控制器，达到操作过程中间接消减内力的目的；在此基础上，通过设计实验，在操作过程中对方向盘施加扰动证明了基于内力调节的协同控制方法具有一定鲁棒性。

根据本发明的一种方案，本发明通过对方向盘转动速度分解为左右臂姿态运动角速度，在引入自适应导纳控制器实现末端柔顺控制的基础上，设计可以间接调节双臂操作过程中内力的协同控制器，通过方向盘单周运动区间下双臂协同操作实验对此进行了验证；结合了自适应导纳控制器、内力一致协同控制器的柔顺控制方法在双臂操作方向盘上也可以较好地应用，对比单臂的方向盘操控鲁棒性更强。

根据本发明的一种方案，本发明基于内力调节的双臂协同控制方案较好地完成了驾驶操作中方向盘控制目标，且提出的两种协同流程中，双臂同时抓握方向盘运动的稳定控制流程较从臂辅助主臂操作方向盘进行配合控制流程的抗扰性更强，但转动效率更低，二者各有优劣。实际应用中，面向不同的使用场景应采用相应的控制流程，提高机械臂对方向盘操作的稳定性、鲁棒性和高效性。

附图说明

图1是示意性表示根据本发明的一种实施方式的柔顺控制方法的步骤框图；

图2是示意性表示根据本发明的一种实施方式的上肢双机械臂握紧操作对象的运动模型图；

图3是示意性表示根据本发明的一种实施方式的上肢双机械臂运动和力的映射关系图；

图4是示意性表示根据本发明的一种实施方式的方向盘操作中双臂内力示意图，其中，（a）表示对方向盘操作影响较弱的内力，(b)表示方向盘操作中左右臂的内力图；

图5是示意性表示根据本发明的一种实施方式的稳定型上肢双机械臂的协同流程图；

图6是示意性表示根据本发明的一种实施方式的快速型上肢双机械臂的协同流程图；

图7是示意性表示实施例1中上肢双机械臂操作方向盘运动时单周转动操作区间内方向盘转角变化曲线图；

图8是示意性表示实施例1中上肢双机械臂操作方向盘的内力曲线图，其中，（a）表示仅ACC控制器且未加协同控制器时双臂操作方向盘的内力曲线，（b）表示ACC控制器且使用协同控制器时双臂操作方向盘的内力曲线；

图9是示意性表示实施例1中施加扰动时上肢双机械臂操作方向盘的内力曲线；

图10是示意性表示实施例2中稳定型协同流程单个周期左右臂运动实验结果，其中，（a）表示左臂7关节坐标系下X轴向转角，（b）表示右臂7关节坐标系下X轴向转角；

图11是示意性表示实施例2中快速型协同流程单个周期左右臂运动实验结果，其中，（a）表示左臂7关节坐标系下X轴向转角，（b）表示右臂基座坐标系下Z轴位移。

具体实施方式

为了更清楚地说明本发明实施方式或现有技术中的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施方式，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

在针对本发明的实施方式进行描述时，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”所表达的方位或位置关系是基于相关附图所示的方位或位置关系，其仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此上述术语不能理解为对本发明的限制。

下面结合附图和具体实施方式对本发明作详细地描述，实施方式不能在此一一赘述，但本发明的实施方式并不因此限定于以下实施方式。

如图1所示，根据本发明的一种实施方式，本发明的一种人形机器人上肢双机械臂的柔顺控制方法，包括：

S1.基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对上肢双机械臂构建联合动力学模型；

S2.对上肢双机械臂的双臂协同控制器和协同流程进行设计；

S3.以上肢双机械臂运行过程中的内力最小为控制目标，基于闭链运动学模型、联合动力学模型、双臂协同控制器和协同流程对上肢双机械臂进行运动控制。

根据本发明的一种实施方式，步骤S1中，基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型的步骤中，包括：

S11.基于操作对象的外形结构获取上肢双机械臂与操作对象的几何关系；

S12.基于几何关系构建上肢双机械臂的位置约束和速度约束，以完成闭链运动学模型的构建。

在本实施方式中，以操作对象为方向盘为例进行说明，参见图2所示，在左右双臂共有基座标系下，基于双臂握紧操作对象的运动模型，考虑从正对方向抓握方向盘后开始运动，左、右臂与操作对象作为一个整体构成封闭运动链，在双臂灵巧手与操作对象之间不发生滑动的情况下，需要左、右臂初始位置对称且跟踪期望轨迹的运动同步进行，同时左、右臂的相对位置与速度满足一定的约束条件。

进而，根据图中几何关系，位置约束可以通过左右臂末端坐标系的相对姿态进行描述，即在协同操作方向盘的过程中，左右臂相对位姿矩阵在切空间下保持不变，进而即可将其位置约束用以下公式表示：

；

式中，表示所述上肢双机械臂的左臂在基座标系下的位姿矩阵，表示所述上肢双机械臂的右臂在基座标系下的位姿矩阵，表示相对位姿矩阵，且表示为，表示在左臂末端的第一灵巧手坐标系下右臂末端的第二灵巧手的姿态矩阵，表示在第一灵巧手坐标系下第二灵巧手的位置向量，表示误差精度（其以极小正数表示，例如，误差精度可设置在0.01rad/s或者0.01m/s以内，则的取值可以为0.01）；

进一步的，为便于问题的分析，设定操作对象的安装平面正对安装上肢双机械臂的机器人本体，进而可获得相应的速度约束为：

；

其中，表示左臂末端的第一灵巧手的平动速度，表示右臂末端的第二灵巧手的平动速度，表示左臂末端的第一灵巧手的转动速度，表示右臂末端的第二灵巧手的转动速度，表示左臂末端的第一灵巧手与操作对象轴心的转动半径， 表示右臂末端的第二灵巧手与操作对象轴心的转动半径，，分别为操作对象转动半径与转动速度。

需要说明的是，本发明中的第一灵巧手和第二灵巧手均采用Inspire-RHF-6-BXF灵巧手来实现抓握动作，其包含6个运动自由度，拇指2个自由度，其他四指各有1个自由度。本发明中上肢双机械臂中的左臂和右臂分别为7自由度拟人机械臂，其中，肩关节设置有3个电机，肘关节设置有一个电机，且分别配置额定扭矩为 22.91Nm 的行星减速机构执行器QDD-PR60-36，腕关节采用3个电机且分别配置额定扭矩为 6.5Nm 的行星减速机构执行器QDD-PR30-36。

根据本发明的一种实施方式，由雅可比矩阵可知，机械臂在关节空间速度向量与笛卡尔空间末端运动速度之间的一种线性映射关系，与此同时，基于雅可比矩阵的转置可知笛卡尔空间末端受到的力与反映到机械臂关节空间力矩的一种线性映射关系。类似的，以机械臂操作对象为参考，定义目标物体存在一种映射矩阵，可描述物体质心速度与物体接触机械臂末端点的广义速度之间的线性映射关系，同时，其转置可描述物体接触机械臂末端点受力与质心系下受力之间的一种线性关系，为下文表述方便，记Q为操作对象的操作矩阵。

如图3所示，基于上肢双机械臂运动和力的映射关系，直观的获得了关节空间、笛卡尔空间、操作空间下运动和力的映射关系。

进而，操作对象质心处的广义速度与上肢双机械臂末端和操作对象接触点的广义速度之间关系可以表示为：

；

操作对象质心处广义力与上肢双机械臂末端和操作对象接触点的广义力之间关系可以表示为：

；

由此，操作对象的动力学方程即可描述为：

；

式中，

；

；

；

，且为操作目标质量，为重力向量，分别为物体质心的速度向量与角速度向量，为单位矩阵，I为转动惯量矩阵，为环境作用在操作对象上的6维广义力向量，为机械臂末端作用于操作物体的6维广义力向量。

进一步的，基于前述的闭链运动学模型，将操作对象的动力学方程进行改写，其表示为：

；

由此，即可获得上肢双机械臂中左臂和右臂的联合动力学模型，其表示为：

；

；

；

；

；

；

其中，表示左臂左关节力矩， 表示右臂右关节力矩，表示关节电机的加速度矩阵（7×1），表示右臂关节的惯性矩阵（7×7），表示左臂关节的惯性矩阵（7×7），表示右臂关节的科氏力矩阵（7×7），表示左臂关节的科氏力矩阵（7×7），表示关节电机的速度矩阵（7×1），表示右臂关节的重力矩阵，表示左臂关节的重力矩阵，表示右臂雅可比矩阵的逆，表示左臂雅可比矩阵的逆，表示右臂操作矩阵的逆，表示左臂操作矩阵的逆，表示操作对象的回转力矩，表示左臂末端和操作对象接触点的广义力，表示右臂末端和操作对象接触点的广义力，下标表示机械臂上电机（或关节）的序号，为操作对象的质量，为单位矩阵，为零矩阵，分别为操作对象的质心的速度向量与角速度向量，为目标坐标系下操作对象的转动惯量矩阵，为操作对象的姿态矩阵，为操作对象姿态矩阵的逆，表示基座标系下操作对象的转动惯量矩阵。

根据本发明的一种实施方式，步骤S2中，对上肢双机械臂的双臂协同控制器和协同流程进行设计的步骤中，双臂协同控制器为基于内力一致性的协同控制器。

在本实施方式中，针对操作对象的操作过程中，上肢双机械臂相互作用力也可以分为操作力与内力。其中，操作力为克服回正力矩以及扰动时双臂施加的方向盘切向力，而内力分为两种情况，分别为对方向盘操作影响较弱的内力和方向盘操作中左右臂的内力，参见图4。

进而可知，上肢双机械臂在匀速转动过程中，理想状态下作用力应符合：

；

式中为操作对象（方向盘）的回正力矩，与分别为左右臂传感器坐标系下轴向的力。实际操作过程中，仅靠双臂各自的初始位姿与轨迹跟踪精度，无法实现严格等于。于是以左臂为基准定义方向盘操作过程中的内力为：

_；

当时表示左臂对右臂转动产生的额外作用力，当时表示右臂对左臂转动产生额外作用力。无论哪种情况，都说明双臂机器人在操作方向盘过程中施力不协调，产生了额外损耗。为减小内力造成的额外损耗，考虑从三个方面实现。一是减少机械臂操作方向盘过程中的控制扰动，由于本文中双臂平台为自行设计，通过铝型材、金属加工件以及3D打印件连接，从平台层面减少电机动作期间的扰动较为困难，且效果有限；二是提高左右臂之间的同步效果，虽然双臂运动通过同一计算机控制，目标轨迹可以做到严格同步对称，但由于电机齿轮结构与光电编码器精度限制，双臂共14个电机的初始零位难以一一相互对准，最终导致目标轨迹的相对偏移；三是提高机械臂的控制精度，由于电机内部三环控制参数可调范围有限，且为了确保安全，出厂设置了非线性环节，同时结合前面两点限制，难以通过改进底层控制器提高机械臂末端的控制精度。

于是考虑到内力产生直接原因是左右臂转动速率不一致，受多机器人协同研究领域中的一致控制方法启发，为减小方向盘操作过程中的内力，进一步设计了一种基于一致控制方法的协同控制器。参考单积分动力学的一致性跟踪算法，该协同控制器目标为控制左右臂运动角速率保持一致，进而实现左右臂力跟踪的一致性，则相应的双臂协同控制器（控制律）表示为：

；

_；

其中，为控制器输出，为左臂和右臂角速度的修正量，分别为比例、积分系数，为惯性系数，为2×2选择矩阵，代表控制器输出的值（角速率值）控制左臂或者右臂，表示在对操作对象操作的过程中左右臂之间的内力，其中，为避免控制器过度消除内力致使操作方向盘转动速率过快，则当时，以减少左臂速率为主（例如，反馈给左臂减速，反馈给右臂加速），当时，以减少右臂速率为主（反馈给右臂减速，反馈给左臂加速），以确保安全，表示左臂传感器坐标系下轴向的力，表示右臂传感器坐标系下轴向的力。

根据本发明的一种实施方式，基于前述对双臂协同控制器的设计，进而可获得其相应的协同控制流程，具体的，左右臂在末端执行器的姿态运动过程中，其通过内力的计算以获取双臂协同控制器反馈的运动角速度控制量。在本实施方式中，单臂控制流程为上位计算机解算笛卡尔空间下的运动参数，经过笛卡尔空间下运动柔顺控制转化为单个电机的控制指令经CAN总线发送给电机驱动器，单个机械臂的各个电机接收指令进行动作。进一步的，在运动过程中，双臂协同控制器接收左右臂末端力传感器信号计算内力，结合选择矩阵转换为运动参数控制修正量，反馈给左右臂，实现运动过程中的内力调节机制。该协同控制流程为最终对机械臂进行控制流程的一部分。结合正对方向盘压力的自适应导纳控制，最终实现机械臂柔顺操作方向盘的整个控制流程。

在本实施方式中，在仅使用力传感器的有限感知场景中，从上肢双机械臂抓握方向盘开始，操作方向盘过程中需要左右臂交替配合才能完成全行程操作，因此需要设计协同控制流程确保双臂操作过程中的动作同步。由于主要操作阶段需要双臂同时无滑动地操作方向盘，设计算法时需要考虑以下几点：

(1)左右臂运动是镜像对称的，故每次“换手”需要运动到相应的对称位置上，以提供更大范围的单次操作行程；

(2)由于方向盘的回正特性，在每次“换手”时需要左右臂根据同步信号依次进行动作，到位后同时开始运动；

(3)基于考虑（2），左右臂在依次“换手”中需要为对侧留出一定空间，单次行程的极限不能大于（实验条件测得），否则灵巧手会出现碰撞。

在本实施方式中，对上肢双机械臂的协同流程进行设计的步骤中，协同流程包括稳定型协同流程和快速型协同流程；其中，稳定型协同流程的设计的步骤，包括：

S021.分别设置左臂和右臂的三种状态，如图5所示，为实现左右臂同步动作，基于有限状态机理论，设置左臂和右臂的三种状态，其中，左臂的三种状态包括：左臂操作方向盘运动状态、左臂换手运动状态、左臂等待同步状态，所述右臂的三种状态包括：右臂操作方向盘运动状态、右臂等待同步状态、右臂换手运动状态；“操作方向盘运动状态”动作为抓住方向盘无相对滑动地进行转动，“换手运动状态”动作为松开方向盘向相反方向运动至对称位置并抓握方向盘，“等待同步状态”动作为静止，接收对侧臂的同步信号。

S022.基于左臂的三种状态构建封闭的左臂运动循环，其中，左臂操作方向盘运动状态、左臂换手运动状态、左臂等待同步状态依次循环的建立连接，以构成左臂运动循环；

S023.基于右臂的三种状态构建封闭的右臂运动循环，其中，右臂操作方向盘运动状态、右臂等待同步状态、右臂换手运动状态依次循环的建立连接，以构成右臂运动循环；

S024.将左臂运动循环和右臂运动循环进行连接，以对左臂运动循环和右臂运动循环建立同步流程，其中，将左臂等待同步状态和所述右臂等待同步状态建立连接，将右臂换手运动状态与左臂等待同步状态建立连接，以构建左臂运动循环和右臂运动循环的左右同步等待循环。其中，左右臂协同动作期间各状态转换图如图5所示，其中虚线为状态转换的同步触发信号，且左臂从“等待同步状态”转移至“操作方向盘运动状态”需要右臂进入“操作方向盘运动状态”的同步信号，右臂从“等待同步状态”转移至“操作方向盘运动状态”需要等待左臂进入“等待同步状态”时发送的同步信号。

由此看出，双臂协同的“稳定型”算法中，等待状态持续的时间为左右臂各“换手”一次所需的时间之和，确保了操作稳定性但整体不够流畅。

如图6所示，在本实施方式中，采用基于单臂操作方向盘运动的方式在稳定型协同流程的基础上设计快速型协同流程，其具体包括：

S21.分别设置左臂和右臂的四种状态，其中，左臂的四种状态包括：第一等待同步状态、操作方向盘运动状态、第二等待同步状态、换手运动状态，右臂的四种状态包括：松开方向盘向后运动状态、第三等待同步状态、向前运动抓握方向盘运动状态、第四等待同步状态；

S22.基于左臂的四种状态构建封闭的第一运动循环，其中，第一等待同步状态、操作方向盘运动状态、第二等待同步状态、换手运动状态依次建立连接，以构成第一运动循环；

S23.基于右臂的四种状态构建封闭的第二运动循环，其中，松开方向盘向后运动状态、第三等待同步状态、向前运动抓握方向盘运动状态、第四等待同步状态依次循环的建立连接，以构成第二运动循环；其中，松开方向盘向后运动状态的过程为松开方向盘向后运动指定距离后停止，向前运动抓握方向盘运动状态的过程为张开灵巧手向前运动至接触方向盘外圈后抓握住方向盘；

S24.将第一运动循环和第二运动循环进行连接，以对第一运动循环和第二运动循环建立同步流程，其中，将第一等待同步状态、第四等待同步状态、第二等待同步状态和第三等待同步状态依次建立连接，以构建第一运动循环和第二运动循环的同步等待循环。

在本实施方式中，左臂从第一等待同步状态转移至对应的操作方向盘运动状态的同时，需要右臂进入对应的第四等待同步状态的同步信号，左臂从第二等待同步状态转移至对应的换手运动状态的同时，需要右臂进入对应的第三等待同步状态的同步信号，相应的，右臂从第三等待同步状态转移至对应的向前运动抓握方向盘运动状态的同时，需要左臂进入对应的第一等待同步状态的同步信号，右臂从第四等待同步状态转移至对应的松开方向盘向后运动状态的同时，需要左臂进入对应的第二等待同步状态的同步信号。

通过上述设置的协同流程，避免了上肢双机械臂镜像对称运动带来的对称位置限制，并增加了单次行程的运动极限范围，同时等待状态持续时间为驱动臂（左臂）“换手”一次所需的时间与辅助臂（右臂）抓握行程时间之和，提升了转动效率。

根据本发明的一种实施方式，本发明的一种用于前述的柔顺控制方法的柔顺控制装置，包括：

模型构建模块，模型构建模块基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对上肢双机械臂构建联合动力学模型；

控制器构建模块，用于生成上肢双机械臂的双臂协同控制器和协同流程；

输出控制模块，输出控制模块以上肢双机械臂运行过程中的内力最小为控制目标，基于闭链运动学模型、联合动力学模型、双臂协同控制器和协同流程对上肢双机械臂进行运动控制。

为进一步验证本方案，对其作进一步举例说明。

实施例1

在本实施方式中，采用方向盘单周转动区间内的协同控制进行实验验证。具体的，对方向盘的单周转动区间定义为，对转动区域进行设定可方便下一步重点研究所设计的协同控制器在双臂同时操作方向盘时的控制效果。其中，实验设置左右臂末端圆周运动半径为0.142m，转动速度为，先顺时针旋转80°，再逆时针旋转160°，再顺时针旋转80°回到原位，方向盘灵敏度设置为50，力反馈强度调节为10，转动范围为900°。双臂操作方向盘运动的示意图如图7所示。

未使用协同控制器时，由于左右臂实际运行速率不完全一致，导致相互作用产生内力，实验结果的内力数据如图8（a）所示。顺时针运动到80°过程中，内力趋向于减小，至-4N附近，在切换运动方向时产生极小时间的停顿，由于惯性作用内力有明显跳变，最大至1.24N；逆时针运动到0°过程中内力趋向于-4N，同时由于回正力矩的减小，左右臂驱动力减小，内力趋向于0；逆时针经过0°附近时，回正力矩减小至0，内力趋向于增加，至-3.78N，双臂内力作用由右臂对左臂产生作用力变为左臂对右臂产生作用力；并在-80°处切换运动方向时出现了相同的内力跳变，直至最终运动到初始0°位置。

使用协同控制器后，实验结果的内力数据如图8(b)所示。前5s内，由于双臂协同不稳定，内力出现先上升至最大值5.69N，后下降的趋势，逐渐稳定至0附近，之后随着方向盘运动速度的改变，内力始终维持在之间，首次稳定后内力均值为0.121N，协同控制器效果明显。

使用协同控制器后，施加外力干扰，实验结果的内力数据如图9所示。其中虚线框区域为外力干扰作用区间，其对内力影响较明显地显示在图像中，协同控制器对于随时间改变的内力作用控制效果不佳，但撤销外力后能较迅速地恢复到之前的状态，将内力控制在之间。其中撤销外力后恢复时间为0.27s，对比图中从扰动中的恢复时间，表明双臂操作方向盘具有更好的稳定性。

实施例2

在本实施方式中，采用方向盘全行程协同控制进行实验验证。具体的，拟人驾驶操作中无法避开方向盘全行程操作这一问题，为了节约驾驶操作成本，提升驾驶操作体验，方向盘被设计为同向转动一圈半至两圈半才能驱动转向机构到最大转角。罗技G920DrivingForce方向盘设置转动范围为，同向行程极限为一圈加90°。由于手臂的结构限制以及方向盘回正特性，拟人驾驶操作方向盘过程中需要双臂配合进行“换手”才能实现单向的全行程控制，这也是协同操作方向盘问题的一个难点。

在前文工作基础上，通过设计实验验证了两种方向盘全行程控制流程，分别为以双臂抓握方向盘同时运动为主进行稳定控制的流程，以及从臂（右臂）辅助主臂（左臂）操作方向盘进行快速控制的流程。前者控制方向盘转动时依靠双臂配合同时进行转动，左右臂依次进行“换手”动作，注重操作过程的稳定性，后者控制方向盘转动时依靠主臂带动方向盘运动，在主臂“换手”时，从臂配合固定住方向盘克服回正力矩，注重操作过程的快速型。

双臂稳定控制的全行程实验设置左右臂末端圆周运动半径为0.142m，转动速度为，为双臂“换手”留出空间故先双臂同时顺时针旋转45°，左右臂依次“换手”至方向盘上下对称的另一侧位置，双臂再继续顺时针旋转90°，依次进行至满行程450°。罗技G920DrivingForce方向盘灵敏度设置为50，力反馈强度调节为10，转动范围为900°。

双臂快速控制全行程实验设置左右臂末端圆周运动半径为0.142m，转动速度为，主臂先顺时针旋转45°，待从臂配合将方向盘固定住后，主臂再“换手”至方向盘上下对称的另一侧位置，随后从臂松开方向盘，主臂操作方向盘顺时针旋转125°,依次进行至满行程450°。G920方向盘灵敏度设置为50，力反馈强度调节为10，转动范围为900°。其中，主臂为左臂。

在实验场景下，两种协同流程均完成了方向盘的全行程操作，取单个运动周期内左右臂的运动曲线图，稳定型协同流程与快速型协同流程实验结果分别如图10与图11所示。

其中，在稳定型协同流程中，左右臂在1.96s之前同时顺时针转动方向盘至45°位置，随后左臂松手离开方向盘（1.96s~3.68s），逆时针运动90°（3.68s~7.51s），前进抓住方向盘（7.51s~9.82s），进入“等待同步状态”，完成“换手”动作，期间右臂保持“等待同步状态”。接下来右臂以相同方式进行“换手”动作，松手后退（9.82s~11.55s），逆时针运动（11.55s~15.88s），前进抓握方向盘（15.88s~17.72s），然后发出同步信号，左右臂同时开始操作方向盘继续旋转90°（17.72s~21.64s）进入下一个操作方向盘运动周期。

在快速型协同流程中，左臂在前3.27s等待右臂松手后退，随后右臂进入“等待同步状态”，左臂顺时针转动方向盘至80°位置（3.27s~6.95s），等待右臂到位（6.95s~9.63s）后开始“换手”。左臂经过松手后退（9.63s~11.88s），逆时针运动125°（11.88s~17.33s），前进抓握方向盘（17.33s~19.08s），完成“换手”动作，后等待右臂松手后退（19.08s~22.41s）进入下一个操作方向盘运动周期。

经过实验验证，在设置相同的操作速度下，采用稳定型协同流程控制全行程操作花费时间102.4s，采用快速型协同流程控制全行程操作花费时间69.4s，时间缩短32.2%。同时从图中可以看出，在机械臂每一次接触到方向盘后，均有额外运动产生，这是由于方向盘的回正力矩在机械臂末端导纳控制器作用下的现象，其中，稳定型协同流程左右臂的恢复转动角度的平均调节时间为0.83s，而快速型协同流程的平均调节时间为1.52s。由此可见，稳定型协同流程在控制效果上要强于快速型协同流程，但在操作效率上要弱于“快速型”协同流程。

上述内容仅为本发明的具体方案的例子，对于其中未详尽描述的设备和结构，应当理解为采取本领域已有的通用设备及通用方法来予以实施。

以上所述仅为本发明的一个方案而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种人形机器人上肢双机械臂的柔顺控制方法，其特征在于，包括：

S1.基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对所述上肢双机械臂构建联合动力学模型；

S2.对所述上肢双机械臂的双臂协同控制器和协同流程进行设计，其中，所述双臂协同控制器为基于内力一致性的协同控制器，其表示为：

；

；

其中，为控制器输出，为左臂和右臂角速度的修正量，、分别为比例、积分系数，为惯性系数，为2×2选择矩阵，代表控制器输出的值控制左臂或者右臂，表示在对操作对象操作的过程中左右臂之间的内力，当时，以减少左臂速率为主，当时，以减少右臂速率为主，表示左臂传感器坐标系下轴向的力，表示右臂传感器坐标系下轴向的力；

所述协同流程包括稳定型协同流程和快速型协同流程；

S3.以所述上肢双机械臂运行过程中的内力最小为控制目标，基于所述闭链运动学模型、所述联合动力学模型、所述双臂协同控制器和所述协同流程对所述上肢双机械臂进行运动控制。

2.根据权利要求1所述的柔顺控制方法，其特征在于，步骤S1中，基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型的步骤中，包括：

S11.基于操作对象的外形结构获取所述上肢双机械臂与操作对象的几何关系；

S12.基于所述几何关系构建所述上肢双机械臂的位置约束和速度约束，以完成所述闭链运动学模型的构建。

3.根据权利要求2所述的柔顺控制方法，其特征在于，步骤S11中，所述操作对象为方向盘；

步骤S12中，所构建的位置约束表示为：

；

式中，表示所述上肢双机械臂的左臂在基座标系下的位姿矩阵，表示所述上肢双机械臂的右臂在基座标系下的位姿矩阵，表示相对位姿矩阵，且表示为，表示在左臂末端的第一灵巧手坐标系下右臂末端的第二灵巧手的姿态矩阵，表示在第一灵巧手坐标系下第二灵巧手的位置向量，表示误差精度；

所构建的速度约束表示为：

；

其中，表示左臂末端的第一灵巧手的平动速度，表示右臂末端的第二灵巧手的平动速度，表示左臂末端的第一灵巧手的转动速度，表示右臂末端的第二灵巧手的转动速度，表示左臂末端的第一灵巧手与操作对象轴心的转动半径， 表示右臂末端的第二灵巧手与操作对象轴心的转动半径，，分别为操作对象转动半径与转动速度。

4.根据权利要求3所述的柔顺控制方法，其特征在于，步骤S1中，对所述上肢双机械臂构建联合动力学模型的步骤中，所述联合动力学模型表示为：

；

；

；

；

；

；

其中，表示左臂左关节力矩， 表示右臂右关节力矩，表示关节电机的加速度矩阵，表示右臂关节的惯性矩阵，表示左臂关节的惯性矩阵，表示右臂关节的科氏力矩阵，表示左臂关节的科氏力矩阵，表示关节电机的速度矩阵，表示右臂关节的重力矩阵，表示左臂关节的重力矩阵，表示右臂雅可比矩阵的逆，表示左臂雅可比矩阵的逆，表示右臂操作矩阵的逆，表示左臂操作矩阵的逆，表示操作对象的回转力矩，表示左臂末端和操作对象接触点的广义力，表示右臂末端和操作对象接触点的广义力，为操作对象的质量，为单位矩阵，为零矩阵，分别为操作对象的质心的速度向量与角速度向量，为目标坐标系下操作对象的转动惯量矩阵，为操作对象的姿态矩阵，为操作对象姿态矩阵的逆，表示基座标系下操作对象的转动惯量矩阵。

5.根据权利要求4所述的柔顺控制方法，其特征在于，对所述上肢双机械臂的协同流程进行设计的步骤中，所述稳定型协同流程的设计的步骤，包括：

S021.分别设置左臂和右臂的三种状态，其中，所述左臂的三种状态包括：左臂操作方向盘运动状态、左臂换手运动状态、左臂等待同步状态，所述右臂的三种状态包括：右臂操作方向盘运动状态、右臂等待同步状态、右臂换手运动状态；

S022.基于所述左臂的三种状态构建封闭的左臂运动循环，其中，所述左臂操作方向盘运动状态、所述左臂换手运动状态、所述左臂等待同步状态依次循环的建立连接，以构成所述左臂运动循环；

S023.基于所述右臂的三种状态构建封闭的右臂运动循环，其中，所述右臂操作方向盘运动状态、所述右臂等待同步状态、所述右臂换手运动状态依次循环的建立连接，以构成所述右臂运动循环；

S024.将所述左臂运动循环和所述右臂运动循环进行连接，以对所述左臂运动循环和所述右臂运动循环建立同步流程，其中，将所述左臂等待同步状态和所述右臂等待同步状态建立连接，将所述右臂换手运动状态与所述左臂等待同步状态建立连接，以构建所述左臂运动循环和所述右臂运动循环的左右同步等待循环。

6.根据权利要求5所述的柔顺控制方法，其特征在于，所述快速型协同流程的设计的步骤，包括：

S21.分别设置左臂和右臂的四种状态，其中，所述左臂的四种状态包括：第一等待同步状态、操作方向盘运动状态、第二等待同步状态、换手运动状态，所述右臂的四种状态包括：松开方向盘向后运动状态、第三等待同步状态、向前运动抓握方向盘运动状态、第四等待同步状态；

S22.基于所述左臂的四种状态构建封闭的第一运动循环，其中，所述第一等待同步状态、所述操作方向盘运动状态、所述第二等待同步状态、所述换手运动状态依次建立连接，以构成所述第一运动循环；

S23.基于所述右臂的四种状态构建封闭的第二运动循环，其中，所述松开方向盘向后运动状态、所述第三等待同步状态、所述向前运动抓握方向盘运动状态、所述第四等待同步状态依次循环的建立连接，以构成所述第二运动循环；

S24.将所述第一运动循环和所述第二运动循环进行连接，以对所述第一运动循环和所述第二运动循环建立同步流程，其中，将所述第一等待同步状态、所述第四等待同步状态、所述第二等待同步状态和所述第三等待同步状态依次建立连接，以构建所述第一运动循环和所述第二运动循环的同步等待循环。

7.一种采用权利要求1至6任一项所述的柔顺控制方法的柔顺控制装置，其特征在于，包括：

模型构建模块，所述模型构建模块基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对所述上肢双机械臂构建联合动力学模型；

控制器构建模块，用于生成所述上肢双机械臂的双臂协同控制器和协同流程，其中，所述双臂协同控制器为基于内力一致性的协同控制器，其表示为：

；

；

其中，为控制器输出，为左臂和右臂角速度的修正量，、分别为比例、积分系数，为惯性系数，为2×2选择矩阵，代表控制器输出的值控制左臂或者右臂，表示在对操作对象操作的过程中左右臂之间的内力，当时，以减少左臂速率为主，当时，以减少右臂速率为主，表示左臂传感器坐标系下轴向的力，表示右臂传感器坐标系下轴向的力；

所述协同流程包括稳定型协同流程和快速型协同流程；

输出控制模块，所述输出控制模块以所述上肢双机械臂运行过程中的内力最小为控制目标，基于所述闭链运动学模型、所述联合动力学模型、所述双臂协同控制器和所述协同流程对所述上肢双机械臂进行运动控制。