CN102672719B - 一种仿人机器人手臂作业动态稳定控制方法 - Google Patents

Abstract

本发明公开了一种仿人机器人手臂作业动态稳定控制方法，该方法通过辅助臂的最优分解动量运动实现机器人作业的动态稳定，保证机器人的全面稳定性：不跳动、不倾倒、不滑动，自动附带保证规划出的辅助臂关节角速度不超限，依据物理原理通过嵌入式高速计算机精密控制和调整机器人的动态稳定性，计算精确，安全可靠，反应迅速，使得机器人作业臂执行高速大范围复杂智能作业时能保持动态稳定，而且这种技术方案不针对特定的机器人结构，适应性广。既实现了高速大范围复杂作业，又能自主保持动态稳定。

Description

一种仿人机器人手臂作业动态稳定控制方法

技术领域

本发明涉及机器人作业稳定控制领域，特别地，涉及一种力/力矩约束最优分解动量的仿人机器人手臂作业动态稳定控制方法。

背景技术

开发出能完成指定作业任务的高智能机器人一直以来是人类的一大梦想。经过多年来坚持不懈的努力，人类所创造出的仿人机器人已经初步实现了步行、奔跑、爬楼梯等腿部运动功能。随着理论研究的深入和工程实践的推进，机器人技术朝着实现更加复杂运动的方向发展，其中就包括机器人手臂作业运动。然而不管是腿部行走，还是手臂作业，机器人的稳定控制方法一直是仿人机器人技术中的核心问题之一。

最开始，机器人的稳定控制方法都是基于重心投影的静态稳定控制方法。然而，1972年南斯拉夫学者Vukobratovic博士提出的ZMP(Zero-Moment Point)控制方法为机器人的动态稳定控制提供了基本的技术基础。之后，ZMP控制方法在机器人动态步行方面的应用取得了巨大的成功，被应用于现今大部分机器人的步行稳定控制，其中包括著名的仿人机器人Asimo和HRP。Asimo采用的是综合多种控制技术的模型ZMP步行稳定控制技术，而HRP则是采用带有步行稳定器的线性倒立摆ZMP步行稳定控制技术。

进入21世纪后，机器人手臂作业技术得到了快速的发展，涌现出一系列针对手臂作业的运动规划和稳定控制技术。在稳定控制方面依次出现了动力学过滤器、自动平衡器以及躯干轨迹补偿等技术，其中动力学过滤器只见于仿真领域，未见实际应用报道；自动平衡器在仿人机器人H6上成功实现了手臂在桌底搜寻物体时躯体的重心投影稳定和惯性力矩约束，但是是一种静态稳定，无法应用于要求高速动态稳定的场合；躯干轨迹补偿技术需要精确计算，并不可避免的涉及到大量循环计算，仿人机器人WABIAN一直致力于该技术的实现。

为了实现更优越的稳定控制，在大量前期研究成果和技术积累的基础上，日本产业技术综合研究所shuuji Kajita教授于2003年公开提出了分解动量控制技术用于仿人机器人HRP-2远程操作下的全身运动自治平衡稳定控制，之后几年成功的实现了拾取地面物体的自治平衡，但是未见报道机器人手臂在更高作业速度下的自治平衡稳定控制。目前这一技术正在发展之中。

机器人手臂动作的复杂性与机器人的稳定性一直是仿人机器人运动控制的一对矛盾，两者很难同时兼顾。分解动量控制技术的出现使得机器人手臂慢速复杂的作业动作得以稳定的实现。但是当机器人手臂做高速复杂作业运动时，机器人的稳定性依旧难以保证，这主要体现在：

1、全维(六维)分解动量控制几乎不能实现。要达到机器人作业时的全面稳定，最好的方法是采用全维分解动量，但是在实际应用中发现全维分解动量控制得到的辅助臂关节角速度几乎都出现超速问题，进而导致辅助臂关节角度超限，关节力矩不足，关节电机发热等问题。

2、若仅做部分维数分解动量，则首先要确定最少需要几维？是哪几维？那么对于每种动作都要做先验评估，而且执行不同动作都要切换到对应的维数。

3、将动量分解到零是不必要的，也不是最佳的。首先，只要满足ZMP条件，当动量不为零时也可以是稳定的，比如匀速飞驰的汽车动量明显不为零，但是依旧是一种稳定的运动。其次，虽然将动量分解为零时稳定性是最强的，但是同时实现代价是最大的，甚至大到无法实现。

发明内容

本发明的目的在于针对现有分解动量控制技术在实际使用中的不足，提供一种既能实现高速复杂动作，又能保证全面稳定性的机器人手臂作业动态稳定控制方法。

