一种基于多传感器信息仿人机器人跌倒状态检测方法

摘要

本发明公开了基于多传感器仿人机器人跌倒状态检测方法，该方法包括：通过融合机器人当前的步行状态，姿态信息和ZMP点位置信息，利用模糊决策系统对机器人当前稳定性进行判定。判定结果包括9种状态：可控状态，向前跌倒，向后跌倒，向左跌倒，向右跌倒，向左前跌倒，向左后跌倒，向右前跌倒和向右后跌倒。该方法适用于仿人机器人在快要摔倒的时候，判断仿人机器人是否处于可控状态，并在机器人不可控时获得仿人机器人摔倒的方向，从而为仿人机器人产生相应的保护动作提供依据。本发明能够真实的反映出机器人运动的稳定情况，可靠性和判定准确率较高。

说明书

技术领域

本发明涉及仿人机器人状态检测技术领域，具体涉及一种基于多传感器信息仿人机器 人跌倒状态检测方法。

背景技术

在仿人机器人步行的时候，需要时刻对仿人机器人的稳定状态进行判定。当机器人有 跌倒的倾向的时候，需要通过反馈控制相应的关节来阻止机器人摔倒。当机器人已经快要 摔倒时，此时机器人的状态已超出了可控的范围，需要让机器人以损伤最小的姿态倒地。

只有获得机器人跌倒时的真实运动状态，才能实施有效的控制策略来防止机器人跌倒 或在跌倒时采取保护措施来防止机器人损伤。判定机器人当前的状态是否可控，即是否要 跌倒以及机器人跌倒的方向，需要一个复杂的判定过程。目前也有一些研究对机器人摔倒 状态进行了研究。

由于机器人在行走过程中会影响姿态传感器信息，所以只利用姿态信息来进行机器人 稳定性和可控性的判定是不全面的。如果只依据ZMP点位置信息也不可行，因为只有机器 人的脚底和地面完全接触，才能利用FSR传感器得到有效的ZMP信息。因此，采用一个综 合的判定方法能大大增加判定的准确率，加快整个判定过程。

发明内容

为解决上述相关技术的不足，本发明提出了基于多传感器信息仿人机器人跌倒状态检 测方法，利用机器人的步行阶段信息，姿态信息和ZMP点位置信息综合判定机器人稳定性 的方法。使用该方法能够实时地融合机器人步态阶段信息，姿态信息和ZMP点位置信息， 并利用模糊决策系统对机器人当前稳定性和可控性进行快速判定，即可实现机器人在活动 状态或静止状态下的稳定性和可控性判定，机器人使用这种检测方法可以快速判定机器人 是否可控，即是否要跌倒和将要跌倒的方向，为机器人下一步动作提供参考。

本发明提供一种分层式的仿人机器人多传感器信息融合模型，该系统能判定机器人综 合的稳定性，包含如下步骤：

(1)通过对姿态传感器信息、零力矩点ZMP传感器信息和机器人步行阶段信息进行 融合，建立分层结构的传感器信息融合模型，从而实现仿人机器人稳定性判定；

(2)利用模糊决策系统将输入的步行阶段、姿态信息、ZMP点位置信息进行融合分 析，得出最终的对机器人稳定状态的判定；其中，把机器人步行阶段作为模糊决策系统 输入之一；把仿人机器人姿态信息作为模糊决策系统输入之一；把ZMP点位置信息作为模 糊决策系统输入之一。

进一步的，步骤(1)所述分层结构的传感器信息融合模型包括低层数据层信息融合 模型和高层决策层融合模型，其中低层数据层信息融合模型分别是步行阶段融合模型、姿 态传感器信息融合模型和FSR传感器信息计算模型；高层决策层融合模型完成对机器人稳 定性进行综合判定：根据融合后的姿态信息、ZMP信息和机器人行走的步行阶段数据共同 建立模糊决策系统，模糊决策系统用来判定机器人是否处于不可控的状态，从而为机器人 控制提供决策的依据。

进一步的，步骤(2)所述的模糊决策系统的判定过程是：将步行阶段信息、姿态信 息和ZMP点位置信息作为模糊决策的输入，通过模糊决策后得到5种判定结果作为输出： 可控状态，向左倒下，向右倒下，向前倒下，向后倒下；由于判定机器人是否前后倒下， 主要和前后方向的姿态信息和前后方向的ZMP点位置信息有关，和左右方向的数据没有 太大关系；同理左右方向上的姿态信息和左右方向上的ZMP点位置和前后方向的数据也 没有太大关系；所以前后方向倒和左右方向倒的判定，分用前后模糊决策系统和左右模糊 决策系统来完成。

进一步的，所述前后模糊决策系统判定机器人在前后方向的稳定性，具体步骤如下：

