IT评测·应用市场-qidao123.com技术社区

标题: 利用线性化 ZMP 束缚条件优化轮式四足机器人的轨迹 [打印本页]

作者: 南飓风 时间: 2024-10-2 22:54
标题: 利用线性化 ZMP 束缚条件优化轮式四足机器人的轨迹
系列文章目录

前言

摘要 -- 我们提出了一种带驱动轮的四足机器人轨迹优化器。通过级联方式求解底座和脚部轨迹的角度、垂直和平面分量，并引入零点平衡准则的新型线性公式，我们只需举行二次规划，从而无需非线性最优规划。然而，纵然是包含完整飞行阶段的步态，我们也能生成执行复杂运动的轨迹，其中包罗同时驾驶、行走和转弯。我们在对装有轮子的四足机器人 ANYmal 的模拟中验证了我们的方法，我们可以或许以 50 Hz 的频率运行所提出的轨迹优化器。据我们所知，这是初次利用在线运动规划器为有轮足式四足机器人演示这种动态运动。

一、引言

        轮式足式机器人有可能将两种运动领域的优点联合起来：轮子的效率和速率以及足式机器人应对挑战性地形的能力。在搜救任务中，时间可能就是生死攸关的问题，而这只是此类系统能极大帮助完成任务的一个例子。然而，迄今为止，尚未有任何运动框架可以或许充实发挥这种组合的紧张上风之一，即同时步行和驾驶的能力。这项研究增补了这一空白，提出了一种轨迹优化器（TO），可为无车轮转向的轮式足式四足机器人（如图 1 所示）的混合1 运动找到在线高动态运动。从工程学角度来看，应对后者的能力很有吸引力，因为它可以低落机器复杂性、总重量和足式机器人的惯性，从而分别提高可靠性、能耗和机动性。
A. 相关工作

        由于系统动力学的高度非线性，利用基于模子的方法探求足式机器人的输入和状态轨迹是一个复杂的问题。为了获得可在线求解的问题，通常采用三种技术： 1) 引入假设以低落模子复杂度；2) 以分层级联的方式求解问题，从而获得一系列更简单的子问题；3) 利用启发式方法获得部门解决方案。
        四足机器人 MIT Cheetah 的计谋规则化模子猜测控制器 (MPC) [2]，就是一个利用了上述三种技术的例子。他们的机器人模子是一个浮动基座，具有无质量的足式腿，二次速率项被忽略。先验地找到了足式腿的摆动时序，并利用基于 Raibert 的立足点猜测[3]的启发式作为目标来引导优化搜刮方向。在一个跨步一连时间的规划范围内，报告的求解时间约为 100 毫秒。
        文献[4]还先容了一种对摆动时序举行优化的 TO。同样，机器人模子是一个带有无质量足式腿的浮动基座。启发式方法以初始猜测和预设步数的情势输入。效果表明，在一个步长的规划范围内，求解时间为 100 毫秒[5]。
        Winkler 等人之前的工作展示了一种 TO 方案，该方案利用非线性规划（NLP）找到了平面质心（CoM）和双脚轨迹[6]。摆动时间是事先确定的，机器人被建模为高度恒定、方向固定的线性倒立摆。对于一个跨步一连时间的规划范围，这些大幅简化导致求解时间在 10 到 120 毫秒之间，详细取决于步态。
        如果先利用启发式方法（如 Raibert 的立足点猜测）盘算出脚部轨迹，则可通过二次规划（QP）3 获得 CoM 轨迹，求解时间可减少到几毫秒 [7]。最近，作者对其方法举行了改进，将垂直 CoM 运动纳入 NLP 问题 [8]。这样就能找到包含完整飞行阶段的步态轨迹，但求解时间也增加了五倍以上。
        [6]-[8]中的模子简化在很大水平上依赖于所谓的零动量点（ZMP）平衡准则。它通过订定动态平衡条件，将站立脚的位置与系统的线性动量和角动量接洽起来，从而实现了相当水平的抽象。它在足式机器人学中的应用由来已久[9]。然而，我们必须认识到，盘算效率的提高是以通用性的低落为代价的。例如，扩展到不平整的地形 [10] 和参加摩擦锥束缚并不简单，而 [4] 等盘算量更大的方法却没有这个缺点。
        上述研究表明，人们对传统足式机器人的动态运动产生了浓厚的爱好。相比之下，有关轮式足式机器人的研究 [11]-[21] 通常侧重于在不平地形上的静态稳定运动。例如，太空探索飞行器[11]-[13]将其四肢用作复杂的悬挂系统，而不是以轮子作为末端执行器的足式机器人。文献[14]-[17]进一步探讨了通过驾驶穿越平展地形和通过行走穿越不规则地形的方法。然而，对于同时行走和驾驶所带来的可能性，纵然是静态稳定的运动，研究也不多。在这方面，波士顿动力公司的轮式双足机器人 Handle [22] 和最近的论文 [23]是两个紧张的例外，后者展示了一种基于 NLP 的轮式足式机器人运动生成通用方法。然而，人们对 Handle 的控制框架知之甚少，而 [23] 的盘算时间通常是规划范围的倍数，因此，以目前的情势在线应用，其速率之慢令人望而却步。