本发明的目的是通过以下方案来实现的：一种仿人机器人手臂作业动态稳定控制方法，包括以下步骤：

1.根据实际情况选定合适的采样周期T；

当今仿人机器人都是采用电子计算机离散化数字式采样控制的，从理论上讲采样周期T越短对机器人的控制偏差越小，但是同时对电子计算机的性能要求越高。我们采用嵌入式高速计算机处理器，采样周期T达到1毫秒，足够实现对机器人的高速精密控制。

2.将机器人姿态调零，通过传感器确认机器人质心在地面上的投影大致位于地面支撑域的中心。

3.根据动量公式计算机器人总动量：

$\left[\begin{array}{c}P\\ L\end{array}\right]=\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]+\left[\begin{array}{c}{P}_2\\ L_2\end{array}\right]=\left[\begin{array}{c}{M}_{{\mathrm{arm}}_1}\\ H_{{\mathrm{arm}}_1}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_1+\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_2;$

其中，P、L分别是三维线动量 $\left[\begin{array}{c}{P}_x\\ P_y\\ P_z\end{array}\right]$ 和三维角动量 $\left[\begin{array}{c}{L}_x\\ L_y\\ L_z\end{array}\right]$ 的简写；

$\left[\begin{array}{c}P\\ L\end{array}\right]$ 是计算得到的机器人六维总动量，三维线动量和三维角动量；

$\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right],\left[\begin{array}{c}{P}_2\\ L_2\end{array}\right]$ 分别是作业臂和辅助臂的动量；

分别是机器人作业臂和辅助臂七个关节角速度组成的七维向量；

M，H分别是线运动惯性矩阵和角运动惯性矩阵，用于组成惯性矩阵；

$\left[\begin{array}{c}{M}_{{\mathrm{arm}}_1}\\ H_{{\mathrm{arm}}_1}\end{array}\right],\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]$ 分别是作业臂和辅助臂七个关节对应的惯性矩阵。

4.根据动量定理和稳定性指标得到地面作用力/力矩约束方程组以及为了便于计算机处理的离散化力/力矩约束方程组：

${\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}-{\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i-T*\left[\begin{array}{c}{F}_1\\ M_1\end{array}\right]\≤\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_2\≤{\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}-{\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i+T*\left[\begin{array}{c}{F}_2\\ M_2\end{array}\right];$

其中，F，M分别是三维力 $\left[\begin{array}{c}{F}_x\\ F_y\\ F_z\end{array}\right]$ 和三维力矩 $\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]$ 的简写，用于组成稳定性约束指标向量；

$\left[\begin{array}{c}{F}_1\\ M_1\end{array}\right],\left[\begin{array}{c}{F}_2\\ M_2\end{array}\right]$ 是机器人稳定性约束指标向量；

${\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i$ 是当前机器人作业臂的动量；

${\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}$ 是前一个采样时刻机器人的击球臂和作业臂的总动量；

该公式的推导过程如下：

5.根据重力作用原理、ZMP稳定判据、惯性力原理和摩擦力原理确定动力稳定性约束指标向量：

(5.1).机器人重力失重/超重稳定性约束指标：

a)失重指标(F1的z分量)，失重将导致机器人对地面的压力减少，完全失重时，机器人则可以完全脱离地面，此时其他稳定指标失效，机器人完全失控。

失重指标(F1的z分量)取为机器人自重的10％到30％之间。

b)超重指标(F2的z分量)，超重是对机器人有利的，但是过大会增加机器人的载荷负担。超重指标(F2的z分量)取为机器人自重的20％到50％之间。

(5.1).机器人倾倒(ZMP)稳定性约束指标：

倾倒稳定性指标包括前后倾倒和左右倾倒。

a)前后倾倒指标(M1、M2的y分量)：前倾和后倾是对称相同的，取为机器人自重乘以支撑域前后方向最大长度的一半。

b)左右倾倒指标(M1、M2的x分量)：左倾和右倾也是对称相同的，取为机器人自重乘以支撑域左右方向最大长度的一半。

(5.1).机器人滑动稳定性约束指标：

机器人滑动包括前后和左右的平移滑动和绕z轴的旋转滑动。

a)平移滑动指标(F1、F2的x、y分量)：采用圆内接正方形的方法将最大允许摩擦力线性化，因此前后和左右滑动指标(F1、F2的x、y分量)均为机器人自重乘以摩擦系数再乘以0.707。

b)旋转滑动指标(M1、M2的z分量)：取为机器人自重乘以两脚中心距的一半再乘以摩擦系数。

