考虑外力冲击干扰和阻尼的多足机器人能量裕度计算方法与流程

技术特征：

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

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

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

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

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

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

$M_{Ri}-C\stackrel{\·}{\mathrm{\δ}}-Ks\mathrm{\δ}=J_i\stackrel{\·\·}{\mathrm{\δ}}$

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

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

$\begin{array}{c}{M}_{Ri}=M_I\·e_i+M_{Gi}+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{M}_{ri}\\ =-H_{i}mg{\mathrm{sin\δ}}_i{\mathrm{cos\θ}}_i+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{r}_i{f}_i\cos \mathrm{\θ}+M_I\·e_i\end{array}$

式中，M_Gi为实时重心处重力相对旋转边线产生的力矩；M_I为实时重心处的等效惯性力矩；M_ri为着地脚因支撑力产生的力矩；f_i为各足端竖直向上的力；θ_i为直线AB与水平面夹角，δ_i为重心绕边线旋转角度；θ为斜线倾角；r_i为各足端到旋转边线的距离；e_i为旋转边线的单位法向量，以支撑多边形逆时针方向为正方向；

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

$J_i\frac{d^2\mathrm{\δ}\left(t\right)}{{\mathrm{dt}}^2}+C\frac{d\mathrm{\δ}\left(t\right)}{dt}+K_S\mathrm{\δ}\left(t\right)=M_{Ri}\left(t\right)$

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

$G\left(s\right)=\frac{\frac{1}{J_i}}{S^2+\frac{C}{J_i}S+\frac{K_S}{J_i}}$

因此求得

$\frac{K_S}{J_i}={w_n}^2\⇒K_S=J_i{w_n}^2=\frac{J_i}{{T_C}^2}$

$T_S=\frac{4}{{\mathrm{\ζw}}_n}=\frac{8J_i}{C}\⇒C=\frac{8J_i}{T_S}$

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

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

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

$\begin{array}{c}{V}_i-V_0={\mathrm{mgS}}_{NEi}+\underset{0}{\overset{{\mathrm{\δ}}_i}{\∫}}C{\stackrel{\·}{\mathrm{\δ}}}_{i}d\mathrm{\δ}+\underset{0}{\overset{{\mathrm{\δ}}_i}{\∫}}{\mathrm{Ks\δ}}_{i}d\mathrm{\δ}-\underset{0}{\overset{{\mathrm{\δ}}_i}{\∫}}{M_{Ri}}^{\′}d\mathrm{\δ}\\ ={\mathrm{mgS}}_{NEi}+{\mathrm{C\δ}}_i+\frac{1}{2}{{\mathrm{Ks\δ}}_i}^2-{M_{Ri}}^{\′}{\mathrm{\δ}}_i\end{array}$

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

当w₀为正时：

$K_i-K_0=\frac{1}{2}{J}_i{w_i}^2-\frac{1}{2}{J}_i{w_0}^2$

当w₀为负时：

$K_i-K_0=\frac{1}{2}{J}_i{w_i}^2-(0-\frac{1}{2}{J}_i{w_0}^2)$

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

E_i＝V_i-V₀+K_i-K₀

求得：

$E_i={\mathrm{mgS}}_{NEi}+{\mathrm{C\δ}}_i+\frac{1}{2}{\mathrm{Ks\δ}}_i^2-\frac{1}{2}{J}_i{w}_0\left|w_0\right|-{M_{Ri}}^{\′}{\mathrm{\δ}}_i$

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

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

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

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

$M_{OC}={}_{OC}{}^{O}R{{}^{O}P}_C$

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

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

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

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

4.1)重心位置参数，

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

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

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

同理求得各部件重心在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＝ECG'cosθ＝H_i(1-cosδ_i)cosθ_i

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

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