①根据机器人支撑脚和地面之间的接触情况与离线步态规划确定机器人步行阶段；

②利用模糊推理算法对ZMP点位置信息进行模糊化；

③利用模糊推理算法对机器人的俯仰角进行模糊化；

④确定输出隶属函数；

⑤根据步骤①～步骤④确定模糊推理规则；

⑥去模糊化；

所述左右模糊决策系统判定机器人在左右方向的稳定性，具体步骤如下：

①根据机器人支撑脚和地面之间的接触情况与离线步态规划确定机器人步行阶段；

②利用模糊推理算法对ZMP点位置信息进行模糊化；

③利用模糊推理算法对机器人的绕x轴滚动角进行模糊化；

④确定输出隶属函数；

⑤根据步骤①～步骤④确定模糊推理规则；

⑥去模糊化。

进一步的，步骤(2)所述机器人步行阶段根据离线步态规划和力传感记录单元FSR (force sensing register)传感器信息来确定。

进一步的，步骤(2)中的仿人机器人姿态信息由姿态传感器采集得到，其中仿人机 器人姿态信息是通过加速度传感器和角速度传感器共同获得。

进一步的，步骤(2)中的ZMP点位置信息由FSR传感器采集得到，根据安装在机器 人脚底FSR传感器计算出机器人双脚支撑阶段和单脚支撑阶段的ZMP位置。

与现有技术相比，本发明具有以下优点：

(1)针对仿人机器人稳定行走提供一种分层的多传感器信息融合框架，根据各种传 感器数据的特点进行数据层、特征层和决策层的数据融合，能够真实的反映出机器人运动 的稳定情况。

(2)采用模糊决策系统对机器人稳定性进行判定，提高可靠性。

(3)融合了多种机器人相关状态信息进行判定，提高判定准确率。

附图说明

图1为仿人机器人稳定判定整体流程图。

图2为仿人机器人模糊决策判定图。

图3为仿人机器人步行阶段确定流程流程图。

图4为支撑脚尺寸示意图。

图5为双脚支撑8个FSR传感器安装位置示意图。

图6为隶属函数图形。

图7为θ隶属函数图形。

图8为隶属函数图形。

图9为φ隶属函数图形。

图10为前后推倒仿人机器人姿态曲线。

图11为前后推倒仿人机器人曲线。

图12为前后推倒机器人实验判定结果图。

图13为仿人机器人前进摔倒姿态曲线图。

图14为仿人机器人前进摔倒曲线。

图15为前进摔倒机器人实验判定结果图。

图16为实施方式中卡尔曼滤波整个过程的示意图。

具体实施方式

下面结合附图详细说明本发明的具体实施方式，但本发明的实施和保护不限于此，需 指出的是，以下若有未特别详细说明的符号或过程，均是本领域技术人员可参照现有技术 实现的。

(1)在仿人机器人步行过程中建立一个机器人多传感器信息融合模型，为机器人的 控制提供信息和判定结果，从而能使控制器控制仿人机器人稳定的行走及在倒地时可以做 出保护动作。根据传感器信息融合的特点，本发明提出一种分层结构的传感器信息融合模 型实现对仿人机器人步行过程稳定性判定。见附图1。

其中加速度传感器和角速度传感器安装在机器人髋关节处。FSR传感器安装在机器人脚底， 其中每只脚底安装4个FSR传感器，共8片FSR传感器。

(2)建立低层数据层传感器信息融合模型，其中包括步行阶段融合模型、姿态传感 器信息融合模型和FSR传感器信息计算模型。

1)步行阶段融合模型。仿人机器人步行阶段确定的过程具体如下：首先根据离线 的步态规划确定出机器人是处于单脚支撑阶段还是双脚支撑阶段。如果是处于单脚支撑阶 段，判定支撑脚上的4个FSR传感器的数据是否有为0的情况，如果有FSR数据为0， 则说明支撑脚和地没有进行面接触，定义这种情况为单脚非面接触支撑阶段；如果支撑脚 上所有FSR的数据都不为0，则说明支撑脚和地是面接触，定义这种情况为单脚面接触 支撑阶段。如果是处于双脚支撑阶段，判定支撑脚上4个外侧点的FSR传感器数据是否 有数据为0，若有数据为0，则说明支撑脚和地没有进行面接触，定义这种情况为双脚非 面接触支撑阶段；如果所有的外侧点的FSR传感器数据都不为0，则说明支撑脚和地是 面接触，定义这种情况为双脚面接触支撑阶段。具体的流程可见附图3。