B. 贡献

        我们为带驱动轮的四足机器人提出了一种 TO，它能在给定步态模式和所需目标状态的环境下，找到行走、驾驶及其混合组合的轨迹。与 [6] - [8] 不同的是，我们在优化之前盘算了角度分量和垂直方向的 CoM 运动。这使得我们无需解决 NLP 问题就能找到驱动曲线和执行具有完整飞行阶段的步态的轨迹。此外，我们还引入了一种姿态脚轨迹参数化方法，该方法在本质上满意了运动学滚动束缚。据我们所知，我们提出了一种将 ZMP 平衡准则表述为线性束缚的新方法。
        正如我们在模拟中演示的那样，这使我们可以或许从前所未有的速率生成轮足式四足机器人的动态混合运动轨迹。末了，我们还讨论了该方法在现实系统和不规则地形中的实用性。
二、问题表述

        对于我们的 TO，我们假设一个简化的机器人模子，其中的足式机器人是无质量的。单个浮体具有机器人的总质量和与默认构型相对应的惯性。此外，我们将车轮建模为平面、不可变形的圆盘。
        图 2 阐明了我们分别对惯性系、基座系和脚坐标系 I、B 和 F 的定义。在轮式脚的环境下，F 的运动在站立过程中受到运动学滚动束缚。由于我们在《盘算模子》中同时求解平面基座和脚的运动轨迹，但力求制止非线性束缚，因此需要满意以下条件： F 的运动学束缚必须不依赖于其与底座的相对位置。对于传统的点接触脚来说，由于它在站立过程中是固定的，因此这一点很容易满意。然而，对于车轮来说，这就对运动造成了限定。对于装有不可转向轮子的 ANYmal [1]，如果底座俯仰角相对于惯性坐标系（w.r.t）为零，则满意条件。附录中对此有进一步概述。

         图 2. 轮式四足机器人的模子，它的足式机器人是无质量的，轮子是平面的、不可变形的。当前的支持多边形用蓝色阴影表示。I 表示惯性坐标系，z 轴与地平面法线 n 相平行；B 表示基本坐标系，原点位于机器人的 CoM。我们将坐标系 F 固定在一条腿的端点，即站立时与地面接触的点（仅显示左后腿），并将该点定义为一条腿的足式。这个定义对我们的环境很有用，因为我们只需改变 F 处的运动学束缚，就能模拟传统的点接触脚和轮子。也就是说，通过定义 F 的 Z 轴与平面法线对齐，X 轴与车轮旋转轴垂直，两者之间的区别就只在于 F 是否可以沿其 X 轴方向具有非零速率分量。
  A. 级联轨迹生成

        图 3 显示了我们的 TO 的完整架构。高层输入是步态4 和由线性和角度基础位置和/或速率组成的参考目标状态。如果步态模式包含完整的飞行阶段，即全部足式腿都处于摆动阶段，则会盘算出基座 z 分量的时变轨迹，其中包含这些弹道自由落体阶段。目标状态用于盘算角度轨迹和平面参考基座轨迹。或者，这些也可以由高级路径规划器等直接提供。由于脚的垂直运动并不影响优化，因此我们将其分开盘算，通过五次多项式对每个摆动阶段举行参数化。为了跟踪轨迹和盘算驱动扭矩，可以利用满身控制器（WBC），例如 [24] 中形貌的控制器。

B. 线性化 ZMP 平衡准则

我们的紧张贡献之一是线性订定了 ZMP 平衡准则。我们从简化的机器人模子开始推导。也就是说，假设机器人的足式没有质量，那么机器人的动力学特性就是自由浮动刚体的动力学特性，其公式为

