一种人形机器人多模组的协同控制方法与流程

本发明涉及人形机器人运动控制，尤其涉及一种人形机器人多模组的协同控制方法。

背景技术：

1、人形机器人具有双腿、双臂、腰部、头部等多个运动模组。现有人形机器人协同控制方法主要分为3类。

2、第1类为独立模组控制方法，将各模组分别视为独立机械臂或腿，分别设计关节空间位置控制器与力控制器，通过上层运动规划器给定各模组的期望轨迹。该方法结构简单，但忽略了躯干与各模组之间的动态耦合。当人形机器人执行双臂协同搬运与双腿动态行走同时进行的任务时，躯干的加速运动会通过惯性耦合传递至双臂末端，导致末端执行器接触力产生幅度较大的振荡；双臂的运动也会改变整体质心分布，使双腿足底接触力分配偏离期望值。

3、第2类为全身任务空间控制方法，将各模组的任务统一描述在任务空间中，利用零空间投影实现多个任务的优先级分配。该方法能处理任务级冗余，但其采用的动力学模型通常将耦合项作为整体扰动进行在线估计或补偿，未对模组间的动态耦合进行结构化建模与前馈补偿，在高动态任务中补偿滞后明显。

4、第3类为全身模型预测控制方法，利用有限时域优化直接求解关节力矩与接触力，可显式处理约束，但优化问题维度高，在线求解耗时较大，难以在嵌入式硬件上以高于100赫兹频率运行，且对动态耦合项的处理高度依赖模型精度，模型误差会导致控制性能下降。上述方法均未在同一控制框架内同时实现模组间动态耦合的结构化前馈补偿与多任务分层约束优化，导致人形机器人在多模组协同操作时末端力控精度、躯干姿态稳定性与接触约束满足程度难以兼顾。

5、因此需要一种能够在高动态工况下显式补偿模组间惯性耦合、同时严格满足接触约束和任务优先级的人形机器人多模组协同控制方法。

技术实现思路

1、本发明要解决的技术问题是：现有人形机器人多模组协同控制方法难以同时补偿模组间动态耦合和满足多任务分层约束，导致协同操作末端力控精度低、姿态稳定性差。

2、本发明提供一种人形机器人多模组的协同控制方法，包括以下步骤：

3、步骤1：采集人形机器人各模组的状态数据，所述状态数据包括所有转动关节的关节角度、关节角速度和关节角加速度，双臂末端执行器的六维接触力与接触力矩，双足足底接触力与接触力矩，躯干姿态四元数、躯干三轴角速度和躯干三轴线加速度；

4、步骤2：根据所述状态数据构建人形机器人浮基全身动力学模型，在线辨识模组间动态耦合张量；

5、步骤3：建立多模组协同任务的分层优先级与任务误差，所述分层优先级从高到低依次为足底接触稳定性任务、躯干姿态任务、双臂末端位姿同步任务、双臂协同接触力任务和关节空间正则化任务，为每个优先级任务定义任务函数、期望值和任务雅可比矩阵，通过导纳控制获得每个优先级任务的期望任务空间加速度；

6、步骤4：将所述模组间动态耦合张量转化为前馈补偿力矩，注入所述浮基全身动力学模型，按照所述分层优先级顺序依次构建融合动态耦合前馈补偿的分层二次规划问题；

7、步骤5：采用交替方向乘子法求解所述分层二次规划问题，获得全局最优关节加速度和最优接触力向量，反解关节驱动力矩指令；

8、步骤6：将所述关节驱动力矩指令和所述最优接触力向量下发至人形机器人各模组执行。

9、本发明的有益效果：

10、1.本发明通过在线辨识模组间动态耦合张量，并将模组间动态耦合张量转化为前馈补偿力矩注入浮基全身动力学模型，对模组间惯性耦合进行结构化前馈补偿，显著降低双臂末端执行器接触力振荡幅值，提升多模组协同操作时的末端力控精度。

