发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何解决机器人普遍存在自主稳定性不高，地形适应性差，外部滑移扰动未知，对工作环境的依赖性强，自由度繁多等缺点从而增加了控制系统的复杂性以及运动过程中缺乏对实时足地滑移估计能力的问题，从而提出一种电驱动哺乳构型足式机器人足地滑移估计方法。

(二)技术方案

为了解决上述技术问题，本发明提供了一种电驱动哺乳构型足式机器人足地滑移估计方法，包括以下步骤：

步骤一、设计哺乳构型足式机器人本体传感数据解算方法，得到的机器人在导航坐标系中的加速度；

步骤二、将步骤一验证得到的机器人在导航坐标系中的加速度，作为基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架的输入变量，从而建立基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架，得到哺乳构型足式机器人位置、速度估计变量；

步骤三、将步骤二得到的哺乳构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立哺乳构型足式机器人足地滑移估计模型，得到哺乳构型足式机器人足地滑移加速度模型；

步骤四、利用步骤三得到的哺乳构型足式机器人足地滑移加速度模型，建立基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法；

步骤五、基于步骤一至步骤四进行哺乳构型足式机器人的足地滑移检测。

优选地，步骤一中，对惯导等传感器的数据进行标定及补偿，通过标定加速度及角加速度信号来抑制哺乳构型牵引力运动方向速度和姿态解算的误差，最终输出准确的哺乳构型机器人牵引力方向本体速度及三维姿态信息，惯导传感实时数据具体解算结果为：

式中，为导航坐标系到机体坐标系的转换矩阵，f_B为空间中的真实加速度和重力导致的加速度之差的力矢量，g_n为重力矢量，为地球自转角度在导航坐标系中的投影，为机器人在导航坐标系中的速度，v_n为机器人在导航坐标系中的速度。

优选地，步骤二中，将步骤一验证得到的机器人在导航坐标系中的加速度作为基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架的输入变量从而建立基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架。

优选地，步骤二中，根据足式机器人单腿及质心状态变量，建立状态估计过程中的预测模型，其具体建模结果为：

式中，f为三维方向的加速度真实值；b_f为加速度计的偏差；b_w为角加速度计的偏差；w_f为加速度计的随机误差；w_w为角加速度计的随机误差；w为三维方向的角速率真实值；为三维方向的加速度测量值；为三维方向的角速率测量值；w_bf为加速度计噪声矩阵；w_bw为角加速度计噪声矩阵。

建立状态估计过程中的测量模型，其具体建模结果为：

式中，p_c为机器人的位置修正量；v_c为机体的速度修正量；p为机器人的位置测量变量；v为机体的速度测量变量。

根据状态变量的预测更新和测量更新，基于反馈校正方法得到足式机器人的状态变量结果如下所示：

式中，为位置估计值，为速度估计值，Δt为时间周期变量；为导航坐标系转换矩阵；

优选地，步骤三中，将步骤二公式(4)计算得到的哺乳构型足式机器人位置、速度估计变量，用于作为单腿动力学方程的输入变量，建立哺乳构型足式机器人足地滑移估计模型.

优选地，步骤三中，首先建立足式机器人单腿滑移的动力学模型，根据哺乳构型足式机器人行走时的单腿滑移模型，得到两连杆的位置矢量：

式中，^LP_sy为牵引方向滑移距离，^LP_sz为法向方向滑移距离，M_H为大腿连杆的质量，L_H为大腿长度；M_K为小腿连杆的质量，L_K为小腿长度，q_H为髋关节角度，q_K为膝关节角度；

定义滑移状态下的动力学系统输入变量为q＝[q_K q_H ^LP_sy ^LP_sz]^T，足-地接触力定义为F＝[F_T F_N]^T，F_T代表足端所受到的切向力，F_N代表足端所受到的法向力。

基于拉格朗日方程，建立单腿三自由度的运动动力学方程，如下所示：

式中，D_d(q)为惯性矩阵，为哥氏力矩阵，G_d(q)为重力矩阵，C_d为重力矩阵，q为关节角度，为关节角速度，为关节角加速度，u为重力矩阵，E_d为重力矩阵，F为足力矢量。

根据单腿滑移动力学运动方程(6)，建立哺乳构型足端牵引力方向滑移加速度模型，如下所示：