6.根据电机和减速器实际性能确定机器人两手臂各关节角速度约束指标向量：

$\begin{array}{c}-{\stackrel{\·}{\mathrm{\θ}}}_{1D}\≤{\stackrel{\·}{\mathrm{\θ}}}_1\≤{\stackrel{\·}{\mathrm{\θ}}}_{1U}\\ -{\stackrel{\·}{\mathrm{\θ}}}_{2D}\≤{\stackrel{\·}{\mathrm{\θ}}}_2\≤{\stackrel{\·}{\mathrm{\θ}}}_{2U}\end{array};$

，由于机器人两手臂的对称性，和四者相等。

7.根据机器人辅助臂各关节允许角度和角速度范围和负载能力确定最优权重矩阵：

最优权重矩阵取为正定对角矩阵，特别的取为单位对角矩阵。通过在线调整最优权重矩阵的各个系数可以获得更优越的性能。

8.根据第(4)、(5)和(6)步得到力/力矩约束最优分解动量控制的机器人手臂作业动态稳定控制的不等式约束凸二次规划问题：

$\begin{array}{cc}\underset{x}{\min }& \frac{1}{2}{{\stackrel{\·}{\mathrm{\θ}}}_2}^{T}Q{\stackrel{\·}{\mathrm{\θ}}}_2\\ \mathrm{subjectto}& -{\stackrel{\·}{\mathrm{\θ}}}_{2D}\≤{\stackrel{\·}{\mathrm{\θ}}}_2\≤{\stackrel{\·}{\mathrm{\θ}}}_{2U}\end{array};$

