一种基于双阈值法ZMP的六足机器人失稳调节算法的制作方法

本发明涉及六足机器人技术领域，具体是一种基于双阈值法zmp的六足机器人失稳调节算法。

背景技术：

六足机器人又叫蜘蛛机器人，是多足机器人的一种。由于复杂地形或危险作业环境，对于施工或者其他作业存在很大的安全隐患，所以需要可适应复杂地形或者危险环境进行移动和工作的多足机器人。由于环境和任务的复杂性要求机器人具有良好的适应能力和灵活的移动能力，六足机器人作为多足机器人的其中一种，以其灵活的自由度较受青睐，长期以来其在非结构环境下的协调稳定运动技术一直是机器人领域研究的热点之一。

目前来说，受限于地形原因，特别是崎岖地形上，要求六足机器人需要保持稳定状态，基于此，本申请提供了一种基于双阈值法zmp的六足机器人失稳调节算法。

技术实现要素：

本发明的目的在于提供一种基于双阈值法zmp的六足机器人失稳调节算法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种基于双阈值法zmp的六足机器人失稳调节算法，包括以下步骤：

s1，设定足端压力阈值n1和机身姿态角的阈值n2，当六足机器人足端压力的输出值大于n1且机身姿态角大于n2时，则判定为机器人失稳状态，否则为稳定状态，稳定状态下的六足机器人以三足行走步态运动；

s2，当六足机器人被判定为失稳状态后，将三足行走步态转换为五足行走步态，六足机器人的六条腿单个依次循环运动，无论何时都至少有五条腿支撑地面；

s3，同时利用zmp法构建六足机器人稳定判据，通过得到的六足机器人机身的姿态信息，经过计算得到六足机器人稳定判据，作为对六足机器人机身姿态调整的依据；

s4，再对zmp稳定裕量所对应边涉及到的两条腿进行调整，使六足机器人机身抬高或降低，从而使稳定裕量恢复到安全地范围内，再重复上述步骤，使六足机器人保持稳定的行走步态。

作为本发明进一步的方案：步骤s1中，机身姿态角包括翻滚角θ和俯仰角φ，与之对应的阈值n2包括翻滚角阈值θn2与俯仰角阈值φn2。

作为本发明进一步的方案：所述翻滚角θ和俯仰角φ为：

式中，gx,gy,gz为六足机器人机身三轴加速度计的输出值。

作为本发明再进一步的方案：当翻滚角ф与俯仰角φ任意一个超出所设定的阈值时，六足机器人均为失稳状态。

作为本发明再进一步的方案：步骤s3中，阈值n1和阈值n2判定顺序为：当足端压力在阈值n1的范围内时，不对三轴加速度计z轴输出量进行判断，判定为六足机器人为稳定状态；当足端压力超出阈值n1时，才对三轴加速度计z轴输出量进行判断，若三轴加速度计z轴输出量在阈值n2的范围内，则判定为六足机器人为稳定状态，否则判定为六足机器人为失稳状态。

作为本发明再进一步的方案：所述zmp稳定裕量的获得方法包括以下步骤：

(1)根据当前姿态下六足机器人与地面所接触点形成的支撑多边形各顶点坐标，求得zmp零力矩点的坐标；

(2)根据zmp零力矩点的坐标求得zmp零力矩点至支撑多边形各边的距离；

(3)通过比较得到的最小值即为六足机器人的zmp稳定裕量。

作为本发明再进一步的方案：所述六足机器人机身姿态调整的依据为sm＞0，sm为zmp零力矩点到支撑多边形的最短距离。

与现有技术相比，本发明的有益效果是：在失稳判定方面，结合足端压力值与机身姿态角两个信息，分别设定两个阈值，并设定了阈值判定顺序，在提高失稳判定准确度的同时优化了失稳判定的效率，在失稳调整方面，结合zmp失稳判据进行机身姿态矫正，首先对五足步态下六足机器人的支撑多边形坐标进行求解，其次根据定义求得zmp点的坐标，最后求的zmp点到支撑多边形各边的距离，将最小距离作为zmp稳定判据，从而对机身姿态进行调整，使六足机器人重新恢复稳定。

附图说明

图1为六足机器人的三足步态示意图。

图2为数据坐标系与世界坐标系的结构示意图。

图3为双阈值法的判断流程图。

图4为zmp法的结构示意图。

图5为机身平面与投影平面的关系图。

图6为翻滚角对机身平面的影响示意图。

图7为俯仰角对机身平面的影响示意图。

图8为六足机器人的五足行走步态示意图。

图9为六足机器人的失稳调整过程图。

图10为一种基于双阈值法zmp的六足机器人失稳调节算法的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

请参阅图1～10，本发明实施例中，一种基于双阈值法zmp的六足机器人失稳调节算法，包括以下步骤：