其中，m 表示机器人的质量，ΘB 表示机器人的惯性张量，g 表示重力矢量，Fext 和 Γext 表示作用在 CoM 上的外力和力矩。左侧的下标表示这些量在哪个坐标系中表示。符号 a、v、r 和 ω 分别表示线性加速率、速率、位置和角速率。例如，I rB F 是以 I 表示的 F 相对于 B 的位置矢量。
对于平展的地面，ZMP 被定义为地面上的一点，在这一点上，重力惯性力引起的力矩--即 (1) 的左手边（LHS）--只有平面法线 n 方向上的一个分量。

其中，RI B 是惯性坐标系到基准坐标系的被动旋转矩阵，即 I r = RIB B r。由于我们让惯性坐标系的 Z 轴与平面法线重合，因此 (2) 可以求解 ZMP 的 x、y 分量，即

其中，zB 和 gz 分别是 I rI B 和 I g 的 z 分量，(-)xy 表示量的平面分量。角度分量的盘算公式为

        其中 RIB,i 表示 RI B 的第 i 行。留意，当 zB 不变时，(3) 表示以 ZMP 为支点的线性倒立摆的运动方程 (EoM)
        为了实现动态平衡，规划的 ZMP 必须始终位于支持多边形内 [10]。在优化过程中，这一标准采用多面体束缚的情势。由于束缚边的方向取决于脚的位置或辅助优化变量（如文献[6]），因此在同时求解脚和底座轨迹时，这些束缚都黑白线性的。因此，要获得线性束缚，必须事先固定边沿方向。如图 4 所示，我们建议在三足式支持阶段这样做。也就是说，我们要求脚位于一组具有固定方向的直线所跨过的凸锥中，这些直线在 ZMP 处相交，使得后者始终包含在所得到的支持多边形中。
        这种线性化的紧张缺点是，它将优化变量限定在满意 ZMP 平衡准则的变量子集中，因此需要细致选择边沿方向，以获得可行的优化问题。直观地说，这个子集的特点是每条边会损失一个旋转自由度（DoF）。
        我们发现，将触地时站立脚的足式髋关节位置毗连起来，选择边沿方向的效果很好。也就是说，我们从一组六个方向向量开始--每对脚的组合各一个 -- 每当一只脚从摆动切换到站立时，我们就将该脚的髋关节位置与全部其他站立脚的髋关节位置毗连起来，重新盘算相关的方向。这一过程可确保在非零偏航运动中，一对落地脚的方向在惯性坐标系中保持不变。否则，在包含双脚支持阶段的纯步行步态中，问题将变得不可行。出于同样的缘故原由，我们通过毗连测量到的脚的位置而不是髋部来初始化初始接地脚对的边沿方向。
三、轨迹优化

        我们将探求双脚和底座轨迹的 x、y 分量的任务表述为一个 QP 问题，其情势为

其中，轨迹参数化变量堆叠在优化变量 ξ 向量中。下文将更详细地先容这种参数化方法，并先容有助于解决 QP 问题的目标和束缚条件。此外，我们还提出了一种热启动方案，并简要讨论了怎样通过求解线性方程组来生成作为 QP 问题输入的剩余轨迹。
A. 参数化

我们通过毗连多项式序列对平面轨迹举行参数化。我们把从时间 t 开始的 n 阶多项式写成

其中，α∈ Rn+1 是系数向量，而

τ := t - tstart。通过对 ηn（t，tstart）举行微分，可以得到与时间相关的导数，例如 p˙(t) = η˙n (t, tstart)T α。
1) 基线段：我们将形貌基点位置平面分量的两个五次多项式组成每个基点段：

其中，t start B,k 是第 k 个基段的起始时间。
2) 摆动脚分段：同样，我们利用成对的三次多项式来组成形貌摆动中脚的运动的线段：

其中，t start F,l 是第 l 个脚段的起始时间。
3) 站姿脚段：对于轮式脚来说，在站立阶段的轨迹必须满意滚动束缚。因此，我们用形貌 F 沿其 x 轴速率的二次多项式和线段开始时的位置 I rstart IF,l 来对这些线段举行参数化。也就是说，F 在惯性坐标系中的速率为5