${\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}-{\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i-T*\left[\begin{array}{c}{F}_1\\ M_1\end{array}\right]\≤\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_2\≤{\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}-{\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i+T*\left[\begin{array}{c}{F}_2\\ M_2\end{array}\right]$

其中,Q为最优权重矩阵。

在每个采样周期T内，通过计算机求解得到下一个时间点使得机器人保持作业动态稳定的辅助臂关节角速度运动规划命令。

9.将机器人作业臂关节角速度作业运动命令和辅助臂关节角速度运动规划命令同时发送给两手臂的关节执行器。其中，机器人作业臂作业时的动态稳定性由在第(8)步第二个约束方程中的稳定性约束指标向量的约束下生成的辅助臂关节角速度规划运动来自动保证，也即保证机器人在作业过程中不跳动、不倾倒、不滑动，而辅助臂关节角速度的超限问题也由第8步第一个约束方程的关节角速度约束指标向量来自动保证。

本发明的有益效果是：本发明通过辅助臂的最优分解动量运动实现机器人作业的动态稳定，保证机器人的全面稳定性：不跳动、不倾倒、不滑动，自动附带保证规划出的辅助臂关节角速度不超限，依据物理原理通过嵌入式高速计算机精密控制和调整机器人的动态稳定性，计算精确，安全可靠，反应迅速，使得机器人作业臂执行高速大范围复杂智能作业时能保持动态稳定，而且这种技术方案不针对特定的机器人结构，适应性广。既实现了高速大范围复杂作业，又能自主保持动态稳定。

附图说明

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

图1是本发明实施例1提供的动态稳定控制器结构图；

图2是本发明实施例1提供的一种辅助臂最优分解动量运动保证仿人机器人作业臂高速作业动态稳定的控制方法的一种流程图；

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明通过辅助臂的最优分解动量控制来化解机器人作业臂执行作业任务产生的不稳定性，通过力/力矩约束来实现机器人作业的全面稳定性。

实施例1

如图1所示，本发明实施例提供了一种机器人乒乓球击球平衡稳定控制器。该机器人击球平衡稳定控制器采用智能结构，由机器人首先判断乒乓球来球方位选取左手臂或右手臂为击球臂(作业臂)，另一只手臂则为辅助臂，通过辅助臂的运动来保证击球臂击球时整个机器人的动态稳定，辅助臂的运动是由击球臂的运动经过最优分解动量控制生成的。击球机器人包括本体1，击球臂(作业臂)2，辅助臂3，击球臂和辅助臂均为7个自由度。

机器人的整个击球运动过程是：首先根据乒乓球来球方向选取一条手臂作为击球臂，另一条手臂则为辅助臂，然后在每个采样周期T由最优分解动量控制器根据击球臂击球运动的各关节运动命令生成辅助臂各关节运动命令，通过辅助臂的各关节运动来控制机器人在击球时的全面稳定。

参见图2，本发明实施例提供了一种仿人机器人手臂作业的动态稳定控制方法，通过辅助臂的运动使得机器人保持作业稳定。具体步骤如下：

步骤201：初始化机器人计算机控制***的采样周期为T＝0.001秒；

步骤202：将机器人调零，此时机器人自身质心在地面的投影大致位于地面双脚支撑域中心；

步骤203：初始化机器人两手臂关节角速度约束指标向量；

步骤204：初始化机器人动力稳定性约束指标向量；

具体地，

平移滑动指标：F1x＝F1y＝F2x＝F2y＝G*mu*0.707；

失重指标F1z＝G*20％；

超重指标F2z＝G*40％；

左右倾倒指标：M1x＝M2x＝G*A/2；

前后倾倒指标：M1y＝M2y＝G*B/2；

旋转滑动指标：M1z＝M2z＝G*mu*C/2。

其中，G是机器人自身重力，mu是机器人脚底与地面摩擦系数，A是机器人地面支撑域左右方向最大宽度，B是机器人地面支撑域前后方向最大长度，C是机器人两脚底中心距。

步骤205：选取速度最优权重矩阵为单位对角矩阵；

步骤206：等待来球，此时机器人已经做好准备，随时响应来球指令；

步骤207：判断有无来球；

如果没有来球，则执行步骤206，否则，执行步骤208。

步骤208：根据初始来球方位选取最适合接球的一条手臂作为击球臂，另一条手臂则成为辅助臂；

步骤209：根据当前来球方位和速度以及运动趋势计算击球臂当前所需关节角速度，一方面作为机器人击球臂击球的动作命令，另一方面根据击球臂的动作和机器人动力稳定性约束指标以及辅助臂关节角速度约束指标和最优权重矩阵得到线性矩阵不等式约束下的凸二次规划最优分解动量控制问题；

步骤210：机器人计算机***实时求解线性矩阵不等式约束凸二次规划最优分解动量控制问题得到为保持机器人击球臂击球稳定所需的辅助臂关节角速度运动规划；

步骤211：同时发送机器人击球臂和辅助臂当前关节角速度命令给各关节执行器，同时执行击球臂的击球动作和辅助臂的最优分解动量控制稳定动作，也即机器人保持作业下的动态稳定；

步骤212：判断本次击球是否进行完毕；

如果击球没有完毕，则执行步骤209，否则，执行步骤213；

步骤213：判断是否进行下一次击球；

如果需要进行下一次击球，则执行步骤206，否则机器人击球活动结束。

本发明实施例依据物理原理通过嵌入式高速计算机精密控制和调整机器人的动态稳定性，计算精确，安全可靠，反应迅速，使得机器人作业臂执行高速大范围复杂智能作业时能保持全面动态稳定性：不跳动、不倾倒、不滑动，自动附带保证规划出的辅助臂关节角速度不超限，而且这种技术方案不针对特定的机器人结构，适应性广。既实现了高速大范围复杂作业，又能自主保持动态稳定。

Claims (1)

1.一种仿人机器人手臂作业动态稳定控制方法，其特征在于，该方法包括以下步骤：

(1)根据实际情况选定合适的采样周期T；

(2)将机器人姿态调零，通过传感器确认机器人质心在地面上的投影大致位于地面支撑域的中心；

(3)根据动量公式计算机器人总动量：

$\left[\begin{array}{c}P\\ L\end{array}\right]=\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]+\left[\begin{array}{c}{P}_2\\ L_2\end{array}\right]=\left[\begin{array}{c}{M}_{{\mathrm{arm}}_1}\\ H_{{\mathrm{arm}}_1}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_1+\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_2;$

其中，P、L分别是三维线动量 $\left[\begin{array}{c}{P}_x\\ P_y\\ P_z\end{array}\right]$ 和三维角动量 $\left[\begin{array}{c}{L}_x\\ L_y\\ L_z\end{array}\right]$ 的简写；

$\left[\begin{array}{c}P\\ L\end{array}\right]$ 是计算得到的机器人六维总动量，三维线动量和三维角动量；

$\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right],\left[\begin{array}{c}{P}_2\\ L_2\end{array}\right]$ 分别是作业臂和辅助臂的动量；

分别是机器人作业臂和辅助臂七个关节角速度组成的七维向量；

M，H分别是线运动惯性矩阵和角运动惯性矩阵，用于组成惯性矩阵；

$\left[\begin{array}{c}{M}_{{\mathrm{arm}}_1}\\ H_{{\mathrm{arm}}_1}\end{array}\right],\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]$ 分别是作业臂和辅助臂七个关节对应的惯性矩阵；