s1，设定足端压力阈值n1和机身姿态角的阈值n2，当六足机器人足端压力(由足端压力传感器测量)的输出值大于n1且机身姿态角(由三轴加速度计测量，三轴加速度计选用自带融合滤波算法的加速度计mpu9250)大于n2时，则判定为机器人失稳状态，否则为稳定状态，稳定状态下的六足机器人以三足行走步态运动，所谓的三足步态即为：将六足机器人的六条腿分为两组，如图1所示，使1、3、5腿为第一组，2、4、6腿为第二组。两组腿交替进行支撑、抬起、旋转等动作，使六足机器人前进，在任意时刻都至少保证一组腿支撑地面，形成一个三角形的稳定支撑机构；

具体的来说：本实施例中的三足步态，在六足机器人稳定行进时，每条支撑腿的足端受力应为其中m为六足机器人的机身质量，g为当地的重力加速度。考虑到行进过程中六足机器人的惯性作用，需要设定一定的的裕量，即足端压力阈值n1为fn1。

所述的机身姿态角包括翻滚角θ和俯仰角φ，与之对应的阈值n2包括翻滚角阈值θn2与俯仰角阈值φn2。根据三轴加速度计的输出值构建数据坐标系，由于六足机器人在行走过程中三轴加速度计会发生偏转，使得数据坐标系与世界坐标系之间存在夹角。六足机器人姿态出现偏差是相对世界坐标系而言的，因此世界坐标系内z轴方向上的重力加速度会分布在数据坐标系的三个轴上，根据三个轴的输出数据可以得出六足机器人的翻滚角ф、俯仰角θ和航向角ψ。所谓翻滚角ф即为两坐标系x轴之间的夹角，俯仰角θ即为两坐标系y轴方向上的夹角，航向角ψ即为两坐标系z轴方向的夹角，数据坐标系与世界坐标系的几何关系如图2所示。

根据几何关系计算，由数据坐标系转换到世界坐标系的过程可表示为：

当六足机器人处于水平状态时，数据坐标系与世界坐标系重合，由于重力加速度g位于世界坐标系的z轴负方向，所以此时该坐标系内存在的向量为(0,0，-g)。当六足机器人失稳时，数据坐标系发生偏转，不再与世界坐标系重合，重力加速度会分解到数据坐标系的三个轴上输出，此时三轴加速度计输出的向量为(gx,gy,gz)，其中gx,gy,gz分别为三轴加速度计在x,y,z轴输出值。将两个向量代入到上式中中，得到：

对其整理得：

由此可计算出六足机器人失稳状态下的翻滚角ф与俯仰角θ，从而准确地描述出六足机器人机身的倾斜状态。通过设置翻滚角阈值θn2与俯仰角阈值φn2即可以判断六足机器人是否失稳，翻滚角ф与俯仰角θ任意一个超出所设定的阈值，六足机器人都会失稳。

为了提高对六足机器人失稳判定的准确性，提高六足机器人运行速度，本文设定阈值判断顺序，如图3所示。当足端压力传感器的输出数据在阈值n1的范围内时，不对加速度计进行判断，判定为六足机器人稳定；当足端压力传感器的输出数据超出阈值n1时，才对加速度计的输出值进行判断，若加速度计的输出数据在阈值n2的范围内，则判定为六足机器人稳定，否则判定为六足机器人失稳；

s2，当六足机器人被判定为失稳状态后，将三足行走步态转换为五足行走步态，六足机器人的六条腿单个依次循环运动，无论何时都至少有五条腿支撑地面，五足行走步态是六足机器人行走方式中最稳定的步态；

s3，同时利用zmp法构建六足机器人稳定判据，通过得到的六足机器人机身的姿态信息，经过计算得到六足机器人稳定判据，作为对六足机器人机身姿态调整的依据；

s4，再对zmp稳定裕量所对应边涉及到的两条腿进行调整，使六足机器人机身抬高或降低，从而使稳定裕量恢复到安全地范围内，再重复上述步骤，使六足机器人保持稳定的行走步态。

当六足机器人在实际的崎岖路面上行进时，不可避免的受到惯性力、外力扰动等干扰，基于静态的稳定性判定方法不适合处于运行状态的六足机器人，本实施例采用基于合外力矩zmp的方法来进行稳定裕量的构建。zmp方法是指水平面上使x,y方向上的力矩为零的零力矩点至支撑多边形(即六足机器人的腿与地面所接触点形成的多边形)投影各边距离中的最小值，如图4所示。

六足机器人在发生倾翻时，其机身平面在水平面的投影情况如图5所示，需要求得投影多边形各边的长度以及相应的几何关系，才能建立调整过程中的支撑多边形，进而构建zmp失稳判据。

因为六足机器人的腿部分布在机身平面的六个顶点上，构建支撑平面需要六个顶点在水平面上的投影情况，需要得到六个投影点的坐标。首先求翻滚角的影响，如图6所示。

