一种基于脚力估计算法的多足机器人动态稳定性定量判定方法技术

本发明专利技术提供了一种基于脚力估计算法的多足机器人动态稳定性定量判定方法，适用于机器人行走于非结构化环境时的稳定性实时检测。本发明专利技术由传感器获得关节和机身运动状态后，通过正、逆运动学求得计算稳定裕度所需的变量值。然后通过脚力估计算法估计支撑腿的脚力，由脚力大小得到最易发生倾翻的支撑轴和落足点。由脚力在质心处的合力和合力矩计算对支撑轴和落足点的倾翻有效合力，进而可以由公式得到支撑轴和落足点的稳定角，选取稳定角中的最小者由公式计算得到机器人的归一化的力角稳定裕度值。本发明专利技术的优点在于：本方法直接判断对支撑轴和落足点的稳定裕度；可以表征质心的高低带来的稳定性变化；不需要采用易损的多维力传感器测量脚力。

【技术实现步骤摘要】

本专利技术属于机器人控制领域，具体是一种基于脚力估计算法的机器人动态稳定性定量判定方法，实时检测多足机器人运动过程中的稳定裕度。
技术介绍
多足机器人是一种具有多支链、冗余和时变拓扑结构的复杂机器人，具有比轮式机器人更强的地形适应能力，在军事和民用领域具有巨大的应用潜力和广泛的应用前景，受到国内外机器人研究者的广泛关注和重视。多足机器人一般按足的数目可分为四足、六足和八足机器人。由于多足机器人行走的环境往往较为复杂，可能会遇到崎岖地形，楼梯，障碍物，甚至滚石等，这些复杂地形和环境干扰对机器人的稳定性带来了很大的挑战。采用准确的稳定性判据实时监控机器人的稳定性对于保持机器人平衡，保证机器人不倾翻，完成救灾任务是至关重要的。机器人的稳定性判定主要是指由一定的稳定裕度值判断机器人会不会出现倾翻摔倒，稳定裕度大于零时机器人处于稳定状态，稳定裕度小于零时机器人将会出现倾翻，即机器人失稳。目前，机器人的稳定性判定方法分为静态稳定判据和动态稳定性判据。静态稳定性判据没有考虑惯性力和外力对稳定性的影响，静态稳定性判据主要有两类，一类是基于重心投影与支撑多边形关系的方法，他们有重心投影法，静态稳定边界法，纵向稳定边界法和偏转纵向稳定边界法。另一类是基于能量的静态稳定判据，它们有能量稳定边界法，倾覆能量稳定边界法和规范化的能量稳定性边界法。动态稳定性判据是指考虑了惯性力和外力影响的稳定性判据，主要分为两类。第一类是基于零力矩点（ZMP,ZeroMomentPoint）的动态稳定性判据。南斯拉夫学者MiomirVukobratovic于1970年提出了ZMP稳定性判据，该方法成为了应用于两足机器人稳定性判定的经典方法，此后众多学者在此概念的基础上提出了自己的判定方法，如压力中心法、有效质量中心法等，其实质都属于ZMP理论。然而有学者指出ZMP方法无法用于行走于非平面地形上的多足机器人的稳定性判定，因为由ZMP的定义可知，ZMP点是惯性力与重力的合力与脚底支撑平面的交点，当机器人支撑腿的数目大于等于四条时，地面地形为非平面地形时，机器人脚与地面的接触点将不会共面，此时ZMP点将无法求得。另外一类动态稳定性判定方法是基于动量的稳定性判据，主要有动态稳定边界法，倾倒稳定性判别法和支撑力矩判别法。这类方法对于具有外力扰动，地面不平和惯性力存在的情况也是完全可行的，但是这类判据没有考虑质心位置变动对稳定性的影响，因为对于一个地面上的物体而言，其质心位置距离地面越高稳定性越差。本专利技术利用多足机器人位姿信息和足端脚力信息对机器人进行动态稳定性判断，不利用支撑平面判断稳定性，直接判断绕支撑轴的倾翻和绕落足点的倾翻，使该方法能够适用于不平路面的情形，而且考虑了质心位置变动对机器人稳定性的影响。无论是基于ZMP的稳定性判定方法，还是本专利技术提出的稳定性判定方法，都需要采用足底与地面接触力计算，因此正确检测足底接触力成为关键和先决条件。目前都采用在足底安装FSR（压力传感器）或者多维力传感器的方法实现对足底力的检测。但是FSR只能检测正压力，无法检测其余方向的力；多维力传感器价格昂贵，且易损坏。本专利技术提出一种基于动力学模型的脚力估计算法，通过机器人的运动状态和电机扭矩估算外界接触力，可以有效检测足底力的大小和方向，而无需力传感器。
技术实现思路
本专利技术的目的在于提出一种基于脚力估计的多足机器人动态稳定性定量判定方法，该方法考虑了惯性力、外力干扰，而且考虑了质心变动对稳定性的影响，具有计算量少、可靠性高等特点，且采用脚力估计方法替代易损的多维力传感器，降低机器人制造成本，提高检测可靠性。本专利技术提供的一种基于脚力估计算法的多足机器人动态稳定性定量判定方法，其特征在于，该方法包括下述步骤：步骤1：实时获取关节角度、机身位姿信息，并利用这些信息对机器人进行正运动学计算，得到机器人各个构件的质心在参考坐标系中的坐标，以及各支撑腿足端点的坐标；步骤2：利用质心计算公式计算机器人总体质心在参考坐标系中的坐标；步骤3：采用脚力估计算法实时获取支撑腿的脚力信息，并向机器人质心处等效各个脚力，得到对机器人质心的合力和合力矩；步骤4：计算最易发生倾翻的两条支撑轴的矢量，与脚力最大的脚相连的两条轴线为最易发生倾翻的两条轴线；步骤5：分别计算合力和合力矩对两条最易发生倾翻的轴线及步骤4中脚力最大的脚的倾翻有效合力；步骤6：分别计算步骤5中得到的三个倾翻有效合力与倾翻轴过质心的垂线的夹角和质心与脚力最大落足点的连线的夹角，三个夹角分别为相应轴线和落足点的稳定角；步骤7：选取步骤6中得到的三个夹角中最小者作为机器人的稳定裕角，稳定裕角与合力相乘后作归一化处理得到稳定裕度值。本专利技术是一种基于脚力估计算法的多足机器人动态稳定性检测方法，用于多足机器人在非结构化环境下运动时稳定性判定，判定方法简洁明确。本专利技术的核心是由脚力估计算法估计足底地面接触力，所有脚力在质心处合成合力和合力矩，然后计算对两条最易发生倾翻的支撑轴和落足点的稳定裕度角。在脚力大小和方向都不变的情况下，如果机器人质心高度增高，有效合力与倾翻轴线过质心的垂线之间的夹角一定会减小，因此该方法能够反映质心变动对稳定性带来的影响。附图说明图1是多足机器人动态稳定性定量检测方法流程图。图2是六足机器人结构与坐标系示意图。图3是绕倾翻轴稳定裕角计算示意图。图4是绕落足点稳定裕角计算示意图。图5是脚力估计算法计算框图。具体实施方式以下结合实施例及附图对本专利技术作进一步详细的说明，但本专利技术实施方式不限于此。该具体实施例选取多足机器人中六足串联机器人（如图2所示）作为作用对象，但实施对象不限于此，四足、八足或并联机器人等同样适用。如图1所示，本专利技术实例提供的一种基于脚力估计算法的多足机器人动态稳定性定量判定方法包括以下步骤。步骤1：实时获取关节角度、机身位姿信息，并利用这些信息对机器人进行正运动学计算，得到机器人各个构件的质心在参考坐标系中的坐标，以及各支撑腿足端点的坐标。图2中Oxyz为参考坐标系，由电机绝对值编码器和机身位姿传感器可以进行机器人正逆运动学计算，从而可以得到图2中所示的各支撑腿落足点在参考坐标系Oxyz中的坐标pi（i=1,2,…,n），同时也可以得到各个构件的坐标。步骤2：利用质心计算公式计算机器人总体质心在参考坐标系中的坐标。其中，r和s分别表示多足机器人腿的个数和每条腿含有独立构件的个数，mb表示机身的质量，mij表示第i条腿第j个构件的质量。步骤3：采用脚力估计算法实时获取支撑腿的脚力信息，并向机器人质心处等效各个脚力，得到对机器人质心的合力和合力矩。支撑腿脚力信息由脚力估计算法估计得到，具体如图5所示。首先需要预先建立机器人腿部机构动力学方程和关节摩擦模型，并对未知参数进行参数辨识，得到精确的机器人腿部动力学模型。机器人在行走过程中，伺服电机可以实时将电机电流信息和编码器得到的角速度信息上传至控制器。动力学模型中需要用到关节的角度、角速度和角加速度，而电机编码器只能得到角速度，因此需要状态观测器估计角度和角加速度，而且编码器直接输出的角速度是带有噪声的信号，状态观测器也可以滤除噪声。本专利技术中采用的状态观测器是卡尔曼滤波器。设，则通过卡尔曼滤波器，即可得到状态的最优估计值、和。由于电机不是直接驱动连杆本文档来自技高网...

