一种足式机器人状态参数观测处理和姿态控制方法、系统

1.本技术涉及机器人技术领域，具体涉及一种足式机器人状态参数观测处理和姿态控制方法、系统。


背景技术：

2.目前，移动机器人大多采用基于视觉的方式，一般还结合激光雷达、毫米波雷达、深度相机等传感器，以对自身周围环境构建相应地图。但是，视觉信息在剧烈运动的情况下，所得到的位姿容易发散且地图是为稀疏的，使得构建出来的地图很难适用于导航。还有，传感器的雷达信息处在平原、建筑稀少的环境下，所得到的位姿也容易发散，所得到的地图也很难用于导航。
3.因此，基于视觉的方法获取机器人自身周围的环境信息，已经满足不了机器人在真实复杂环境下行走需求。


技术实现要素：

4.有鉴于此，本发明的目的在于提供一种足式机器人状态参数观测处理和姿态控制方法、系统，通过对足式机器人的状态参数观测，并根据所观测的状态参数对地形起伏情况进行估计，为后续足式机器人姿态调节和步态调整提供参考，进而提升足式机器人的运动稳定性和鲁棒性。
5.本说明书实施例提供以下技术方案：
6.本说明书实施例提供一种足式机器人状态参数观测处理方法，包括：
7.获取姿态角和关节角度信息；其中，姿态角为世界坐标系下足式机器人躯干对应的姿态角，由惯性测量单元测量得到；关节角度信息为机器人坐标系下足式机器人各足关节对应的角度信息，从足式机器人中各关节编码器获取得到；
8.根据足式机器人的几何参数、所述关节角度信息和所述姿态角，确定第一触地平面的第一法向量，第一触地平面为三个触地足端各自对应触地点构成的平面；
9.将世界坐标系下水平面的法向量经欧拉角旋转得到第二法向量，以从所述第一法向量和所述第二法向量为相等关系中，获得所述第一触地平面相对于世界坐标系下水平面的俯仰角估计值和横滚角估计值。
10.本说明书实施例还提供一种足式机器人姿态控制方法，包括：
11.获取足式机器人的当前俯仰角和当前横滚角，其中所述当前俯仰角和所述当前横滚角对应为根据本技术中任意一项所述足式机器人状态参数观测处理方法中得到的俯仰角估计值和横滚角估计值；
12.根据所述当前俯仰角和所述当前横滚角调整足式机器人的运动参数。
13.优选地，确定第一触地平面的第一法向量包括：
14.基于足式机器人的几何参数和所述关节角度信息，确定虚拟平面的虚拟法向量，虚拟平面为机器人坐标系下三个触地足端各自对应触地点构成的平面；
15.根据第一旋转矩阵将所述虚拟法向量转化为世界坐标系下对应的所述第一法向量：其中为所述第一法向量，为所述虚拟法向量，wrb为机器人坐标系相对于世界坐标系的第一旋转矩阵，旋转顺序采用欧拉角z-y-x旋转顺序：
[0016][0017]
其中，θ和φ对应为所述姿态角中第一触地平面相对于世界坐标系中水平面的俯仰角和横滚角，ψ为所述姿态角中第一触地平面相对于世界坐标系中水平面的横摆角。
[0018]
优选地，确定虚拟平面的虚拟法向量包括：
[0019]
根据足式机器人质心处在机器人坐标系下的坐标，以及三个触地足端的关节尺寸、关节相距质心处的几何尺寸和所述关节角度信息，确定三个触地足端各自对应的坐标；
[0020]
基于三个触地足端各自对应的坐标，根据两相交直线确定出虚拟平面的虚拟法向量，其中直线为由三个触地足端中任意两个足端确定的直线。
[0021]
优选地，选取多个时刻的第一法向量，并经过最小二乘法加权得到用于表征综合地形平面的估计法向量，以将估计法向量作为与所述第二法向量具有相等关系的第一法向量。
[0022]
优选地，世界坐标系下水平面的法向量为单位法向量所述估计法向量为均一化后的法向量第二旋转矩阵为wrg＝
w r
x
(α)
×
w ry(β)
×
w rz(γ)，其中，α为所述俯仰角估计值和β为所述横滚角估计值；
wrz
(γ)为绕z轴旋转γ角度的矩阵，与单位法向量相同，为
wry
(β)为绕y轴旋转β角度的矩阵；
wrx
(α)为绕x轴旋转α角度的矩阵。
[0023]
优选地，在得到所述俯仰角估计值和/或所述横滚角估计值后，所述足式机器人状态参数观测处理方法还包括：采用最小二乘法对所述俯仰角估计值和/或所述横滚角估计值进行拟合，以得到随时间变化的所述俯仰角估计值和/或所述横滚角估计值，拟合直线如下：
[0024][0025]
其中，x变量为触地时间t，y变量为俯仰角α或者是横滚角β。
[0026]
优选地，通过足式机器人平行于所述第一触地平面以对角步态前行，对地形的俯仰角估计值进行直线拟合；
[0027]
和/或，通过足式机器人平行于所述第一触地平面以对角步态侧行，对地形的横滚角估计值进行直线拟合。
[0028]
本说明书实施例还提供一种足式机器人状态参数观测处理系统，包括：
[0029]
参数获取模块，用于获取姿态角和关节角度信息；其中，姿态角为世界坐标系下足式机器人躯干对应的姿态角，由惯性测量单元测量得到；关节角度信息为机器人坐标系下足式机器人各足关节对应的角度信息，从足式机器人中各关节编码器获取得到；
[0030]
法向量处理模块，用于根据足式机器人的几何参数、所述关节角度信息和所述姿态角，确定第一触地平面的第一法向量，第一触地平面为三个触地足端各自对应触地点构成的平面；
[0031]
参数估计模块，用于将世界坐标系下水平面的法向量经欧拉角旋转得到第二法向量，以从所述第一法向量和所述第二法向量为相等关系中，获得所述第一触地平面相对于世界坐标系下水平面的俯仰角估计值和横滚角估计值。
[0032]
本说明书实施例还提供一种足式机器人姿态控制系统，包括：
[0033]
参数确定模块，用于确定足式机器人的当前俯仰角和当前横滚角，其中所述当前俯仰角和所述当前横滚角对应为根据本技术中任意一项所述足式机器人状态参数观测处理方法中得到的俯仰角估计值和横滚角估计值；
[0034]
参数调整模块，用于根据所述当前俯仰角和所述当前横滚角调整足式机器人的运动参数。
[0035]
与现有技术相比，本说明书实施例采用的上述至少一个技术方案能够达到的有益效果至少包括：
[0036]
本技术公开一种融合imu(inertial measurement unit，惯性测量单元)和四足机器人的关节角度信息的机器人状态参数观测方案，通过融合机器人姿态角对地形起伏情况进行近似估计，弥补基于视觉的方法带来的对真实地起伏观测的偏差，或者在视觉信息严重失真的情况下仍然能够提供有效的地形起伏估计结果，因而对于机器人运动过程中保持机身稳定时需要维持自身姿态和运动控制有很大帮助，有利于实现四足机器人对机身pry角度解耦控制和提高运动的稳定性和鲁棒性，提高了移动机器人对全向地形的越障能力和通过性。
附图说明
[0037]
为了更清楚地说明本技术实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
[0038]
图1是本技术中足式机器人状态参数观测及调整的整体结构示意图；
[0039]
图2是本技术中足式机器人状态参数观测处理方法的流程图；
[0040]
图3是本技术中坐标系构建的示意图；
[0041]
图4是本技术中右前腿的动力学模型示意图；
[0042]
图5是本技术中选取触地足端来构建触地平面的示意图；
[0043]
图6是本技术中基于相交两条直线确定法向量的示意图；
[0044]
图7是本技术中拟合中坡度为10
°
的示意图；
[0045]
图8是本技术中拟合中坡度为5
°
的示意图；
[0046]
图9是本技术中俯仰角经过拟合后结果的示意图；
[0047]
图10是本技术中横滚角经过拟合后结果的示意图；
[0048]
图11是本技术中一种足式机器人状态参数观测处理系统的结构示意图；
[0049]
图12是本技术中一种足式机器人姿态控制方法的流程示意图；
[0050]
图13是本技术中一种足式机器人姿态控制系统的结构示意。
具体实施方式
[0051]
下面结合附图对本技术实施例进行详细描述。
[0052]
以下通过特定的具体实例说明本技术的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本技术的其他优点与功效。显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。本技术还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本技术的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
[0053]
要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本技术，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目和方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。
[0054]
还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本技术的基本构想，图式中仅显示与本技术中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
[0055]
另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践。
[0056]
移动机器人(如足式机器人)一般是依靠视觉方式，来获取自身周围环境信息，导致在剧烈运动场景下得到的位姿容易发散，且构建出来的环境地图也是稀疏的，导致无法进行实际导航。另外，依靠视觉方式，甚至是结合传感器(如微波雷达、激光雷达、深度相机等)来获取周围环境信息，但在全向地形中，例如像沼泽地、松软的沙地、水路两栖路面等复杂路面环境，由于地面刚度不够，足式机器人实际落足点的控制，往往与所构建的地形起伏平面存在很大偏差，同样获取到的环境信息很难用于实际导航。
[0057]
因此，基于视觉获取机器人自身周围的环境信息满足不了真实需求机器人的越障能力和通过性。
[0058]
有鉴于此，通过对足式机器人以及机器人步态等深入研究及改进探索，提出一种新处理方案：不再基于传统视觉方式，而是融合imu(inertial measurement unit，惯性测量单元)和足式机器人的关节角度信息，来对真实地形起伏情况进行近似估计，进而实时观测得到机器人足端运动学模型的状态参数，使得机器人能够基于准确的真实地形数据进行行进中的运动控制，很好地维持自身姿态和运动控制，保持了机身稳定；还有，通过对机身
pry角度解耦控制和提高运动稳定性和鲁棒性，实现机器人对真实地面环境信息进行可靠获取，方便在全向地形中进行实时运动导航。
[0059]
如图1所示，一种足式机器人状态参数观测处理总体思路如下：机器人在行进中执行控制指令输入后，首先利用机身质心处惯性测量单元imu中的加速度计得到躯干的姿态角信息，以及利用各个关节编码器获得各个关节对应的关节角度信息，然后基于这些信息实时得到足端在真实起伏地形中的运动学参数，并从中分离出机器人在触地时刻机器人所处真实起伏地形下的俯仰角和横滚角等状态参数，最后基于观测得到的这些状态参数，调整机器人下一时刻的机身姿态，并选取合适步态形式进行下一时刻的机器人运动控制，能够提高机器人运动稳定性和鲁棒性。
[0060]
因此，即使机器人是在地势起伏多变的全向地形(如前述沼泽地、松软的沙地、水路两栖路面等复杂路面)行走，因为是基于机器人在实际地形下运动时的姿态角和关节信息来实时观测得到相应状态参数，使得这些状态参数是与真实地形密切相关的，进而在基于这些观测结果进行姿态、步态等运动参数调整时，其稳定性、鲁棒性等控制特性非常好，能够实现机器人在全向地形中进行运动导航。
[0061]
需要说明的是，相比于基于视觉的方法获取机器狗自身周围的环境信息，像沼泽地，松软的沙地，水路混合路面等环境，由于地面刚度不够，足式机器人实际落足点和采用视觉建图的地面有很大偏差，所以通过不采用视觉的方法，而是基于自身imu传感器和关节编码器信息等获得对所处地面的俯仰角和横滚角的估计，可以弥补基于视觉的方法带来的对真实地起伏观测的偏差，或者在视觉信息严重失真的情况下仍然能够提供有效的地形起伏估计结果。
[0062]
此外，通过观测并从中分离出机器人所处环境地面的俯仰角估计值和横滚角估计值，有利于实现机器狗对自身躯干在纵向和横向的独立控制。例如，根据所估计的机身姿态和地形起伏情况调节机器狗运动过程中的一些动态参数，比如步态类型，步幅，步高和步频，更有利于足式机器人对所处环境的通过性和自身运动的稳定性。
[0063]
以下结合附图，说明本技术各实施例提供的技术方案。
[0064]
如图2所示，本说明书实施例提供一种方法，可包括：
[0065]
步骤s202、获取姿态角和关节角度信息；其中，姿态角为世界坐标系下足式机器人躯干对应的姿态角，由惯性测量单元测量得到；关节角度信息为机器人坐标系下足式机器人各足关节对应的角度信息，从足式机器人中各关节编码器获取得到。
[0066]
实施中，可以利用机身质心处imu中的加速度计得到躯干的姿态角度(比如经过imu内部的扩展卡尔曼滤波算法得到)，以及可以利用各个关节编码器获得关节角度信息(可以是带有高斯白噪声)。
[0067]
如图3所示，建立机身姿态坐标系(即机器人坐标系，也可称机身坐标系，下面不作区分)：世界坐标系{w}固定在水平地面上，其x、y、z轴朝向分别为东北天(enu)，满足右手法则；机器人坐标系{b}固定在机身质心处，其x、y、z轴朝向分别为机身躯干前进方向，机身躯干左边朝向，z轴为垂直方向，满足右手法则。图3中，g平面(plane)为足端触地点所形成的虚拟平面，g平面相对于世界坐标系{w}的空间姿态角为俯仰估计角和横滚估计角
[0068]
如图4所示，建立足端运动学模型：从机器人质心处推导足端各点相对于机器人坐标系{b}的位置坐标，以及定义四足机器人的四条腿的编号，其中右前腿fr编号i为0，左前腿fl编号i为1，右后腿rr编号i为2，左后腿rl编号i为3。进一步，还定义有机器人连杆的几何尺寸参数，例如关节距离质心p
com
的横向距离h
x
和纵向距离hy，第一关节长度l1、第二关节长度l2和第三关节长度l3等。
[0069]
需要说明的是，在得到的足端运动学模型中，机器人相关的几何参数，比如坐标数据，可以是机器人坐标系下的数据，也可以是转化为世界坐标系下对应的数据。
[0070]
步骤s204、根据足式机器人的几何参数、所述关节角度信息和所述姿态角，确定第一触地平面的第一法向量，第一触地平面为三个触地足端各自对应触地点构成的平面。
[0071]
在机器人的几何参数、关节角度信息和姿态角度等数据支撑下，可以确定出三个触地足端所构成的触地平面以及该触地平面的法向量(这里记为第一法向量)。具体地，根据空间几何信息可知，已知空间中三个点的位置，可以求取出这三个点所构成的空间平面。
[0072]
在一种示例中，根据当前足式机器人的步态形式，选取三个足端作为触地足端点，例如以四足机器人对角步态为例，选取一组相对的足触地，和另外一个在下一时刻最先触地的足端作为第三个触地点。
[0073]
在一种示例中，如图5所示，通过在足端设置有压力传感器，可以分别通过检测四个足端压力pi(i＝0，1，2，3)，是否大于触地压力阈值pc(可以根据实际地形情况进行设置，这里不作限定)，若是则认为该足端处于触地状态。
[0074]
需要说明的是，通过选取出三个合适触地足端点pi，进而可以根据几何学求取出三个触地足端构成平面的法向量，而且一个空间平面的俯仰角和横滚角，可以根据它的法向量得到，最终可以根据该法向量与水平面之间的角度差异来估计出触地时真实地面的坡度(即俯仰角)和横滚角。
[0075]
步骤s206、将世界坐标系下水平面的法向量经欧拉角旋转得到第二法向量，以从所述第一法向量和所述第二法向量为相等关系中，获得所述第一触地平面相对于世界坐标系下水平面的俯仰角估计值和横滚角估计值。
[0076]
需要说明的是，将水平面的法向量按欧拉角旋转后，可以得到一个新的法向量(这里记为第二法向量)，假如前述第一法向量与此处的第二法向量相等，则可以从该相等关系中分离出机器人触地平面相对于水平面的坡度和横滚角度信息。
[0077]
因此，鉴于第一法向量是真实触地平面的法向量，且该第一法向量是基于机器人当前处理所处姿态得到，也包含有imu得到的姿态角信息、机身姿态等，而第二法向量是假设按当前俯仰角、横滚角经过欧拉角旋转得到的法向量，所以在理论上，这两个法向量应该具有相等关系，因而可以从该相等关系中估计出真实地形下机器人触地对应的俯仰角和横滚角，即将观测得到的结果作为对应的俯仰角估计值和横滚角估计值，从而实现机器人在真实地形下行进中触地时对应的姿态(如俯仰、横滚)参数，始终是与真实地面密切相关。
[0078]
在一些实施方式中，针对触地平面(即第一触地平面)的法向量求取时，可以基于不共线的三点确定一个平面，以及基于三点的坐标可以确定平面及其法向量。
[0079]
在一种示例中，如图6所示，两条相交直线构成平面，并基于向量关系可以确定出该平面的法向量bng，表达式如下：其中b表示在机器人坐标系下的法向量，g表示足端触地点所形成的g平面。
[0080]
通过三个触地足端的坐标来表达这两条直线的过程如下：
[0081]
根据足端触地点选取规则，假设选取的最先触地的三个足端位置分别记为bpa，bpb，bpc，各自对应坐标分别如下：
[0082][0083]
因此，在求取到三个足端点坐标后，可以基于三个触地足端各自对应的坐标，根据两相交直线确定出虚拟平面的虚拟法向量，其中直线为由三个触地足端中任意两个足端确定的直线。
[0084]
例如，空间中相交的两条直线l1，l2，可以表达如下：
[0085][0086][0087]
其中，直线l1经过点位置bpa和bpb，直线l2经过点位置bpb和bpc。
[0088]
在一种示例中，可以利用几何关系求出获取足端点坐标。具体地，可以根据足式机器人质心处在机器人坐标系下的坐标，以及三个触地足端的关节尺寸、关节相距质心处的几何尺寸和所述关节角度信息，确定三个触地足端各自对应的坐标。
[0089]
例如，基于前述图4定义的运动学模型，以右前腿足端为例，该足端的坐标为：
[0090][0091]
其中，bpi是右前腿足端相对于机器人坐标系{b}的位置坐标，式中各符号所代表内容如下：si＝sinθi，s
i,j
＝sin(θi+θj)，ci＝cosθi，c
i,j
＝cos(θi+θj)，其中θi为对应关节的关节角度信息。
[0092]
另外，η和γ是算式中反映计算项正负号(即符号系数)，定义如下：
[0093][0094]
另外，可以根据空间几何信息可知，三个点构成的空间平面，可以表示为平面方程：apa+bpb+cpc+d＝0，式中a，b，c，d为平面方程的系数，bpa，bpb，bpc为足端触地点坐标。
[0095]
因此，通过确定出触地足端点坐标，可以基于坐标数据来确定出触地平面，进而确定平面的法向量，最终通过法向量求取触地平面相对于水平面的坡度和横滚角。
[0096]
还有，基于向量关系可以获取出g平面的法向量，记为即前述这里不再展开说明。
[0097]
在一些实施方式中，可以基于机器人坐标系先求出触地平面的法向量，进而再将法向量旋转到世界坐标系下，以便从中分离出法向量中包含的俯仰角和横滚角信息。
[0098]
基于前述示例，可以直接在机器人坐标系下确定出触地平面的法向量，进而将该法向量旋转到世界坐标系下，便于在世界坐标系下进行数据处理。
[0099]
在一些实施方式中，基于足式机器人的几何参数和所述关节角度信息，确定虚拟平面的虚拟法向量，虚拟平面为机器人坐标系下三个触地足端各自对应触地点构成的平面，具体可以通过前述示例确定出触地平面(这里记为虚拟平面)在机器人坐标系下的虚拟法向量；然后，将该虚拟法向量转换为世界坐标系下的法向量(即前述第一法向量)。
[0100]
具体地，将g平面法向量转化到世界坐标系{w}下的法向量表达式如下：式中wrb为机器人坐标系{b}相对于世界坐标系{w}的旋转矩阵，旋转顺序采用欧拉角z-y-x旋转顺序，对应关系为：横摆角ψ(绕z轴)，俯仰角θ(绕y轴)，翻滚角φ(绕x轴)，绕动系的顺序为先绕z轴，再绕y轴，最后绕x轴。
[0101]
因此，旋转矩阵的表达式如下：
[0102][0103]
其中，θ和φ对应为所述姿态角中第一触地平面相对于世界坐标系中水平面的俯仰角和横滚角，ψ为所述姿态角中第一触地平面相对于世界坐标系中水平面的横摆角。
[0104]
需要说明的是，θ和φ可以为imu经过卡尔曼滤波，姿态融合算法得到的机器人坐标系{b}相对于世界坐标系{w}的俯仰角和横滚角，ψ为imu经过融合内部磁力计方位角信息得到机器人坐标系{b}相对于世界坐标系{w}的横摆角。
[0105]
因此，通过前述坐标计算和旋转矩阵计算，得到的第一法向量包含有imu得到的姿态角信息、关节角度信息、机器人自身几何参数，使得第一法向量能够真实地反映了机器人在实际地形行进中的状态。因此，从中分离得到的俯仰角和横滚角，必然与真实地形密切相关。
[0106]
在一些实施方式中，可以通过选取多个时刻的第一法向量，并经过最小二乘法加权得到用于表征综合地形平面的估计法向量，以将估计法向量作为与所述第二法向量具有相等关系的第一法向量，以此来保证估计得到的平面的法向量更准确地收敛于真实道路情况。
[0107]
实施中，可以进一步选取多个时刻t的法向量，经过最小二乘法，进行加权得到一个综合地形平面估计法向量
[0108][0109]
在一些实施方式中，可将法向量进行均一化处理，便于利用单位向量进行旋转。
[0110]
实施中，世界坐标系下水平面的法向量为单位法向量虚拟平面的估计法向量为均一化后的法向量则第二旋转矩阵记为：
[0111]wrg＝
w r
x
(α)
×
w ry(β)
×
w rz(γ)，
[0112]
其中，α为所述俯仰角估计值和β为所述横滚角估计值；
wrz
(γ)为绕z轴旋转γ角度的矩阵，与单位法向量相同，为
wry
(β)为绕y轴旋转β角度的矩阵；
wrx
(α)为绕x轴旋转α角度的矩阵。
[0113]
另外，对空间法向量进行均一化处理如下式所示：
[0114][0115]
因此，通过将虚拟平面的单位法向量记为水平地面的单位法向量记为其中水平地面经过绕欧拉角x-y-z顺序，即绕世界坐标系(静系)z轴旋转，再绕世界坐标系y轴的旋转和绕世界坐标系x轴的旋转得到记旋转矩阵为wrg。欧拉角选取x-y-z顺序目的是先绕世界坐标系z轴旋转后，水平面法向量指向不变。
[0116]
综上，可以使用旋转矩阵的方式获得空间中两平面的法向量之间的关系来估计俯仰角和横滚角：
[0117]
在将第二旋转矩阵代入到上述相等等式后，以及采用α和β表达俯仰角和横滚角(为了与imu得到躯干姿态角进行区分)，可以得到以下关系式：
[0118][0119]
进一步解方程后，可以得到地形俯仰角α和横滚角β的表达式分别如下：
[0120][0121][0122]
通过法向量旋转得到相等关系，并从中分离出地形相关的俯仰角和横滚角，可以将所得结果作为姿态的观察结果。根据前述内容可知：机器人在实际地形行进中，通过实时对行进实际进行观测，使得观测结果是与实际地形密切相关的估计值。
[0123]
在一些实施方式中，在得到所述俯仰角估计值和/或所述横滚角估计值后，可以通过进一步拟合，来获得更好的估计值。
[0124]
得到地形俯仰角α和横滚角β之后，采用最小二乘法对所述俯仰角估计值和/或所述横滚角估计值进行拟合，以得到随时间变化的所述俯仰角估计值和/或所述横滚角估计值，设拟合直线如下：
[0125][0126]
其中：拟合直线的斜率为k，和已经确定的斜率k，利用待定系数法则求出截距b。对于坡度角度估计，这里选取的x变量为时间t，y变量为地面坡度角(即俯仰角)或者是近似地面侧倾角度(即横滚角)，这样可以得到随时间变化的世界坐标系{w}下的地形观测角度和
[0127]
在一些实施方式中，可以通过足式机器人平行于所述第一触地平面以对角步态前行，对地形的俯仰角估计值进行直线拟合；
[0128]
和/或，通过足式机器人平行于所述第一触地平面以对角步态侧行，对地形的横滚角估计值进行直线拟合。
[0129]
这里选取的地面坡度为10
°
和5
°
的斜坡，分别如图7和图8示意：分别让机器狗平行于斜坡和垂直于斜坡以对角步态行走，得到地形的俯仰角估计值和侧偏角估计值，结果如图9和图10示意。
[0130]
从结果来看，所得结果和真实值有一定的偏差，和关节角度传感器的高斯白噪声
和imu传感器测得的机身姿态角的数据精度有关，但是经过最小二乘拟合之后和真值拟合情况基本满足步态算法对地形估计的精确度要求，验证了所提出的地形估计方法的有效性，为后续足式机器人的步态调节和姿态调整作为参考依据。
[0131]
基于相同发明构思，本技术还提供一种足式机器人状态参数观测处理系统，基于该观测处理系统，可以得到机器人状态参数的观测结果，通过对真实地形下行进中的状态参数进行实时观测，实现了观测结果能够如实地反映真实地形的行进状态。
[0132]
如图11所示，一种足式机器人状态参数观测处理系统包括：
[0133]
参数获取模块1101，用于获取姿态角和关节角度信息；其中，姿态角为世界坐标系下足式机器人躯干对应的姿态角，由惯性测量单元测量得到；关节角度信息为机器人坐标系下足式机器人各足关节对应的角度信息，从足式机器人中各关节编码器获取得到；
[0134]
法向量处理模块1103，用于根据足式机器人的几何参数、所述关节角度信息和所述姿态角，确定第一触地平面的第一法向量，第一触地平面为三个触地足端各自对应触地点构成的平面；
[0135]
参数估计模块1105，用于将世界坐标系下水平面的法向量经欧拉角旋转得到第二法向量，以从所述第一法向量和所述第二法向量为相等关系中，获得所述第一触地平面相对于世界坐标系下水平面的俯仰角估计值和横滚角估计值。
[0136]
需要说明的是，观测处理系统可以针对前述任意一个实施例所述足式机器人状态参数观测处理的相关步骤及功能，进行功能模块的划分及实现，这里不再以示例进行展开说明，具体可参见方法的示例内容。
[0137]
基于相同发明构思，本技术还提供一种足式机器人姿态控制方法及系统，以基于观测得到的俯仰角和横滚角进行机器人行进姿态控制及调整。
[0138]
如图12所示，一种足式机器人姿态控制方法包括：
[0139]
步骤1202、获取足式机器人的当前俯仰角和当前横滚角，其中所述当前俯仰角和所述当前横滚角对应为根据前述示例中任意一项所述足式机器人状态参数观测处理方法中得到的俯仰角估计值和横滚角估计值；
[0140]
步骤1204、根据所述当前俯仰角和所述当前横滚角调整足式机器人的运动参数。
[0141]
需要说明的是，运动参数可以是根据所估计的机身姿态和地形起伏情况而对机器人(也可称机器狗)在运动过程中进行调整的一些动态参数，比如步态类型，步幅，步高和步频，更有利于足式机器人对所处环境的通过性和自身运动的稳定性。
[0142]
如图13所示，一种足式机器人姿态控制系统包括：
[0143]
参数确定模块1301，用于确定足式机器人的当前俯仰角和当前横滚角，其中所述当前俯仰角和所述当前横滚角对应为根据前述示例中任意一项所述足式机器人状态参数观测处理方法中得到的俯仰角估计值和横滚角估计值；
[0144]
参数调整模块1303，用于根据所述当前俯仰角和所述当前横滚角调整足式机器人的运动参数。
[0145]
本说明书中，各个实施例之间相同相似的部分互相参见即可，每个实施例侧重说明的都是与其他实施例的不同之处。尤其，对于后面说明的实施例而言，描述比较简单，相关之处参见前述实施例的部分说明即可。
[0146]
以上所述，仅为本技术的具体实施方式，但本技术的保护范围并不局限于此，任何
熟悉本技术领域的技术人员在本技术揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本技术的保护范围之内。因此，本技术的保护范围应以权利要求的保护范围为准。