位置的积分效果为

        其中 I∈R2×2 表示同一矩阵。由于 (11) 对于非小角度运动没有剖析解，因此必须利用数值积分方案。不外，由于积分不依赖于任何优化变量，因此不会增加 QP 问题的复杂性。
        4) 分段一连时间：我们通过设定基脚、摆动脚和站立脚的最大一连时间的上限来间接设定求解复杂度。思量到这些限定，我们将每个摆动和站立阶段分别划分为多少隔断相等的摆动脚和站立脚分段，并选择基脚分段，使得每当一只脚在站立和摆动之间切换时，也有一个新的基脚分段开始。这对于不相交的支持多边形之间的转换非常紧张，拜见第 III-C2 节。
B. 目标

        下面列出了 (5) 中目标函数的代价项概览。为了使得到的 Hessian Q 为正定值，我们在其对角线元素上添加了一个正则因子 ρ，如 [7] 中的ρ = 10-8。这就确保了 QP 问题的凸性。
        1) 加速最小化：例如，与 [8] 和 [25] 中的做法一样，我们利用以下情势的代价来最小化形成轨迹的多项式的加速率

        其中，Δt 表示分段一连时间。该积分对全部 n 阶多项式都有剖析解，并可作为 Δt 的函数离线盘算。
        2) 基准状态：为了让机器人朝着抱负的目标状态前进，我们会对最终基准位置、速率和加速率与参考轨迹的偏差举行处罚。此外，为了以可调整的精度跟踪所需的路径，我们对参考轨迹的位置偏差举行采样，这也是抵消漂移的正则化方法。
        3) 臀部到脚的隔断：为制止出现脚型过宽或过窄的解决方案，我们对一条足式跑鞋的臀部与足部之间的隔断按以下方式举行处罚

其中，rB H 为从基点到臀部的位置。我们在每个姿态段对这一目标举行固定次数的采样。
4) ZMP 和支持多边形中心：关于足式上的稳健平衡和负载分配，最好让 ZMP 远离支持多边形的边和顶点。因此，我们对 ZMP 与支持多边形多少中心的隔断举行以下处罚

        其中，G(t) 表示 t 时间的足式脚集合，nG(t) 表示它们的数目。
C. 束缚条件

        下面列出了与 (5) 相关的束缚条件。
        1) 初始状态：我们对初始轨迹位置和速率举行限定，使其与测量到的机器人状态和上一个解决方案之间的线性插值得到的融合状态相匹配。我们不对初始基础加速率举行束缚，因为这会低落对干扰的反应能力，并可能阻碍 ZMP 平衡准则的实现。
        2) 航段交界处：要获得两次一连可变的基本轨迹，一连线段的位置、速率和加速率必须在交界处匹配：

然而，我们省略了在标志着两个可能不相交的支持多边形6 之间过渡的交界处对加速率的限定，因为在这种环境下，ZMP 需要可以或许在两个多边形之间跳跃7。
我们对一连脚段的位置和速率施加雷同的束缚，以获得一次一连可变的轨迹。不外，一连的姿态段需要特别思量，以免引入多余的束缚，因为数值优化程序可能要求相等束缚矩阵具有全秩。因此，我们将相应的束缚条件写成

        其中 V (t, tstart F,l ) = (t - t start F,l )2 (t - t start F,l )100 。(16) 的第三行要求在坐标系中表示的速率的 x 分量相等。由于 y 分量在结构上为零，见 (10)，因此不必加以限定。
        3) 足式伸展：为防止双腿到达运动学极限，我们要求每只脚都位于以地面投影臀部位置为中心的规则多边形内。我们以固定频率对这一不等式束缚举行采样
        4) ZMP 平衡标准：第 II-B 节中提出的 ZMP 平衡标准的表述会导致基脚和站脚的不等式束缚，其情势为

        其中 e∈R2 是形貌边沿方向的法向量。标量既可以是单足式和双足式支持阶段的正放松余量，也可以是负安全余量，用于防止 ZMP 位于支持多边形边界附近的解。我们以固定的频率对所得到的束缚条件举行采样，但完全飞行阶段除外，因为在这些阶段不存在 ZMP。
        5) 单纯行走：如果找到的解决方案能让姿态脚保持在原位，我们可以添加相等束缚条件，要求参数化二次速率多项式的姿态系数为零，即 αF,l = 0。这样，我们的 TO 就能用于没有轮子的传统四足机器人。