式中F_T为足地牵引向力参数，M_H为大腿连杆的质量，L_H为大腿长度，M_K为小腿连杆的质量，L_K为小腿长度，q_H为髋关节角度，q_K为膝关节角度。

优选地，步骤四中，利用步骤三计算得到的哺乳构型足式机器人足地滑移加速度模型公式(7)，建立基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法.

优选地，步骤四中，建立哺乳构型足式机器人股节和胫节的转动速度变量分别w_Hz，w_Az：

式中w_Hx,w_Hy,w_Hz分别为股节沿牵引、侧向和法向的转动速度变量；w_Kx,w_Ky,w_Kz分别为胫节沿牵引、侧向和法向的转动速度变量。

根据股节和胫节的转动速度，建立哺乳构型足式机器人足地滑移状态变量方程：

式中为足地滑移速度变量，f_s(y_s,w_s1)为足地滑移加速度变量。

最终根据公式(7)得到的哺乳构型足式机器人的足端滑移加速度为：

式中F_T为足地牵引向力参数，M_H为大腿连杆的质量，L_H为大腿长度，M_K为小腿连杆的质量，L_K为小腿长度，q_H为髋关节角度，q_K为膝关节角度；

哺乳构型足式机器人的足端滑移速度解算为：

式中，为足地速度矢量，为状态估计得到的基节的侧向速度。

哺乳构型足式机器人的足端滑移位置解算为：

式中，^LP_sy为足地位置矢量，q_K0为膝关节初始角度，q_H0为髋关节初始角度，为状态估计得到的基节的侧向速度，Δt为时间周期变量。

根据式(11)，(12)，(13)最终得到哺乳构型足式机器人的滑移状态方程为：

式中w_sy为白噪声矩阵，协方差矩阵为Q_sy。

基于扩展卡尔曼滤波的循环迭代方法对两种测量数据的信息进行融合得到足地滑移估计值p_s(y_s,w_s3)，如图4所示，定义哺乳构型足式机器人牵引方向的足地滑移率S_I(t)为：

^LP_yei为控制周期结束时，由正运动学计算出的足端在机器人坐标系下的位置；^LP_ysi为控制周期开始时，由正运动学计算出的足端在机器人坐标系下的位置；当滑移率处于S_I(t)∈[0,1)这个范围内时，证明哺乳构型足式机器人的运动过程为有效运动。

优选地，步骤五中，基于步骤一至步骤四得到的哺乳构型足式机器人的足地滑移检测流程如下：

步骤1：哺乳构型足式机器人的足地滑移检测起始于足端落地时刻，通过足力传感器检测落地时刻；

步骤2：利用步骤1足力传感器检测到的足地落地时刻状态，进行支撑相和摆动相的切换，通过哺乳构型足式机器人本体传感数据解算得到惯导和运动学状态变量；

步骤3：利用步骤2计算得到的惯导和运动学状态变量，作为状态估计框架中的输入变量。通过基于扩展卡尔曼滤波的哺乳构型足式机器人状态估计框架，基于惯导、运动学及足力数据融合得到的哺乳构型足式机器人位置、速度状态变量；

步骤4：利用步骤3计算得到的位置、速度、姿态状态变量，作为滑移估计的输入变量。通过哺乳构型机器人足地滑移估计模型，建立哺乳构型足端牵引力方向滑移加速度模型；

步骤5：利用步骤4计算得到的滑移加速度模型，作为足地滑移状态估计的判定基础。通过基于扩展卡尔曼滤波的哺乳构型足地滑移状态估计方法，建立哺乳构型足式机器人的足地滑移率检测条件，检测足地滑移情况；

步骤6：利用步骤5判定得到的足地滑移情况，通过足力传感器检测判断支撑相是否结束，当支撑相结束后通过步态切换转变成摆动相。

本发明还提供了一种所述方法在足式机器人技术领域中的应用。

(三)有益效果

通过对一种电驱动哺乳构型足式机器人足地滑移估计方法的研究，使得本发明所提哺乳构型足式机器人在牵引力方向移动过程中具有滑移状态实时检测功能，并可以通过检测到的足地滑移情况实现足地滑移抑制功能，并提高足式机器人控制的运动稳定性。