技术特征：
1.一种足式机器人状态参数观测处理方法，其特征在于，包括：获取姿态角和关节角度信息；其中，姿态角为世界坐标系下足式机器人躯干对应的姿态角，由惯性测量单元测量得到；关节角度信息为机器人坐标系下足式机器人各足关节对应的角度信息，从足式机器人中各关节编码器获取得到；根据足式机器人的几何参数、所述关节角度信息和所述姿态角，确定第一触地平面的第一法向量，第一触地平面为三个触地足端各自对应触地点构成的平面；将世界坐标系下水平面的法向量经欧拉角旋转得到第二法向量，以从所述第一法向量和所述第二法向量为相等关系中，获得所述第一触地平面相对于世界坐标系下水平面的俯仰角估计值和横滚角估计值。2.根据权利要求1所述的足式机器人状态参数观测处理方法，其特征在于，确定第一触地平面的第一法向量包括：基于足式机器人的几何参数和所述关节角度信息，确定虚拟平面的虚拟法向量，虚拟平面为机器人坐标系下三个触地足端各自对应触地点构成的平面；根据第一旋转矩阵将所述虚拟法向量转化为世界坐标系下对应的所述第一法向量：其中为所述第一法向量，为所述虚拟法向量，
w
r
b
为机器人坐标系相对于世界坐标系的第一旋转矩阵，旋转顺序采用欧拉角z-y-x旋转顺序：其中，θ和φ对应为所述姿态角中第一触地平面相对于世界坐标系中水平面的俯仰角和横滚角，ψ为所述姿态角中第一触地平面相对于世界坐标系中水平面的横摆角。3.根据权利要求2所述的足式机器人状态参数观测处理方法，其特征在于，确定虚拟平面的虚拟法向量包括：根据足式机器人质心处在机器人坐标系下的坐标，以及三个触地足端的关节尺寸、关节相距质心处的几何尺寸和所述关节角度信息，确定三个触地足端各自对应的坐标；基于三个触地足端各自对应的坐标，根据两相交直线确定出虚拟平面的虚拟法向量，其中直线为由三个触地足端中任意两个足端确定的直线。4.根据权利要求1-3中任意一项所述的足式机器人状态参数观测处理方法，其特征在于，选取多个时刻的第一法向量，并经过最小二乘法加权得到用于表征综合地形平面的估计法向量，以将估计法向量作为与所述第二法向量具有相等关系的第一法向量。5.根据权利要求4所述的足式机器人状态参数观测处理方法，其特征在于，世界坐标系下水平面的法向量为单位法向量所述估计法向量为均一化后的法向量第二旋转矩阵为
w
r
g
＝
w r
x
(α)
×
w r
y
(β)
×
w r
z
(γ)，其中，α为所述俯仰角估计值和β为所述横滚角估计值；
w
r
z
(γ)为绕z轴旋转γ角度的矩阵，与单位法向量相同，为
w
r
y
(β)为绕y轴旋转β角度的矩阵；
w
r
x
(α)为绕x轴旋转α角度的矩阵。6.根据权利要求1所述的足式机器人状态参数观测处理方法，其特征在于，在得到所述俯仰角估计值和/或所述横滚角估计值后，所述足式机器人状态参数观测处理方法还包括：采用最小二乘法对所述俯仰角估计值和/或所述横滚角估计值进行拟合，以得到随时间变化的所述俯仰角估计值和/或所述横滚角估计值，拟合直线如下：其中，x变量为触地时间t，y变量为俯仰角α或者是横滚角β。7.根据权利要求6所述的足式机器人状态参数观测处理方法，其特征在于，通过足式机器人平行于所述第一触地平面以对角步态前行，对地形的俯仰角估计值进行直线拟合；和/或，通过足式机器人平行于所述第一触地平面以对角步态侧行，对地形的横滚角估计值进行直线拟合。8.一种足式机器人姿态控制方法，其特征在于，包括：获取足式机器人的当前俯仰角和当前横滚角，其中所述当前俯仰角和所述当前横滚角对应为根据权利要求1-7中任意一项所述足式机器人状态参数观测处理方法中得到的俯仰角估计值和横滚角估计值；根据所述当前俯仰角和所述当前横滚角调整足式机器人的运动参数。9.一种足式机器人状态参数观测处理系统，其特征在于，包括：参数获取模块，用于获取姿态角和关节角度信息；其中，姿态角为世界坐标系下足式机器人躯干对应的姿态角，由惯性测量单元测量得到；关节角度信息为机器人坐标系下足式机器人各足关节对应的角度信息，从足式机器人中各关节编码器获取得到；法向量处理模块，用于根据足式机器人的几何参数、所述关节角度信息和所述姿态角，确定第一触地平面的第一法向量，第一触地平面为三个触地足端各自对应触地点构成的平面；参数估计模块，用于将世界坐标系下水平面的法向量经欧拉角旋转得到第二法向量，以从所述第一法向量和所述第二法向量为相等关系中，获得所述第一触地平面相对于世界坐标系下水平面的俯仰角估计值和横滚角估计值。10.一种足式机器人姿态控制系统，其特征在于，包括：参数确定模块，用于确定足式机器人的当前俯仰角和当前横滚角，其中所述当前俯仰角和所述当前横滚角对应为根据权利要求1-7中任意一项所述足式机器人状态参数观测处理方法中得到的俯仰角估计值和横滚角估计值；参数调整模块，用于根据所述当前俯仰角和所述当前横滚角调整足式机器人的运动参数。

技术总结
本申请提供一种足式机器人状态参数观测处理和姿态控制方法、系统，应用于机器人技术领域，其中足式机器人状态参数观测处理方案，不再基于传统单一视觉方式，而是融合IMU和关节角度信息，通过数据融合来对行进中的真实地形起伏情况进行近似估计，实现机器人对真实地面环境信息进行可靠获取，以及让机器人能够基于准确的地形数据对自身姿态进行稳定性和鲁棒性良好的运动控制，方便机器人在全向地形中进行实时运动。进行实时运动。进行实时运动。


技术研发人员：汪际亮 刘绍勋 牛志华 潘铮 李博远 周世昱 林兆鹏
受保护的技术使用者：上海交通大学
技术研发日：2023.03.23
技术公布日：2023/8/14