【技术保护点】
一种基于脚力估计算法的多足机器人动态稳定性定量判定方法，其特征在于，该方法包括下述步骤：(1)实时获取关节角度、机身位姿信息，并利用这些信息对机器人进行正运动学计算，得到机器人各个构件的质心在参考坐标系中的坐标，以及各支撑腿足端点的坐标；(2)利用质心计算公式计算机器人总体质心在参考坐标系中的坐标；(3)采用脚力估计算法实时估计支撑腿的脚力信息，并向机器人质心处等效各个脚力，得到对机器人质心的合力和合力矩；(4)计算最易发生倾翻的两条支撑轴的矢量，与脚力最大的脚相连的两条轴线为最易发生倾翻的两条轴线；(5)分别计算合力和合力矩对两条最易发生倾翻的轴线及步骤4中脚力最大的脚的倾翻有效合力；(6)分别计算步骤5中得到的三个倾翻有效合力与倾翻轴过质心的垂线的夹角和质心与脚力最大落足点的连线的夹角，三个夹角分别为相应轴线和落足点的稳定角；(7)选取步骤6中得到的三个夹角中最小者作为机器人的稳定裕角，稳定裕角与合力相乘后作归一化处理得到稳定裕度值。

【技术特征摘要】
1.一种基于脚力估计算法的多足机器人动态稳定性定量判定方法，其特征在于，该方法包括下述步骤：(1)实时获取关节角度、机身位姿信息，并利用这些信息对机器人进行正运动学计算，得到机器人各个构件的质心在参考坐标系中的坐标，以及各支撑腿足端点的坐标；(2)利用质心计算公式计算机器人总体质心在参考坐标系中的坐标；(3)采用脚力估计算法实时估计支撑腿的脚力信息，并向机器人质心处等效各个脚力，得到对机器人质心的合力和合力矩；(4)计算最易发生倾翻的两条支撑轴的矢量，与脚力最大的脚相连的两条轴线为最易发生倾翻的两条轴线；(5)分别计算合力和合力矩对两条最易发生倾翻的轴线及步骤4中脚力最大的脚的倾翻有效合力；(6)分别计算步骤5中得到的三个倾翻有效合力与倾翻轴过质心的垂线的夹角和质心与脚力最大落足点的连线的夹角，三个夹角分别为相应轴线和落足点的稳定角；(7)选取步骤6中得到的三个夹角中最小者作为机器人的稳定裕角，稳定裕角与合力相乘后作归一化处理得到稳定裕度值。2.根据权利要求1所述的多足机器人的动态稳定性定量判定方法，其特征在于，步骤(3)中，由脚力估计算法估计脚力大小和方向。首先采用状态估计器由编码器得到的电机速度估计角位移和角加速度，得到状态的最优估计值和再根据扰动观测器得到关节有效转矩根据动力学模型计算驱动机构运动所需关节扭矩关节驱动力包含两部分，一部分是无外力作用时驱动机构运动所需驱动力，另一部分是外力作用对关节驱动力的影响，则可...

【专利技术属性】
技术研发人员：钟国梁，陈龙，邓华，辛桂阳，
申请(专利权)人：中南大学，
类型：发明
国别省市：湖南;43