基于三条及以上支撑腿的足式机器人地形参数测量方法

摘要

本发明公开了一种基于三条及以上支撑腿的足式机器人地形参数测量方法，利用足式机器人运动控制所必须的传感器信息即可计算出所处地形的姿态角，即翻滚角以及俯仰角，可满足足式机器人运动控制的要求，简单易行、精度比较高；对于运动过程中，总有腿处于支撑相(每个腿的支撑状态由足端的力所决定，测量值大于门限值，则认为该腿处于支撑相)，且支撑腿数目大于2的足式机器人步态控制来说，可仅采用本方法计算地形姿态角以及机身高度，而不需要外在的激光雷达或者视觉摄像机，降低了成本，不存在复杂的数学运算，精度更高。

说明书

技术领域

本发明属于地形参数测量技术领域，涉及一种基于三条及以上支 撑腿的足式机器人地形参数测量方法。

背景技术

足式机器人主要应用于非结构化环境中，在非结构化的环境中的 行走步态有别于平面环境，对机器人的稳定性要求更高，不断变换的 上下坡度更容易是机器人翻倒。足式机器人适应复杂的非结构环境的 前提是可以感知周围的地形环境，而现有的足式机器人通常对激光雷 达、视频图像信息处理得出外部地形信息，成本高、运算量大。

发明内容

(一)发明目的

本发明的目的是：提供一种基于三条及以上支撑腿的足式机器人 地形参数测量方法，利用足式机器人本体传感器的信息进行计算，实 现简单易行且计算量小。

(二)技术方案

为了解决上述技术问题，本发明提供一种基于三条及以上支撑腿 的足式机器人地形参数测量方法，其包括以下步骤：

足式机器人在行走时，在支撑腿不发生滑移的情况下，根据传感 器测量得到足式机器人的支撑腿的足端在机身坐标系下的位置矢量， 所述支撑腿为着地的腿，支撑腿速度为零表示所述支撑腿不发生滑 移；

把足式机器人支撑腿位置矢量写成下面矩阵的形式：

$P_n=\left(\begin{array}{ccc}{p}_{1\_x}& p_{1\_y}& p_{1\_z}\\ p_{2\_x}& p_{2\_y}& p_{2\_z}\\ .& .& .\\ p_{n\_x}& p_{n\_y}& p_{n\_z}\end{array}\right)---\left(1\right)$

公式(1)中：p_{i_x}p_{i_y}p_{i_z}分别为第i条支撑腿在世界坐标系下的 xyz方向的足端位置，n≥3；

记n条腿所在平面的法向量为：

$\overrightarrow{G}=\left(\begin{array}{c}{g}_{n\_x}\\ g_{n\_y}\\ g_{n\_z}\end{array}\right)---\left(2\right)$

则有：

其中，为n维单位向量；

由公式(3)求解可得：

的单位矢量为：

由大地相对于世界坐标系的法向量可得大地相对于世界坐标 系的翻滚角及俯仰角：

公式(6)中，G_roll为大地相对于世界坐标系的翻滚角；为y方 向上的分量；

$G_{pitch}=arctan(-\frac{{\overrightarrow{G}}_{n\_x}}{{\overrightarrow{G}}_{n\_z}})---\left(7\right)$

公式(7)中，G_pitch为大地相对于世界坐标系的俯仰角，为x 方向上的分量，为z方向上的分量；

$H_{r\_g}=-{\overrightarrow{G}}_n·{\overrightarrow{P}}_{fi}---\left(8\right)$

公式(8)中，H_{r_g}为相对于大地坐标系的机身高度；

$H_{r\_w}=\frac{H_{r\_g}}{{\overrightarrow{G}}_{n\_z}}---\left(9\right)$

公式(9)中，H_{r_w}为相对于世界坐标系的机身高度。

本发明还提供了一种基于上所述测量方法的足式机器人运动控 制方法，利用所述测量方法得到的地形翻滚角、俯仰角以及机身高度 调整足式机器人的姿态，以适应地形变化，增加机器人运动的稳定性。

(三)有益效果