2)姿态传感器信息融合模型。针对姿态传感器信息融合方法，选择加速度传感器 ADXL204和角速度传感器IDG300作为仿人机器人的姿态传感器。并建立了加速度传感 器和角速度传感器的卡尔曼滤波方程。整个卡尔曼滤波方法过程描述如下(以下仅仅为举 例)：

①被估计的过程信号

卡尔曼滤波器用于估计离散时间过程的状态变量x∈Rⁿ。这个离散时间过程可由离散 的随机差分方程来进行描述：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1

观测方程描述如下：

Z_k＝H_kX_k+V_k

其中，X_k是系统的n维状态向量，Φ_k,k-1为n×n维非奇异状态转移矩阵；Γ_k,k-1是n×p 维系统过程噪声输入矩阵；H_k是m×n维观测矩阵；随机信号W_k和V_k分别表示随机过程 系统过程噪声序列和随机过程系统观测噪声序列。它们为相互独立，正态分布的白色噪 声：

W_k-N(0，Q_k)

V_k-N(0，R_k)

其中，Q_k是系统过程噪声W_k的n×p维方差矩阵，R_k是系统观测噪声V_k的m×n维方 差阵。

关于系统过程噪声和观测噪声的统计特性，我们假定如下：

$E\left[W_k{W}_j^T\right]=Q_k{\delta }_{\mathrm{kj}}$

$E\left[V_k{V}_j^T\right]=R_k{\delta }_{\mathrm{kj}}$

其中δ_kj是克罗尼克(Kronecker)δ函数。

②离散卡尔曼滤波器算法

卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态，然 后以(含噪声的)测量变量的方式获得反馈。因此卡尔曼滤波器可分为两个部分：时间更 新方程和测量更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计 的值，以便为下一个时间状态构造先验估计。测量更新方程负责反馈——也就是说，它将 先验估计和新的测量变量结合以构造改进的后验估计。时间更新方程也可视为预测方程， 测量更新方程可视为校正方程。最后的估计算法成为一种具有数值解的预测－校正算法。

下面直接给出随机线性离散系统基本的卡尔曼滤波方程：

状态预测

${\hat{X}}_{k,k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}$

状态估计

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

滤波增益矩阵

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

预测误差方差阵

$P_{k,k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^T$

更新误差方差阵

$P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T$

卡尔曼滤波整个过程如图16所示。

计算完预测方程和校正方程后，整个过程再次重复。上一次计算得到的后验估计被作 为下一次计算的先验估计。这种递归推算是卡尔曼滤波器最吸引人的特性之一—它比其它 滤波器更容易实现：例如维纳滤波器，每次估计必须直接计算全部数据，而卡尔曼滤波器 每次只根据以前的测量变量递归计算当前的状态估计。

③姿态传感器卡尔曼滤波信息融合

对芯片在转动过程中由加速度传感器得到的角度信息和由角速度传感器得到的角速度 信息，利用卡尔曼滤波进行信息融合，来获得姿态信息。

设第k时刻转过的角度为θ_k，角速度为同时设在从第k-1时刻到第k时刻之间的角 加速度为且使用角加速度的误差作为系统的白噪声W_k；设第k时刻与第k-1时刻的时 间差为Δt。

则系统第k个时刻的状态向量X_k为$\left(\begin{array}{c}{\theta }_k\\ {\stackrel{·}{\theta }}_k\\ {\stackrel{··}{\theta }}_k\end{array}\right),$根据随机差分方程和运动学方程，可得该系 统状态方程为：

$X_k=\left(\begin{array}{c}{\theta }_k\\ {\stackrel{·}{\theta }}_k\\ {\stackrel{··}{\theta }}_k\end{array}\right)={\Phi }_{k,k-1}{X}_{k-1}+{\Gamma }_{k,k-1}{W}_{k-1}=\left(\begin{array}{ccc}1& \mathrm{\Delta t}& {\mathrm{\Delta t}}^2/2\\ 0& 1& \mathrm{\Delta t}\\ 0& 0& 1\end{array}\right)X_{k-1}+\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)W_{k-1}$

其中，其中W_k为系统过程噪声向量，且W_k-N(0，Q_k)Δt是传感器数据采集的时 间间隔。

在姿态传感器信息卡尔曼融合的过程中，观测到的是角度信息和角速度信息，则观测 向量Z_k记为$\left(\begin{array}{c}{\theta }_{\mathrm{zk}}\\ {\stackrel{·}{\theta }}_{\mathrm{zk}}\end{array}\right),$根据随机过程的观测方程可得：

$Z_k={\mathrm{HX}}_k+V_k=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right)X_k+V_k$

其中V_k为二维的观测噪声向量，且V_k-N(0，R_k)。

