本发明涉及机器人技术领域，尤其涉及四足机器人足端触地检测方法及系统。
背景技术：
目前，四足机器人足端触地状态检测主要有以下三种方式：
一、根据步态规划判断当前时刻足端触地状态；
二、在足端部署布尔量传感器，如接触开关等，判断足端触地状态；
三、在足端部署气压或者压力传感器，检测足端与地面之间压力的模拟量，通过模拟量的大小判断足端接触状态。
上述三种方式各自具有以下缺陷：
方式一：要求地形平整，如果在崎岖地形或阶跃地形，根据规划得到的触地状态不可靠；
方式二：传感器安装困难，使用寿命低，长期使用不可靠；
方式三：传感器部署在足端，电气走线困难，传感器本身对机械结构、材料要求较高，整体部署成本高。
技术实现要素：
本发明为了解决上述技术问题提供四足机器人足端触地检测方法及系统。
本发明通过下述技术方案实现：
四足机器人足端触地检测方法，包括以下步骤：
s1，计算每条腿的足端离地高度，根据每条腿的足端离地高度计算出其足端高度接触概率；
根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
根据步态相位计算出步态规划接触概率；
s2，根据四条腿的步态规划接触概率、足端高度接触概率、膝关节扭矩接触概率，利用卡尔曼滤波融合得到最终的四腿足端接触概率；根据所述四腿足端接触概率，判断足端触地状态。
进一步的，采用公式（1）计算足端高度接触概率
式（1）中，
进一步的，采用公式（2）计算膝关节扭矩接触概率
式（2）中，
进一步的，四足机器人的步态是周期变化的，在每一个步态周期中步态相位在0-1之间递增，每个步态周期按腿部运动状态分为接触阶段和摆动阶段；
0≤步态相位<
在规划接触阶段，采用公式（3）计算步态规划接触概率
在规划摆动阶段，采用公式（4）计算步态规划接触概率
式（3）、式（4）中，
进一步的，采用递推公式（7）计算最终的四腿足端接触概率：
其中，
式（7）中，
其中，通过惯性传感器进行姿态解算得到机器人的质心位置和身体姿态，结合腿部电机编码器反馈的电机位置，计算得到每条腿的足端离地高度。
四足机器人本体足端触地检测系统，包括测量单元、输入单元和卡尔曼滤波单元；测量单元包括足端高度测量模块、足端高度接触概率测量模块和膝关节扭矩接触概率测量模块；输入单元包括步态规划接触概率计算模块；
足端高度测量模块：负责计算每条腿的足端离地高度；
足端高度接触概率测量模块：负责根据每条腿的足端离地高度计算出其足端高度接触概率；
膝关节扭矩接触概率测量模块：负责根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
步态规划接触概率计算模块：负责计算每条腿的步态规划接触概率；
卡尔曼滤波单元：负责根据输入的步态规划接触概率以及测量的足端高度接触概率、膝关节扭矩接触概率，计算并输出最终的足端触地概率。
与现有技术相比，本发明具有以下有益效果：
1，本发明在不部署多余的传感器在足端的前提下，依靠四足机器人本体基础传感器和融合算法精确估计足端与地面之间的接触状态，可解决传感器布线难题；
2，本发明即使在崎岖地形或阶跃地形，依然可得到可靠的触地状态。
附图说明
此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。
图1是本发明的电气原理图；
图2是本发明的流程图；
图3是步态规划接触概率模型的原理图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。
本发明公开的四足机器人足端触地检测方法，包括以下步骤：
s1，计算每条腿的足端离地高度，根据每条腿的足端离地高度计算出其足端高度接触概率；
根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
根据步态相位计算出步态规划接触概率；
s2，根据四条腿的步态规划接触概率、足端高度接触概率、膝关节扭矩接触概率，利用卡尔曼滤波融合得到最终的四腿足端接触概率；根据所述四腿足端接触概率，判断足端触地状态。
本发明公开的四足机器人本体足端触地检测系统，包括测量单元、输入单元和卡尔曼滤波单元；测量单元包括足端高度测量模块、足端高度接触概率测量模块和膝关节扭矩接触概率测量模块；输入单元包括步态规划接触概率计算模块；
足端高度测量模块：负责计算每条腿的足端离地高度；
足端高度接触概率测量模块：负责根据每条腿的足端离地高度计算出其足端高度接触概率；
膝关节扭矩接触概率测量模块：负责根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
步态规划接触概率计算模块：负责计算每条腿的步态规划接触概率；
卡尔曼滤波单元：负责根据输入的步态规划接触概率以及测量的足端高度接触概率、膝关节扭矩接触概率，计算并输出最终的足端触地概率。
基于上述四足机器人足端触地检测方法及系统，本发明公开一实施例。
实施例1
如图1所示，本实施例中四足机器人每条腿三个自由度，三个电机挂在同一条can总线上，可反馈电机位置、转速、扭矩等数据。数据转换模块负责can总线数据和rs485总线数据转换；主控单元通过rs485总线与数据转换模块进行数据通信。主控单元可选用通用主控计算机。
结合图2详细介绍四足机器人足端触地检测方法。
一、运动学模型：通过惯性传感器（imu）进行姿态解算得到的质心位置和身体姿态，结合电机编码器反馈的电机位置，计算得到每条腿的足端离地高度分别为：
二、假设足端触地时，足端离地高度服从均值为
式（1）中，
三、假设足端触地时，膝关节扭矩服从均值为
式（2）中，
四、步态规划接触概率模型
四足机器人的步态是周期变化的并事先规划好的，在每一个步态周期中步态相位从0-1之间递增。每个步态周期按腿部运动状态可以分为接触阶段和摆动阶段。0≤步态相位<
例如，最常见的对角步态，0≤步态相位<0.5为接触阶段，0.5≤步态相位<1为摆动阶段。
步态相位越接近接触相中点其触地概率越大，反之越接近接触相中点其触地概率越小，如图3所示。
4.1规划接触阶段，步态规划接触概率模型为式（3）：
4.2规划摆动阶段，步态规划接触概率模型为式（4）：
式（3）、式（4）中，
五、将步态规划接触概率作为系统输入，将足端高度接触概率和膝关节扭矩接触概率作为系统测量，利用卡尔曼滤波融合得到最终的四腿足端触地概率，具体计算方法如下：
1，卡尔曼滤波状态转移方程如式（5）所示：
式（5）中，状态变量
2，卡尔曼滤波观测方程如式（6）所示：
式（6）中，
3，根据卡尔曼滤波状态转移方程（5）和观测方程（6）可得到卡尔曼滤波的递推公式（7）：
（7）中，
4，采用公式（7）计算出最终的四腿足端接触概率
本发明在不部署多余的传感器在足端的前提下，依靠四足机器人本体基础传感器和融合算法精确估计足端与地面之间的接触状态。
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
1.四足机器人足端触地检测方法，其特征在于：包括以下步骤：
s1，计算每条腿的足端离地高度，根据每条腿的足端离地高度计算出其足端高度接触概率；
根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
根据步态相位计算出步态规划接触概率；
s2，根据四条腿的步态规划接触概率、足端高度接触概率、膝关节扭矩接触概率，利用卡尔曼滤波融合得到最终的四腿足端接触概率；根据所述四腿足端接触概率，判断足端触地状态。
2.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：采用公式（1）计算足端高度接触概率
式（1）中，
3.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：采用公式（2）计算膝关节扭矩接触概率
式（2）中，
4.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：四足机器人的步态是周期变化的，在每一个步态周期中步态相位在0-1之间递增，每个步态周期按腿部运动状态分为接触阶段和摆动阶段；
0≤步态相位<
在规划接触阶段，采用公式（3）计算步态规划接触概率
在规划摆动阶段，采用公式（4）计算步态规划接触概率
式（3）、式（4）中，
5.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：采用递推公式（7）计算最终的四腿足端接触概率：
其中，
式（7）中，
6.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：通过惯性传感器进行姿态解算得到机器人的质心位置和身体姿态，结合腿部电机编码器反馈的电机位置，计算得到每条腿的足端离地高度。
7.四足机器人本体足端触地检测系统，其特征在于：包括测量单元、输入单元和卡尔曼滤波单元；测量单元包括足端高度测量模块、足端高度接触概率测量模块和膝关节扭矩接触概率测量模块；输入单元包括步态规划接触概率计算模块；
足端高度测量模块：负责计算每条腿的足端离地高度；
足端高度接触概率测量模块：负责根据每条腿的足端离地高度计算出其足端高度接触概率；
膝关节扭矩接触概率测量模块：负责根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
步态规划接触概率计算模块：负责计算每条腿的步态规划接触概率；
卡尔曼滤波单元：负责根据输入的步态规划接触概率以及测量的足端高度接触概率、膝关节扭矩接触概率，计算并输出最终的足端触地概率。
技术总结