D. 热启动

        就最优性而言，不需要利用初始猜测，因为我们求解的是一个凸 QP 问题，因此它有唯一的最优值。但是，通过提供一个合适的初始猜测，可以大大减少数值 = 优化程序的迭代次数。因此，我们用上次优化的解在相应的移位时间刹时 t ˆstart 的状态来初始化形貌每个分段初始状态的系数，并将其余分段系数设为零。我们所说的 “移动 ”是指，对于一个分段的起始时间 t start，上一个解的评估效果为 ˆstart = t start + Δtp ，其中 Δtp 是自上一次优化开始以来所经过的时间（我们假设内部轨迹从零开始）。因此，轨迹的末了 Δtp 长部门没有解决方案。作为简单的补救步伐，我们设置相关的分段系数，使其与上一个解决方案的最终状态相匹配。特别是在位置偏移较大的环境下，这比将其设置为零更好。
E. 输入轨迹

        如图 3 所示，我们的优化将角度轨迹分量、垂直基座运动和需要跟踪的平面参考基座轨迹作为输入。这些都可以用任何参数化方法来表示，以确保两次一连可微分性。我们选择五次多项式序列，这些序列由雷同于 (15) 的相等束缚毗连。为了塑造它们，我们利用情势为 (12) 的代价项，并对交界处的位置、速率和/或加速率施加相等束缚。我们通过融合前一个解决方案、参考目标状态、测量状态和步态模式中的信息来获得这些束缚条件。特别是，我们要求垂直基础加速率在完全飞行阶段与重力相匹配。由于所产生的最小化问题只包含线性相等束缚，因此可以通过提出对偶问题并求解相关的线性方程组直接获得其解决方案，因此不需要迭代优化程序。
四、效果与讨论

        我们用 C++ 实现了 TO。在矩阵盘算中，我们利用了开源线性代数库 Eigen [26]，在求解 (5) 时，我们利用了开源、最先进的求解器 OSQP [27]，该求解器基于乘数瓜代法，并利用了我们问题的稀疏性。运动学和动力学盘算利用开源库 Kindr [28] 和 RBDL [29]，仿真在机器人仿真环境 Gazebo [30] 中举行，物理引擎为 ODE [31]。
A. 设置

        我们在模拟装有不可转向驱动轮的 ANYmal 时测试了我们的框架。根据其关节构型，我们将基本俯仰轨迹设为零，以满意第二节所述条件。这确保了 (11) 中的积分不依赖于任何优化变量。我们还将滚转运动设为零，因为我们认为它对于执行下文中的运动并不紧张。在轨迹跟踪方面，我们利用了文献[24]中形貌的 WBC，它能以 400 Hz 的频率生成关节和车轮执行扭矩，同时思量到各种束缚条件，如执行器限定和摩擦锥束缚。
        关于求解复杂度，我们统一选择最大分段一连时间为 0.2 秒，并每隔 0.1 秒对目标和束缚条件举行一次抽样，但 ZMP 平衡准则除外，我们每隔 0.05 秒对其举行一次抽样。对于 2 秒的规划时间跨度，这导致 QP 问题的大小约为 6 × 102 个优化变量、3 × 102 个相等束缚条件和 7 × 102 个不等式束缚条件。因此，TO 的总盘算时间约为 20 毫秒，其中解决 QP 问题约占一半时间。
B. 模拟

        图 5 展示了我们的 TO 针对不同步态和参照物生成的轨迹。得益于第 III-C5 节中的纯步行束缚，我们的 TO 可用于为具有点接触脚的四足机器人生成运动轨迹，如图 5(a) 所示，用于动态横向步行（步态模式示意图见图 7）。然而，图 5(b) 和 5(c) 显示了我们方法的真正上风，在这两张图中，驱动大大减少了到达目标位置所需的步长和步数。图 5(c) 中的参照物解决方案尤其引人注目。我们的 TO 并没有试图通过横向踱步直接到达目标状态，而是找到了一种雷同于倒车入库的解决方案。相比之下，这大大减少了底座的横向移动，而根据我们的经验，横向移动对于稳健执行起搏步态至关紧张，因为起搏步态自己会体现出较大的横向加速率。10 这一效果凸显了在一次优化中盘算双脚和底座轨迹平面分量的好处，因为这种举动是利用启发式方法独立盘算双脚轨迹所无法预期的。

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。

欢迎光临 IT评测·应用市场-qidao123.com技术社区 (https://dis.qidao123.com/)