在卡尔曼滤波中由于Q_k,R_k不变，分别记为Q和R。则通过初始化向量X₀、初始方差 阵P₀、系统过程噪声方差阵Q和系统测量噪声方差阵R就可以在迭代中得出最优估计值 由于这些初值尤其是Q和R的设定，直接影响卡尔曼滤波的效果，所以对X₀、P₀、 Q和R的设定进行了研究。

(a)初始值X₀的确定。

对于X₀，即最初的最优估计，直接根据起初的观测值Z₀得出。

$X_0=H^T{Z}_0=\left(\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\end{array}\right)Z_0$

(b)初始值P₀的确定。

对于P₀，设该系统最初的实际状态值为X，则初始残差为：

${\tilde{X}}_0=X-X_0$

由观测方程可得：

$Z_0=H(X_0+{\tilde{X}}_0)+V_0$

则：

$Z_0=H(H^T{Z}_0+{\tilde{X}}_0)+V_0$

整理得：

$H{\tilde{X}}_0+V_0=(I-{\mathrm{HH}}^T)Z_0=0$

若初始残差${\tilde{X}}_0=\left(\begin{array}{c}{\tilde{\theta }}_0\\ {\widetilde{\stackrel{·}{\theta }}}_0\\ {\widetilde{\stackrel{··}{\theta }}}_0\end{array}\right),V_0=\left(\begin{array}{c}{v}_{{\theta }_0}\\ v_{{\stackrel{·}{\theta }}_0}\end{array}\right),$则由上式可知:

$\left(\begin{array}{c}{\tilde{\theta }}_0={-v}_{{\theta }_0}\\ {\widetilde{\stackrel{·}{\theta }}}_0={-v}_{{\stackrel{·}{\theta }}_0}\end{array}\right)$

又因为为初始时刻加速度误差，即本模型定义的系统误差，则所以有

$P_0=E\left({\tilde{X}}_0{\tilde{X}}_0^T\right)=\left(\begin{array}{ccc}E\left(v_{{\theta }_0}{v}_{{\theta }_0}^T\right)& 0& 0\\ 0& E\left(v_{w_0}{v}_{w_0}^T\right)& 0\\ 0& 0& E\left({\mathrm{ww}}^T\right)\end{array}\right)$

因为$R=E\left(V_k{V}_k^T\right)=\left(\begin{array}{cc}E\left(v_{\theta }{v}_{\theta }^T\right)& 0\\ 0& E\left(v_w{v}_w^T\right)\end{array}\right)$和Q＝E(ww^T)，所以只要确定Q和R即可确定 P₀。

(c)Q和R的确定。

对于确定Q和R，从概率统计的角度看，就必须先分别收集到对应的V_k,W_k序列，这 又必须先确定第k时刻实际的系统状态值，难度较大。

考虑到以下三个因素：

a)在本模型中要确定Q和R，只需确定三个未知变量E(ww^T)、和

b)Q和R同乘以一个常数对应的滤波增益K值不变这个特性，即Q和R的作用反映在 其元素相互间的比例而非具体数值上。

c)由于卡尔曼滤波器是基于线性最小误差方差估计的，理论上最优的状态估计值曲线 对实际值曲线符合最小二乘估计。

因此这里采用对三个未知变量E(ww^T)、和进行枚举的方式，利用前N 个观测到的样本数据，采用卡尔曼滤波方法，最终得到和真实曲线最符合最小二乘法的这 三个变量值。在具体的操作中，由于这里的枚举估计是粗略的，为了降低枚举的时间复杂 度，同时扩大变量间的取值空间，采用开方后再枚举的方法。

令$q=\sqrt{E\left({\mathrm{ww}}^T\right)},r1=\sqrt{E\left(v_{\theta }{v}_{\theta }^T\right)},r2=\sqrt{E\left(v_w{v}_w^T\right)}$

对q,r1,r2分别从1～10每间隔1进行枚举，每一次枚举结合测量到的300个数据 (N＝300)，通过卡尔曼滤波得到这300个样本数据的曲线，最终找出和真实曲线最符合最 小二乘法的曲线，并确定对应的E(ww^T)、和数值。最终得出 q＝10,r1＝1,r2＝1最好,因此确定出Q和R。

$Q=\left[100\right],R=\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right)$

④针对姿态传感器信息融合的Sage-husa卡尔曼自适应滤波简化算法。

由于在本模型中E[W_k]＝q_k＝0,E[V_k]＝r_k＝0,则传统的Sage-husa自适应滤波算法可 简化为：

