[发明专利]考虑外力冲击干扰和阻尼的多足机器人能量裕度计算方法

权利要求书

1.一种考虑外力冲击干扰和阻尼的多足机器人能量裕度计算方法，其特征在于：包括如下步骤：

步骤一：把机器人的翻转动作转换为包含弹簧阻尼特性的机械旋转系统，计算机器人的能量裕度表达式；

步骤二：建立机器人在笛卡尔坐标系下的D-H数学模型；

步骤三：结合步骤二解析机器人能量裕度函数所需有关参数；

所述的步骤一通过以下方式实现：

1.1)根据牛顿第二定律列出机器人的动力学微分方程：

其中，将绕边线翻转时的机器人，视为一个机械旋转系统，该系统由转动惯量为J_i、扭转刚度为K_S和粘性阻尼系数为C的阻尼器组成；

1.2)求解机器人绕边线旋转的实时重心处合力矩，即驱动力矩：

式中，M_Gi为实时重心处重力相对旋转边线产生的力矩；M_I为实时重心处的等效惯性力矩；M_ri为着地脚因支撑力产生的力矩；f_i为各足端竖直向上的力；θ_i为直线AB与水平面夹角，构成倾翻边界的两足端位置坐标：A(x_A,y_A,z_A)，B(x_B,y_B,z_B)，直线AB的表达式：

δ_i为重心绕边线到最高点的旋转角度；θ为斜线倾角；r_i为各足端到旋转边线的距离；e_i为旋转边线的单位法向量，以支撑多边形逆时针方向为正方向；

1.3)系数K_S、C测定，根据机械旋转系统整理得

两边进行拉氏变换，求得其传递函数为：

因此求得

式中，T_C、T_S分别为当机器人受到一个脉冲力时，机械旋转系统的震荡周期与稳定时间；

1.4)求解机器人考虑外力冲击干扰和阻尼的能量裕度

机器人绕边线旋转到最高点的势能变化：

机器人绕边线旋转到最高点的动能变化：

当w₀为正时：

当w₀为负时：

假设机器人绕边线旋转到最高点时w_i＝0，因此求得机器人当前位置因外界干扰不发生完全侧翻，所能抵抗的能量为：

E_i＝V_i-V₀+K_i-K₀

求得：

上述公式中，w₀、M_Ri'分别为实时重心位置绕边线旋转的角速度与合力矩,方向延旋转边线，以支撑多边形逆时针方向为正方向；

根据能量稳定裕量的概念可知，定义能量稳定裕度为SE_m，设最小稳定裕度SE，当SEm≥SE时，机器人能稳定工作。

2.如权利要求1所述的考虑外力冲击干扰和阻尼的多足机器人能量裕度计算方法：其特征在于：所述的步骤二通过以下方式实现：

在机器人的各关节处与躯体中心，建立笛卡尔坐标系，设机器人坐标系C到世界坐标系O的之间的旋转变换为线性变换为^OP_C，因此求得机器人坐标系C到世界坐标系O的齐次变换为：

当空间有n个坐标系时，若已知相邻坐标系之间的齐次变换矩阵，则：

M_0n＝M₀₁·M₀₂·…M_n-1n

其中M_ij为相邻坐标系的齐次变换，由此建立机器人各部分在世界坐标系O中的位置。

3.如权利要求2所述的考虑外力冲击干扰和阻尼的多足机器人能量裕度计算方法：其特征在于：所述的步骤三通过以下方式实现：

4.1)重心位置参数，

各部件均为匀质材料，因此其重心均在其几何中心处，根据步骤二中齐次变换矩阵，求各部件重心在世界坐标系O的位置；

以躯体重心在世界坐标系O中的位置求解：

式中，表示躯体重心在机身坐标系O_C中的位置；

同理求得各部件重心在世界坐标系O的位置，根据各重心位置坐标代入以下公式求得实时重心位置：

其中，W为机器人的总量，单位kg；n为机器人的足数，x_o,y_o,z_o为机身重心在世界坐标系O中位置坐标，x_j,y_j,z_j为各部件重心在世界坐标系O中位置坐标；

4.2)求解实时重心绕支撑边线旋转到最高位置的竖直位移；

设构成倾翻边线的两足端位置坐标：

A(x_A,y_A,z_A)，B(x_B,y_B,z_B)

首先，由几何关系式，求直线AB的表达式与重心CG到直线AB的距离H_i；

再次，求重心绕AB旋转到最高点的竖直位移；

S_NEi＝H_i(1-cosδ_i)cosθ_i

式中θ_i为直线AB与水平面夹角，δ_i为重心绕边线到最高点的旋转角度；

最后把解析出的相关参数代入到能量稳定裕度的公式中，求得机器人稳定裕度值，用于稳定性判定，保证机器人稳定工作。