11、2.本发明通过建立足底接触稳定性、躯干姿态、双臂末端位姿同步、双臂协同接触力和关节空间正则化的分层优先级任务，并采用导纳控制生成期望任务空间加速度，在双臂协同搬运与双腿动态行走同步进行时，显著减小躯干姿态角偏差均方根值，提升躯干姿态稳定精度。

12、3.本发明通过将线性化多面体摩擦锥约束和关节力矩边界约束纳入分层二次规划，并采用交替方向乘子法求解，严格满足足底接触摩擦锥约束，避免足底接触力违反摩擦锥约束，保证人形机器人动态行走过程中的接触稳定性。

1.一种人形机器人多模组的协同控制方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种人形机器人多模组的协同控制方法，其特征在于，步骤2中所述在线辨识模组间动态耦合张量具体为：根据采集的关节角度向量、关节角速度向量与关节角加速度向量，构建人形机器人浮基全身动力学模型：

3.根据权利要求1所述的一种人形机器人多模组的协同控制方法，其特征在于，步骤3中所述足底接触稳定性任务的误差为：

4.根据权利要求2所述的一种人形机器人多模组的协同控制方法，其特征在于，步骤4具体为：将辨识得到的模组间动态耦合张量转化为前馈补偿力矩：

5.根据权利要求4所述的一种人形机器人多模组的协同控制方法，其特征在于，步骤5中所述采用交替方向乘子法求解具体为：将每个优先级二次规划问题中的摩擦锥约束和关节力矩边界约束通过引入辅助变量和指示函数处理，构建增广拉格朗日函数：

6.根据权利要求1所述的一种人形机器人多模组的协同控制方法，其特征在于，步骤1中关节角度和关节角速度由关节编码器采集，采样周期为1毫秒，并经过一阶低通滤波处理；关节角加速度由关节角速度差分并经截止频率50赫兹的一阶低通滤波获得；双臂末端执行器的六维接触力与接触力矩由安装于腕部的六维力传感器采集，采样周期为1毫秒；双足足底接触力与接触力矩由每只脚4个一维压力传感器阵列采集并转换为足底合力与合力矩，采样周期为1毫秒；躯干姿态四元数、躯干三轴角速度与躯干三轴线加速度由安装于躯干的惯性测量单元采集，采样周期为1毫秒，并经扩展卡尔曼滤波姿态解算；所有采集数据经过均值为零的滑动窗口去噪处理。

7.根据权利要求2所述的一种人形机器人多模组的协同控制方法，其特征在于，步骤2中遗忘因子的取值为0.98。

8.根据权利要求3所述的一种人形机器人多模组的协同控制方法，其特征在于，所述足底接触稳定性任务的比例增益矩阵的对角元素取值范围为100至400，微分增益矩阵的对角元素取值范围为20至60；所述躯干姿态任务的比例增益矩阵的对角元素取值范围为50至200，微分增益矩阵的对角元素取值范围为10至40；所述双臂末端位姿同步任务的比例增益矩阵的对角元素取值范围为80至300，微分增益矩阵的对角元素取值范围为15至50；所述双臂协同接触力任务的比例增益矩阵的对角元素取值范围为20至100，微分增益矩阵的对角元素取值范围为5至20；所述关节空间正则化任务的比例增益矩阵的对角元素取值范围为10至50，微分增益矩阵的对角元素取值范围为5至15。

9.根据权利要求4所述的一种人形机器人多模组的协同控制方法，其特征在于，步骤4中所述摩擦锥约束集合具体为线性化多面体摩擦锥，将每个接触点处的切向接触力分量约束在以法向接触力分量为轴的线性化多面体内，线性化多面体的摩擦系数取值范围为0.6至1.0，多面体边数取值为4至8。

10.根据权利要求1所述的一种人形机器人多模组的协同控制方法，其特征在于，步骤6中关节驱动力矩指令通过can总线以1毫秒周期发送至人形机器人各关节驱动器，最优接触力向量发送至双臂末端执行器力控模块与双足足底力控模块，各模组按照接收到的指令同步执行协同动作。