${\tilde{Z}}_k=Z_K-H_k{\hat{X}}_{k,k-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{\tilde{Z}}_k$

$R_k=(1-d_{k-1})R_{k-1}+d_{k-1}({\tilde{Z}}_k{\tilde{Z}}_k^T-{\mathrm{HP}}_{k,k-1}{H}^T)$

$Q_{k=(1-d_{k-1})}{Q}_{k-1+d_{k-1}}[K_k{\tilde{Z}}_K{\tilde{Z}}_k^T{K}_k^T+P_k-{\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T]$

d_k＝(1-b)/(1-b^k+1)

其中0＜b＜1为遗忘因子,通常在0.95-0.99之间选取。由上面的式子可知，简化的 Sage-husa自适应卡尔曼滤波跟传统卡尔曼滤波的区别在于,利用每次得到的信息在每一步 的迭代中加入了对Q_k和R_k的动态估计，从而达到滤波自适应的效果。

⑤针对姿态传感器信息融合的Sage-husa卡尔曼自适应滤波改进算法。

(a)Q_k和R_k估计的改进方法。

Sage-husa自适应卡尔曼滤波算法本身也存在着缺点，就是在系统噪声方差阵和观测 噪声方差阵均未知的情况下，不能同时对两者进行估计，否则将会引起滤波器的发散，因 此不能对Q_k和R_k同时进行估计。

通过实验与分析发现，由于系统噪声方差阵Q_k在通过传感器模型初始化时的补偿后， 在滤波过程中变化不大；而测量噪声方差阵R_k对滤波的影响最为明显。因此在利用 Sage-husa自适应卡尔曼滤波方法时，对测量噪声方差阵R_k进行调整，而不对系统噪声Q_k进行调整。

(b)自适应滤波起作用的条件。

为了减少计算量，提高算法的实时性，本文设定了自适应滤波起作用的条件。

在滤波正常的情况下，即R_k-1＝R_k，可得：

${\tilde{Z}}_k={\mathrm{HX}}_k+V_k-H{\hat{X}}_{k,k-1}=H(X_k-{\hat{X}}_{k,k-1})+V_k$

则

$\left(\begin{array}{c}{\tilde{Z}}_k{\tilde{Z}}_k^T=[H(X_k-{\hat{X}}_{k,k-1})+V_k]{[H(X_k-{\hat{X}}_{k,k-1})+V_k]}^T\\ =H(X_k-{\hat{X}}_{k,k-1}){(X_k-{\hat{X}}_{k,k-1})}^T{H}^T+H(X_k-{\hat{X}}_{k,k-1})V_k^T+V_k{(X_k-{\hat{X}}_{k,k-1})}^T{H}^T+V_k{V_k}^T\end{array}\right)$

由于V_k是观测误差与预测误差相互独立，则有

$E\left[(X_k-{\hat{X}}_{k,k-1}){V_k}^T\right]=0,E\left[V_k{(X_k-{\hat{X}}_{k,k-1})}^T\right]=0$

所以在滤波正常情况下：

$E\left[{\tilde{Z}}_k{\tilde{Z}}_k^T\right]={\mathrm{HP}}_{k,k-1}{H}^T+R$

若滤波情况异常，实际的观测误差的方差必然会与上式差别较大。

因此采用判别式判断滤波是否异常。

${\tilde{Z}}_k{\tilde{Z}}_k^T>=\eta *\mathrm{tr}\left(E\right[{\tilde{Z}}_k{\tilde{Z}}_k^T\left]\right)$

其中η为常数可自己设定，tr为表示矩阵的迹。若k时刻成立，说明实际误差将超 过理论预计值的η倍，滤波发散，此时需要启动Sage-husa自适应卡尔曼滤波方法来预测 出新的R。当η＝1，为最严格的收敛判据条件；本实验设定η＝5。

3)FSR传感器信息融合。

①单脚支撑阶段ΔZMP

在单脚支撑阶段ΔZMP通过一个脚上4个FSR传感器测量到的力信息计算出ΔZMP的 值。单脚作为支撑脚时，FSR传感器安装位置见附图4。

其中，dxA,dyA，dxB,dyB，dxC,dyC，dxD,dyD是安装点位置A，B,C,D距离脚边沿x 轴，y轴方向的距离(其中x轴为机器人前进方向，y轴为机器人向左摆动方向)。FW表 示脚宽，FL表示脚长。

建立以O点为原点的x,y坐标系，A点、B点、C点和D点的坐标为和M为支撑多边形的支撑中心点。其中：

$\left(\begin{array}{c}{x}_{P_A}=\mathrm{FL}-\mathrm{dxA}\\ y_{P_A}=\mathrm{FW}-\mathrm{dyA}\end{array}\right)$      $\left(\begin{array}{c}{x}_{P_B}=\mathrm{FL}-\mathrm{dxB}\\ y_{P_B}=\mathrm{dyB}\end{array}\right)$

$\left(\begin{array}{c}{x}_{P_C}=\mathrm{dxC}\\ y_{P_C}=\mathrm{FW}-\mathrm{dyC}\end{array}\right)$      $\left(\begin{array}{c}{x}_{P_D}=\mathrm{dxD}\\ y_{P_D}=\mathrm{dyD}\end{array}\right)$

根据FSR传感器测出的力大小，可以求出单脚支撑时ZMP点坐标(x_ZMP,_yZMP)。

$\left(\begin{array}{c}{x}_{\mathrm{ZMP}}=\frac{f_A{x}_{P_A}+f_B{x}_{P_B}+f_C{x}_{P_C}+f_D{x}_{P_D}}{f_A+f_B+f_C+f_D}\\ y_{\mathrm{ZMP}}=\frac{f_A{y}_{P_A}+f_B{y}_{P_B}+f_C{y}_{P_C}+f_D{y}_{P_D}}{f_A+f_B+f_C+f_D}\end{array}\right)$

其中，f_A、f_B、f_C和f_D是支撑脚上4个FSR传感器测到的力数据。

中心M的坐标

$\left(\begin{array}{c}{x}_{P_M}=\frac{\mathrm{FL}}{2}\\ y_{P_M}=\frac{\mathrm{FW}}{2}\end{array}\right)$

所以

$\mathrm{\Delta ZMP}=\mathrm{ZMP}-P_M={\left(\begin{array}{cc}{x}_{\mathrm{ZMP}}-x_{P_M}& y_{\mathrm{ZMP}}-y_{P_M}\end{array}\right)}^T$

②双脚支撑阶段ΔZMP

在双脚支撑阶段ΔZMP通过每个脚上4个FSR传感器，共8个FSR传感器测量脚底安 装点的力，计算出ΔZMP的值。双脚作为支撑脚时，FSR传感器安装位置，见附图5。

其中，dxA1,dyA1，dxB1,dyB1，dxC1,dyC1，dxD1,dyD1是在左脚安装点位置A1， B1,C1,D1距离脚边沿x轴，y轴方向的距离。dxA2,dyA2，dxB2,dyB2，dxC2,dyC2， dxD2,dyD2是在右脚安装点位置A2，B2,C2,D2距离脚边沿x轴，y轴方向的距离。M点为 双足支撑时的几何中心。

建立以O点为原点的x,y坐标系，A1点、B1点、C1点、D1点和A2点、B2点、C2点、 D2点的坐标为和其中，

$\left(\begin{array}{c}{x}_{P_{A1}}=\mathrm{FL}-\mathrm{dxA}1\\ y_{P_{A1}}=\mathrm{FW}+s_y-\mathrm{dyA}1\end{array}\right)$   $\left(\begin{array}{c}{x}_{P_{B1}}=\mathrm{FL}-\mathrm{dxB}1\\ y_{P_{B1}}=s_y+\mathrm{dyB}1\end{array}\right)$

$\left(\begin{array}{c}{x}_{P_{C1}}=\mathrm{dxC}1\\ y_{P_{C1}}=\mathrm{FW}+s_y-\mathrm{dyC}1\end{array}\right)$   $\left(\begin{array}{c}{x}_{P_{D1}}=\mathrm{dxD}1\\ y_{P_{D1}}=s_y+\mathrm{dyD}1\end{array}\right)$

$\left(\begin{array}{c}{x}_{P_{A2}}=\mathrm{FL}+s_x-\mathrm{dxA}2\\ y_{P_{A2}}=\mathrm{FW}-\mathrm{dyA}2\end{array}\right)$   $\left(\begin{array}{c}{x}_{P_{B2}}=\mathrm{FL}+s_x-\mathrm{dxB}2\\ y_{P_{B2}}=\mathrm{dyB}2\end{array}\right)$

$\left(\begin{array}{c}{x}_{P_{C2}}=s_x+\mathrm{dxC}2\\ y_{P_{C2}}=\mathrm{FW}-\mathrm{dyC}2\end{array}\right)$   $\left(\begin{array}{c}{x}_{P_{D2}}=s_x+\mathrm{dxD}2\\ y_{P_{D2}}=\mathrm{dyD}2\end{array}\right)$

根据FSR传感器测出的力大小，可以求出双脚支撑时ZMP点坐标(x_ZMP,_yZMP)。

$\left(\begin{array}{c}{x}_{\mathrm{ZMP}}=\frac{f_{A1}{x}_{P_{A1}}+f_{B1}{x}_{P_{B1}}+f_{C1}{x}_{P_{C1}}+f_{D1}{x}_{P_{D1}}+f_{A2}{x}_{P_{A2}}+f_{B2}{x}_{P_{B2}}+f_{C2}{x}_{P_{C2}}+f_{D2}{x}_{P_{D2}}}{f_{A1}+f_{B1}+f_{C1}+f_{D1}+f_{A2}+f_{B2}+f_{C2}+f_{D2}}\\ y_{\mathrm{ZMP}}=\frac{f_{A1}{y}_{P_{A1}}+f_{B1}{y}_{P_{B1}}+f_{C1}{y}_{P_{C1}}+f_{D1}{y}_{P_{D1}}+f_{A2}{y}_{P_{A2}}+f_{B2}{y}_{P_{B2}}+f_{C2}{y}_{P_{C2}}+f_{D2}{y}_{P_{D2}}}{f_{A1}+f_{B1}+f_{C1}+f_{D1}+f_{A2}+f_{B2}+f_{C2}+f_{D2}}\end{array}\right)$

其中，f_A1、f_B1、f_C1、f_D1、f_A2、f_B2、f_C2和f_D2是支撑脚上8个FSR传感器测到的 力数据。

中心M的坐标

$\left(\begin{array}{c}{x}_{P_M}=\frac{\mathrm{FL}+s_x}{2}\\ y_{P_M}=\frac{\mathrm{FW}+s_y}{2}\end{array}\right)$

所以

$\mathrm{\Delta ZMP}=\mathrm{ZMP}-P_M={\left(\begin{array}{cc}{x}_{\mathrm{ZMP}}-x_{P_M}& y_{\mathrm{ZMP}}-y_{P_M}\end{array}\right)}^T$

(3)通过仿人机器人当前的步行状态、姿态信息和ZMP点位置信息，共同实现对 机器人稳定可控性的判定。提出模糊决策方法对步行阶段、姿态信息和ZMP点位置信息 进行融合，从而得到最终判定。步行阶段、姿态信息和ZMP点位置信息作为模糊决策的 输入，通过模糊决策后得到5种判定结果作为输出：可控状态，向左倒下，向右倒下，向 前倒下，向后倒下。由于判定机器人是否前后倒下，主要和前后方向的姿态信息和前后方 向的ZMP点位置信息有关，和左右方向的数据没有太大关系；同理左右方向上的姿态信 息和左右方向上的ZMP点位置和前后方向的数据也没有太大关系。所以前后方向倒和左 右方向倒的判定，分两个判定部分来完成，这样就大大的简化了判定方法和处理过程。整 个框架如附图2。

1)前后模糊决策系统