(4)根据动量定理和稳定性指标得到地面作用力/力矩约束方程组以及为了便于计算机处理的离散化力/力矩约束方程组：

${\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}-{\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i-T*\left[\begin{array}{c}{F}_1\\ M_1\end{array}\right]\≤\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_2\≤{\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}-{\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i+T*\left[\begin{array}{c}{F}_2\\ M_2\end{array}\right];$

其中，F，M分别是三维力 $\left[\begin{array}{c}{F}_x\\ F_y\\ F_z\end{array}\right]$ 和三维力矩 $\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]$ 的简写，用于组成稳定性约束指标向量；

$\left[\begin{array}{c}{F}_1\\ M_1\end{array}\right],\left[\begin{array}{c}{F}_2\\ M_2\end{array}\right]$ 是机器人稳定性约束指标向量；

${\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i$ 是当前机器人作业臂的动量；

${\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}$ 是前一个采样时刻机器人的击球臂和作业臂的总动量；

(5)根据重力作用原理、ZMP稳定判据、惯性力原理和摩擦力原理确定动力稳定性约束指标向量，所述动力稳定性约束指标向量包括机器人重力失重/超重稳定性约束指标、机器人倾倒稳定性约束指标和机器人滑动稳定性约束指标；其中，所述机器人重力失重/超重稳定性约束指标包括失重指标和超重指标，所述失重指标为机器人自重的10％到30％之间；所述超重指标为机器人自重的20％到50％之间；所述机器人倾倒稳定性约束指标包括前后倾倒指标和左右倾倒指标，所述前后倾倒指标为机器人自重乘以支撑域前后方向最大长度的一半；所述左右倾倒指标取为机器人自重乘以支撑域左右方向最大长度的一半；所述机器人滑动稳定性约束指标包括平移滑动指标和旋转滑动指标，所述平移滑动指标为机器人自重乘以摩擦系数再乘以0.707，所述旋转滑动指标为机器人自重乘以两脚中心距的一半再乘以摩擦系数；

(6)根据电机和减速器实际性能确定机器人两手臂各关节角速度约束指标向量：

$\begin{array}{c}-{\stackrel{\·}{\mathrm{\θ}}}_{1D}\≤{\stackrel{\·}{\mathrm{\θ}}}_1\≤{\stackrel{\·}{\mathrm{\θ}}}_{1U}\\ -{\stackrel{\·}{\mathrm{\θ}}}_{2D}\≤{\stackrel{\·}{\mathrm{\θ}}}_2\≤{\stackrel{\·}{\mathrm{\θ}}}_{2U}\end{array};$ 由于机器人两手臂的对称性，和四者相等；

(7)根据机器人辅助臂各关节允许角度和角速度范围和负载能力确定最优权重矩阵：最优权重矩阵为正定对角矩阵或单位对角矩阵；

(8)根据步骤(4)、(5)和(6)得到力/力矩约束最优分解动量控制的机器人手臂作业动态稳定控制的不等式约束凸二次规划问题：

$\begin{array}{cc}\underset{x}{\min }& \frac{1}{2}{{\stackrel{\·}{\mathrm{\θ}}}_2}^{T}Q{\stackrel{\·}{\mathrm{\θ}}}_2\\ \mathrm{subject\; to}& -{\stackrel{\·}{\mathrm{\θ}}}_{2D}\≤{\stackrel{\·}{\mathrm{\θ}}}_2\≤{\stackrel{\·}{\mathrm{\θ}}}_{2U}\end{array};$

${\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}-{\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i-T*\left[\begin{array}{c}{F}_1\\ M_1\end{array}\right]\≤\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_2\≤{\left[\begin{array}{c}P\\ L\end{array}\right]}_{i-1}-{\left[\begin{array}{c}{P}_1\\ L_1\end{array}\right]}_i+T*\left[\begin{array}{c}{F}_2\\ M_2\end{array}\right]$

其中，Q为最优权重矩阵；

在每个采样周期T内，通过计算机求解得到下一个时间点使得机器人保持作业动态稳定的辅助臂关节角速度运动规划命令；

(9)将机器人作业臂关节角速度作业运动命令和辅助臂关节角速度运动规划命令同时发送给两手臂的关节执行器；其中，机器人作业臂作业时的动态稳定性由在步骤(8)第二个约束方程中的稳定性约束指标向量的约束下生成的辅助臂关节角速度规划运动来自动保证，也即保证机器人在作业过程中不跳动、不倾倒、不滑动，而辅助臂关节角速度的超限问题也由步骤(8)第一个约束方程的关节角速度约束指标向量来自动保证。