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

摘要

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

主权项

一种仿人机器人手臂作业动态稳定控制方法，其特征在于，该方法包括以下步骤：(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{·}{\theta }}_1+\left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{·}{\theta }}_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]\le \left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{·}{\theta }}_2\le {\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{·}{\theta }}_{1D}\le {\stackrel{·}{\theta }}_1\le {\stackrel{·}{\theta }}_{1U}\\ -{\stackrel{·}{\theta }}_{2D}\le {\stackrel{·}{\theta }}_2\le {\stackrel{·}{\theta }}_{2U}\end{array};$由于机器人两手臂的对称性，和四者相等；(7)根据机器人辅助臂各关节允许角度和角速度范围和负载能力确定最优权重矩阵：最优权重矩阵为正定对角矩阵或单位对角矩阵；(8)根据步骤(4)、(5)和(6)得到力/力矩约束最优分解动量控制的机器人手臂作业动态稳定控制的不等式约束凸二次规划问题：$\begin{array}{cc}\underset{x}{\min }& \frac{1}{2}{{\stackrel{·}{\theta }}_2}^{T}Q{\stackrel{·}{\theta }}_2\\ \mathrm{subject\; to}& -{\stackrel{·}{\theta }}_{2D}\le {\stackrel{·}{\theta }}_2\le {\stackrel{·}{\theta }}_{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]\le \left[\begin{array}{c}{M}_{{\mathrm{arm}}_2}\\ H_{{\mathrm{arm}}_2}\end{array}\right]{\stackrel{·}{\theta }}_2\le {\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)第一个约束方程的关节角速度约束指标向量来自动保证。