①输入函数隶属函数确定

a)机器人步行阶段(GS)

根据仿人机器人支撑脚和地面接触情况和离线步态规划把机器人步行阶段(GS)分 为4种状态，分别为双脚面接触支撑阶段Stage1,双脚非面接触支撑阶段Stage2，单脚面 接触支撑阶段Stage3，单脚非面接触支撑阶段Stage4。

由于机器人的步行阶段可以根据离线步态规划和脚底的8个FSR传感器信息来确定， 所以对机器人的步行状态不用进行模糊化，用下式来表示步行阶段GS。

$\mathrm{GS}\left(x\right)=\left(\begin{array}{cc}\mathrm{Stage}1& \mathrm{if><mo>=</mo><mn>1</mn>}\\ \mathrm{Stage}2& \mathrm{if><mo>=</mo><mn>2</mn>}\\ \mathrm{Stage}3& \mathrm{if><mo>=</mo><mn>3</mn>}\\ \mathrm{Stage}4& \mathrm{if><mo>=</mo><mn>4</mn>}\end{array}\right)$

b)ZMP点位置信息

对输入进行模糊化，把分为5种状态：NB、NS、Z、PS、PB；根据 多次实验的数据，设定每种状态隶属函数如附图6。

其中Z的隶属函数采用三角隶属函数；NS和PS隶属函数采用梯形隶属函数；NB和PB 的隶属函数采用半梯形隶属函数。