设六足机器人的机身长度为l，机身高度为h，则求得各个顶点的坐标为：a(-l,0,h)、d(l,0,h)、

根据旋转坐标轴的关系，设a’点的坐标为(x,0,z)，根据余弦定理和空间内两点的距离公式可得：

得到：

a'(-lcosφ,0,h-lsinφ)；

同理可得：

d'＝(lcosφ,0,h+lsinφ)；

其次求俯仰角对机身平面投影点的影响，如图7所示。

对于俯仰角来说，由上式可得：

a'(-lcosφ,0,h-lsinφ)；

d'＝(lcosφ,0,h+lsinφ)；

根据余弦定理和空间内两点的距离公式，设a”(-lcosφ,y,z)，得到：

a”＝(-lcosφ,-lsinφsinθ,h-lsinφcosθ)；

同理可得：

d”＝(-lcosφ,lsinφsinθ,h+lsinφcosθ)；

设zmp零力矩点的坐标为(xzmp,yzmp)，支撑多边形为五边形bcdef，如图8所示，根据零力矩点的定义可得zmp点的坐标，进而可求得零力矩点到支撑多边形各边的距离。

得到zmp零力矩点到支撑多边形各边的距离后，通过比较得到最小值作为六足机器人的zmp稳定裕量。若zmp零力矩点始终在支撑多边形的内部，则六足机器人系统是动态稳定的，六足机器人可以保持动态平稳；若zmp零力矩点位于支撑区域的边缘，则六足机器人系统是临界稳定的，六足机器人可能绕zmp零力矩点旋转；若zmp零力矩点位于支撑区域的外侧时，则六足机器人系统不稳定，六足机器人会发生失稳翻倒。设定zmp零力矩点到支撑多边形的最短距离为sm，则机器人的失稳调整依据为sm＞0。

在调整过程中，为了保证六足机器人机身的稳定，通过调整两条腿来使六足机器人恢复稳定。调整方案是对zmp稳定裕量对应的边所涉及到的两条腿进行调整，使六足机器人机身抬高或降低，从而使对应的稳定裕量恢复到安全地范围内，六足机器人恢复稳定后，遵循五足步态走过崎岖地形，完成六足机器人的失稳调整过程，如图9所示：在崎岖路面，机身前倾，然后编号为3和5的两条腿上支，使机身恢复稳定，再落下编号为4的腿，抬起编号为3的腿，采用五足步态度过崎岖路面。

需要特别说明的是，本技术方案中，在失稳判定方面，结合足端压力值与机身姿态角两个信息，分别设定两个阈值，并设定了阈值判定顺序，在提高失稳判定准确度的同时优化了失稳判定的效率，在失稳调整方面，结合zmp失稳判据进行机身姿态矫正，首先对五足步态下六足机器人的支撑多边形坐标进行求解，其次根据定义求得zmp点的坐标，最后求的zmp点到支撑多边形各边的距离，将最小距离作为zmp稳定判据，从而对机身姿态进行调整，使六足机器人重新恢复稳定。

相对于单阈值法来说，为了比较双阈值失稳判定方法相对于单阈值失稳判定方法的准确性，选取室外环境对六足机器人进行行走测试。进行了基于n1、n2的双阈值失稳判定方法行走实验，基于阈值n1的单阈值失稳判定实验和基于阈值n2的单阈值失稳判定实验。在实验中，六足机器人所处的地形环境完全相同，选取路径中100个崎岖度较大的路面作为易失稳点，对六足机器人的状态，基于双阈值法的判定结果，基于n1的单阈值法判定结果以及基于n2的单阈值法判定结果进行记录，得到的结果如表1所示。

表1双阈值失稳判定与单阈值失稳判定结果

通过对得到的数据进行处理，得到基于双阈值法的失稳判定准确度为98.63％，基于n1的单阈值法失稳判定准确度为65.34％，基于n2的单阈值法失稳判定准确度为73.58％。对比数据可以发现基于双阈值法的失稳判定方法相比基于单阈值法的失稳判定方法，判断准确率有大幅度提高。

为了验证本文所提出的双阈值依次判定法对判定效率的提高，设定了双阈值同时判定法的对比实验。实验环境选择与上述相同的实验环境，在两次实验中记录了失稳判定结果与起点到终点的时间，如表2所示。

表2双阈值依次判定与双阈值同时判定结果

通过对得到的数据进行处理，得到双阈值依次判定法判定的准确度为98.63％，双阈值同时判定法判定的准确度为99％，判断准确度有小范围提高。通过利用秒表计时，得到双阈值依次判定法六足机器人的行走时间为125秒，双阈值同时判定法六足机器人行走时间为178秒，前者相对后者行走时间节省了29.78％。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。