上述技术方案所提供的基于三条及以上支撑腿的足式机器人地 形参数测量方法，仅仅利用足式机器人运动控制所必须的传感器信息 即可计算出所处地形的姿态角，即翻滚角以及俯仰角，可满足足式机 器人运动控制的要求，简单易行、精度比较高；对于运动过程中，总 有腿处于支撑相(每个腿的支撑状态由足端的力所决定，测量值大于 门限值，则认为该腿处于支撑相)，且支撑腿数目大于2的足式机器 人步态控制来说，可仅采用本方法计算地形姿态角以及机身高度，而 不需要外在的激光雷达或者视觉摄像机，降低了成本，不存在复杂的 数学运算，精度更高。

附图说明

图1为本发明的测量方法的计算原理图。

具体实施方式

为使本发明的目的、内容和优点更加清楚，下面结合附图和实施 例，对本发明的具体实施方式作进一步详细描述。

如图1所示，本发明提供了一种足式机器人地形参数测量方法， 其特征在于，包括以下步骤：

足式机器人在行走时，在支撑腿不发生滑移的情况下，根据传感 器测量得到足式机器人的支撑腿的至少三个足端在机身坐标系下的 位置矢量，所述支撑腿为着地的腿，支撑腿速度为零表示所述支撑腿 不发生滑移。

本实施例中以四足机器人为例描述。如图1所示，支撑腿为1腿， 2腿以及3腿，各条腿的位置矢量可以通过安装在腿部的传感器数据 进行运动学结算后得到。因此地形参数测量的具体步骤为：

把足式机器人支撑腿位置矢量写成下面矩阵的形式：

$P_n=\left(\begin{array}{ccc}{p}_{1\_x}& p_{1\_y}& p_{1\_z}\\ p_{2\_x}& p_{2\_y}& p_{2\_z}\\ .& .& .\\ p_{n\_x}& p_{n\_y}& p_{n\_z}\end{array}\right)---\left(1\right)$

公式(1)中：

p_{i_x}p_{i_y}p_{i_z}分别为第i条支撑腿在世界坐标系下的xyz方向的 足端位置；

记n条腿所在平面的法向量为：

$\overrightarrow{G}=\left(\begin{array}{c}{g}_{n\_x}\\ g_{n\_y}\\ g_{n\_z}\end{array}\right)---\left(2\right)$

则有：

$P_n\overrightarrow{G}=\overrightarrow{n}---\left(3\right)$

其中为n维单位向量；

由公式(3)求解可得：

$\bar{G}={\left({P_n}^T{P}_n\right)}^{-1}{P_n}^T\overrightarrow{n}---\left(4\right)$

的单位矢量为：

${\overrightarrow{G}}_n=\frac{\overrightarrow{G}}{\left|\overrightarrow{G}\right|}---\left(5\right)$

由大地相对于世界坐标系的法向量可得大地相对于世界坐标 系的翻滚角及俯仰角：

$G_{roll}=arcsin\left({\overrightarrow{G}}_{n\_y}\right)---\left(6\right)$

公式(6)中，G_roll为大地相对于世界坐标系的翻滚角；为y方 向上的分量；

$G_{pitch}=arctan(-\frac{{\overrightarrow{G}}_{n\_x}}{{\overrightarrow{G}}_{n\_z}})---\left(7\right)$

公式(7)中，G_pitch为大地相对于世界坐标系的俯仰角,为x 方向上的分量，为z方向上的分量；

$H_{r\_g}=-{\overrightarrow{G}}_n·{\overrightarrow{P}}_{fi}---\left(8\right)$

公式(8)中，H_{r_g}为相对于大地坐标系的机身高度；

$H_{r\_w}=\frac{H_{r\_g}}{{\overrightarrow{G}}_{n\_z}}---\left(9\right)$

公式(9)中，H_{r_w}为相对于世界坐标系的机身高度。

基于上述测量方法，其应用包括以下步骤：利用所述测量方法得 到的地形翻滚角、俯仰角以及机身高度调整足式机器人的姿态以适应 地形的变化，增加机器人运动的稳定性。

本发明可应用于机器人支撑腿的数量不小于3即可，如三足，均 作为支撑腿；或者四足、五足、六足机器人等多足机器人。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领 域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以 做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。