c)绕y轴俯仰角θ

对θ输入进行模糊化，把θ分为5种状态：NB、NS、Z、PS、PB；根据多次实验的数 据，设定每种状态隶属函数如附图7所示。其中NS、Z和PS的隶属函数采用三角隶属函 数，NB和PB的隶属函数采用半梯形隶属函数。

②输出隶属函数确定

由于决策系统的输出是一个确定的结果，由Fuzzy_fb表示，采用Sugeno模糊系统。 Sugeno模糊系统的输出是一个常数或是输入的函数，在本系统中，输出是一个常数，设 输出为常数-1时，表示要向前倒下FF；输出为常数0时，表示稳定可控NF；输出为常数 1时，表示要向后倒下BF。

③模糊推理

根据多次实验的经验，设计出推理规则，见表1(前后方向模糊决策系统的推理规则)。

表1

④去模糊化

去模糊化的方法采用隶属度最大的方法。

2)左右模糊决策系统

①输入函数隶属函数确定

a)机器人步行阶段GS

机器人步行阶段(GS)的设定和前后模糊决策系统设定的一样，一共分为4种状态：双 脚面接触支撑阶段Stage1,双脚非面接触支撑阶段Stage2，单脚面接触支撑阶段 Stage3，单脚非面接触支撑阶段Stage4。

b)ZMP点位置信息隶属函数设定。

ZMP点位置信息隶属函数和前后模糊决策系统中的隶属函数一样。把 分为5种状态：NB、NS、Z、PS和PB；每种状态隶属函数如附图8所示。

c)绕x轴滚动角φ隶属函数。

对绕x轴旋转角φ输入进行模糊化，把φ分为5种状态：NB、NS、Z、PS、PB；隶属函 数的选择和俯仰角θ类似，根据多次实验的数据，设定每种状态隶属函数如附图9所示。

②输出隶属函数确定

输出用Fuzzy_lr表示，和前后模糊决策系统类似，输出是一个常数，设输出为常数-1 时，表示要向左倒下LF；输出为常数0时，表示稳定可控NF；输出为常数1时，表示要向 右倒下RF。

③模糊推理

根据多次实验的经验，设计出推理规则见表2(左右方向模糊决策系统的推理规则)。

表2

④去模糊化

去模糊化的方法和前后模糊决策系统类似，采用隶属度最大的方法。

3)综合模糊决策

根据前后模糊决策系统可以得到机器人前后方向的稳定性状态，根据左右模糊决策 系统可以得到机器人左右方向的稳定性状态，把两者组合成一个二维向量来表示 ${\mathrm{Fuzzy}}_{\mathrm{out}}=\left(\begin{array}{c}{\mathrm{Fuzzy}}_{\mathrm{fb}}\\ {\mathrm{Fuzzy}}_{\mathrm{lr}}\end{array}\right),$这个向量代表了机器人综合稳定性状态。例如，${\mathrm{Fuzzy}}_{\mathrm{out}}=\left(\begin{array}{c}0\\ 0\end{array}\right)$表示 机器人处于可控状态；${\mathrm{Fuzzy}}_{\mathrm{out}}=\left(\begin{array}{c}1\\ 0\end{array}\right)$表示机器人倒向后方；${\mathrm{Fuzzy}}_{\mathrm{out}}=\left(\begin{array}{c}-1\\ 1\end{array}\right)$表示机器人倒 向前右方。一共有9种可能的输出结果，分别是：可控，向前方跌倒，向后方跌倒，向左 方跌倒，向右方跌倒，向前左方跌倒，前右方跌倒，向后左方跌倒，向后右方跌倒。

(4)稳定可控性综合判定实验

通过仿人机器人的实验，来对提出的稳定可控性综合判定方法进行测试。

1)机器人静态测试

把站立的仿人机器人向前推倒，接着用手扶起，再向后推倒，最后用手扶起来，来对 提出的机器人行走稳定可控性的判定方法进行验证。采集到姿态传感器数据见附图10和 ΔZMP_[-1 1]数据见附图11。

其中在附图10中，由于实验过程中在机器人倒地的时候，还没有和地接触前都被我 们用手接住，所以前后方向绕y轴俯仰角度θ在正负60度左右的时候，就表示机器人已经 倒地了，若是机器人在倒地的时候完全和地面接触时，这时姿态角度接近正负90度。根 据文中的前后模糊决策系统得到仿人机器人的稳定可控性综合判定输出结果，见附图12。 和仿人机器人实际动作进行比较，附图12中仿人机器人的稳定可控性综合判定输出是正 确的。

2)机器人动态测试

控制仿人机器人前进，机器人在前进的过程中向前摔倒来对本文提出的机器人行走 稳定可控性的判定方法进行验证。采集到姿态传感器数据见附图13和ΔZMP_[-1 1]数据见附 图14。

其中在附图13中，由于实验过程中机器人向前倒地的时候，还没有和地接触前就被 我们用手接住，所以前后方向绕y轴俯仰角度θ在-80度左右的时候，就表示机器人已经倒 地了，若是机器人在倒地的时候完全和地面接触时，这时姿态角度接近正负90度。根据 前后模糊决策系统得到仿人机器人的稳定可控性综合判定输出结果，见附图15。和仿人机 器人实际动作进行比较，附图15中仿人机器人的稳定可控性综合判定输出是正确的。

通过控制仿人机器人向前向后摔倒和在向前行走时向前摔倒2个实验来验证基于模糊 决策的机器人行走稳定可控性判定方法的有效性。

如上即可较好的实现本发明并取得前述的技术效果。