可变形四足机器人规划与控制框架：提升复杂地形适应性的仿生多构型运动策略

《Advanced Intelligent Systems》：Planning and Control Framework for a Quadruped Robot With Changeable Configuration

本文提出了一种集成非线性全身规划器与分层全身控制器（WBC）的控制框架，解决了多构型四足机器人在复杂地形下的构型切换与运动控制难题。该框架通过结合参考姿态、系统动力学、落脚点位置等非线性约束的优化控制问题（OCP）计算最优状态，并利用基于二次规划（QP）的分层优化实现精准运动跟踪，实验验证了其在斜坡、不规则砖地、悬垂障碍及双平台桥梁等多种挑战性场景中的鲁棒性与多功能性。

　　

引言

腿足机器人从自然界中腿足动物获得仿生灵感，展现出卓越的地形适应性和灵活运动能力。其中，四足机器人因其比双足机器人更高的稳定性以及比多足机器人更简单的机械结构而受到广泛关注。现有四足机器人主要基于哺乳动物（如狗、猎豹）或爬行动物和昆虫（如蜥蜴、蜘蛛）的仿生模型设计，这些机器人因仿生对象不同而表现出不同的运动能力。单构型四足机器人在运动能力和适应性方面存在局限，而多构型四足机器人结合了哺乳动物和爬行动物形态的优势，因此受到关注。

当前主流四足机器人大多采用类哺乳动物构型，根据膝关节弯曲方向可分为肘-膝、膝-膝、膝-肘和肘-肘四种类型。例如，Mini Cheetah、Aliengo和Spot采用的肘-肘构型在爬楼梯和爬坡等任务中表现出强大的运动能力和地形适应性；而膝-膝构型更适用于下楼梯和下坡，肘-膝构型则更适合快速瞬态运动任务。类哺乳动物机器人通常具有相对较高的重心（CoG），表现出更高的敏捷性，能够克服具有挑战性的地形，但其运动稳定性通常低于类爬行动物四足机器人，且其越障能力受膝关节弯曲方向的限制。相比之下，类爬行动物四足机器人（如TITAN-XIII和SILO4）具有低重心、高稳定性和强大的越障能力，尤其在狭窄空间中表现突出，但其灵活性和运动速度相对较低。总体而言，具有单一固定构型的机器人在机动性和越障能力方面常遇到限制，有时会导致腿部与障碍物边缘碰撞。相比之下，多构型四足机器人（如THU-QUAD II、ALPHRED和Origaker）可以动态切换膝关节方向和形态，从而实现卓越的机动性和地形适应性。

现有多构型机器人的构型规划主要基于运动学模型。THU-QUAD II和ALPHRED利用基于运动学的规划器和结构设计特点实现构型切换和在复杂地形上的运动。Origaker使用预定义轨迹和不同的逆运动学（IK）求解器实现模式切换和在不同复杂环境中的运动。上述方法的潜在局限性在于规划时可能未充分考虑机器人的运动可达性，这可能会限制其实现机器人全部适应潜力的能力。对于具有冗余关节的变形机器人，逆运动学常因多个可行的关节配置而呈现歧义。Kun Xu等人通过锁定特定关节解决了四自由度（DoF）单腿变形四足机器人的构型切换和稳定运动问题。类似地，RoboSimian使用逆运动学查找表结合RRT-Connect算法进行运动规划。这些方法通常通过固定某些关节来实现控制，这可能限制了在导航复杂地形时充分利用冗余关节优势的能力。相比之下，Weaver机器人将末端执行器脚趾方向约束为与重力矢量对齐，从而在每个腿的所有关节在多样化地形中实现协调灵活的使用。然而，这种方向约束可能限制机器人能够穿越的最大障碍高度。本文通过制定包含最大关节扭矩、关节运动范围和参考运动构型等约束的非线性优化问题，充分探索了多构型四足机器人在复杂环境中的潜力。这些因素对于规划多构型四足机器人充分利用所有关节执行复杂任务（如穿越与机器人尺寸相当的障碍物）至关重要。

在四足机器人的控制策略方面，基于模型和基于强化学习的方法被广泛采用以实现精确的轨迹跟踪和稳定运动。ALPHRED和THU-QUAD II使用PD控制器进行各种构型下的运动控制；然而，这些方法主要依赖于运动学模型，限制了机器人在动态或复杂环境中的适应性。相比之下，MIT Cheetah 3和Mini Cheetah采用模型预测控制和全身控制实现多种步态的高速运动。类似地，HyQ使用基于虚拟模型的全身控制框架成功导航具有挑战性的地形（如缝隙）。ANYmal采用强化学习技术实现在多样化环境中的运动，包括山道、松散碎石甚至跑酷场景。由于横向伸展的肢体姿态，类爬行动物构型通常比类哺乳动物构型需要更多的重力支撑能量来维持身体稳定性。由于这些形态差异，直接将现有控制方法应用于多构型四足机器人在导航异构粗糙地形时仍然是一个挑战。本文通过将规划器优化的状态和控制输入与分层全身控制器集成来解决这个问题，实现灵活的构型切换和运动控制。

为了充分利用具有多自由度的多构型四足机器人在越障和地形适应性方面的优势，必须开发一个明确考虑机器人运动约束的运动规划与控制框架。这样的框架能够实现不同构型之间的灵活转换，并最大化每种形态的结构优势。因此，本文提出了一种集成非线性、多约束全身规划器与分层全身控制器的控制框架。

机器人系统概述

开发了一款名为SMAR-Q的56公斤四足机器人，能够改变其构型以适应各种环境。机器人的每条腿有四个自由度。每个自由度由定制化的本体感知电机驱动，可实现全扭矩控制。与传统的四足机器人相比，本文描述的机器人在原有的髋关节外展内收（HAA）关节和髋关节屈伸（HFE）关节之间增加了一个髋关节内外旋（HIE）关节。机器人利用冗余自由度实现在各种运动构型之间的切换，包括类爬行动物和类哺乳动物构型。这种适应性使机器人能够根据不同运动场景选择合适的构型，从而增强其在复杂地形中的通过性和稳定性。

HFE和膝关节屈伸（KFE）电机同轴平行安装，KFE执行器通过连杆机构将运动传递到小腿。机器人的躯干和腿部外壳由7075高强度铝合金加工而成，脚部结合了天然橡胶和3D打印组件。控制系统基于搭载Intel Core i7-8665UE处理器的UP Xtreme，并运行带有PREEMPT_RT补丁的实时Linux内核，以确保控制程序的实时执行。动力系统为关节执行器和控制器提供稳定的42V和5V输出。驱动系统包括16个自主研发的本体感知执行器，每个执行器通过9:1的行星齿轮减速比可提供高达±144 Nm的扭矩。

展示了多种类哺乳动物和类爬行动物运动构型，这些构型可以根据需要任意切换。每种构型都有特定的优势：肘-膝构型适用于平坦地形；膝-肘构型适用于不平坦或起伏地形；肘-肘构型适用于爬楼梯和爬坡；膝-膝构型适用于下楼梯和下坡，可减少膝关节与环境的碰撞并增强稳定性；类爬行动物构型具有低重心和强稳定性的特点，使机器人能够通过狭窄空间并克服悬垂障碍。类哺乳动物构型（a-d）可以通过反转膝关节方向来互换，而无需改变落脚点轨迹。类哺乳动物和类爬行动物构型之间的转换首先通过反转膝关节弯曲方向，然后通过外展/内收降低躯干和调整脚部位置来实现。机器人系统的整体控制框架包括一个多约束全身运动规划器和一个分层全身控制器。

全身规划器

为了充分利用多构型机器人的多自由度能力，需要开发一个全身运动规划器，在尊重关节空间限制、扭矩能力和自碰撞约束的同时，最大化机动性和对复杂地形的适应性。引入了一个基于多约束的非线性规划器，考虑了机器人的关节运动限制、自碰撞避免、脚-地接触力、参考构型和其他相关约束。不同构型之间的运动规划基于机器人的运动学-动力学模型。该框架通过将不同构型下的机器人系统视为具有预定义模式的切换系统来处理动态系统的最优控制问题（OCP）。因此，我们通过构建惩罚函数、松弛障碍函数等来处理具有等式和不等式约束的切换系统的最优控制问题。然后应用序列二次规划（SQP）求解器计算最优解。

系统建模

多关节浮动基系统可以表示为欠驱动刚体和完全驱动肢体的组合。因此，多构型四足机器人的动力学如下。广义关节角度和速度分别用q和其导数表示。M(q)表示广义质量矩阵；h(q, q?)代表科里奥利力、离心力和重力项的 combined。J_c^T(q)是脚部接触的堆叠雅可比矩阵，τ_a表示关节扭矩。f_c表示脚部施加的接触力。

躯干在方程中的欠驱动动力学可以通过适当的变换用质心动力学重新表述。其中m是四足机器人的总质量，k表示质心动量，p_com和l_com分别表示线动量角动量。n_c表示支撑相的腿数。r_ci/com表示机器人脚部接触点c_i相对于质心坐标系的位置，f_ci表示作用在第i只脚上的接触力。

同时，质心动量使用质心动量矩阵A(q)表示为相对于关节位置和速度的向量。质心动量矩阵A(q)由躯干A_b(q)和驱动关节分量A_j(q)的贡献组成。q_b表示身体姿态，q_j表示关节位置。因此，质心动量可以使用矩阵A(q)表示。其中x_b是世界坐标系下的身体姿态。方程中的机器人系统状态向量定义为x = [k^T, q_b^T, q_j^T]^T。方程中的输入向量定义为u = [f_c^T, q?_j^T]^T，由脚部接触力和关节角速度组成，其中n_a表示关节执行器的数量。

参考落脚点

落脚点规划结合了Raibert启发式p_sym和离心力引起的落脚点偏移p_cent来确定标称落脚点位置。同时，为了平衡系统稳定性和规划复杂性，我们优先考虑参考脚部位置，同时降低严格方向控制的优先级。其中p_hip,i表示第i条腿的髋关节相对于世界坐标系的位置。p_k表示第k个时间点躯干质心在世界坐标系中的位置，l_i是髋关节相对于躯干坐标系的位置，k_p是反馈增益。R_des表示期望的姿态旋转矩阵，R_plane表示地形自适应旋转矩阵。v_B,des表示机器人的期望身体速度，而v_B表示来自机器人的实际身体速度反馈。地形自适应角度使用参考文献中提出的地形估计方法进行估计。

摆动相轨迹使用三次贝塞尔曲线进行规划，该曲线结合了期望的起飞和着陆位置和速度以及顶点位置来生成脚部轨迹。参考关节角速度设置为零。机器人的总重量在所有接触腿之间平均分配以定义接触力参考。对应于基座和参考落脚点的参考关节位置使用微分运动学和参考构型计算。逆运动学使用参考文献中提到的的最小二乘法求解。图中的构型被用作预定义的运动参考构型，这有效地防止求解器收敛到局部最优或无法收敛。因此，机器人的参考关节角度为q?_j,ref = H^-1J_c,i^T(J_c,iH^-1J_c,i^T)^-1(v_foot,i,des + K_ike)。其中H表示关节权重矩阵，J_c,i表示与第i条腿相关的接触雅可比矩阵，v_foot,i,des是第i条腿的期望脚部速度。e表示脚部位置的跟踪误差，K_ik表示比例增益矩阵。Δt = 2 ms是控制器更新间隔，q_ref表示参考关节角度。

成本与软不等式约束

机器人的运行成本函数L(x, u, t)主要由多构型跟踪成本函数L_e和惩罚成本函数L_p组成。

跟踪成本函数L_e主要基于质心动量、身体姿态以及由落脚点轨迹生成的参考关节角度。

L_e = (1/2)[α₁(x - x_ref,s)^TQ_S(x - x_ref,s) + α₂(x - x_ref,d)^TQ_D(x - x_ref,d) + u^TR_Du]。

其中Q_S和Q_D是半正定权重矩阵，R_D是正定权重矩阵。x_ref,s表示构型切换期间的参考状态变量，x_ref,d表示运动期间的参考状态变量。参数α₁实现不同形态之间的构型切换，在主动切换时α₁ = 1，否则为0。类似地，参数α₂激活给定构型内的稳定运动任务，当在当前构型中执行运动时α₂ = 1，否则为0。

惩罚成本用于通过使用松弛障碍函数（RBF）增强成本函数来处理不等式约束。松弛障碍函数是连续且二次可微的，可以转换为接近约束边界δ或远离约束边界δ的不同函数。

B(s, μ, δ) = { μ(-ln(-s) + (s/δ)) if s ≤ -δ; μ((s/δ)² + ln(-s) - 1) if -δ < s < 0 }。

其中μ表示障碍函数的权重（或强度）。因此，总惩罚函数公式如下：

L_p = Σ_i=1⁴[B_JL,i(q_j,i, μ_JL, δ_JL) + B_FC,i(f_c,i, μ_FC, δ_FC)] + Σ_k=1^n_pB_SC,k(d_k, μ_SC, δ_SC)。

其中B_JL,i表示第i条腿的关节限制约束，B_FC,i表示第i条腿的摩擦锥约束。B_SC,k表示自碰撞约束，其中n_p是自碰撞对的总数。

腿部的关节限制约束包括上边界约束s_max = q_j,max - q_j和下边界约束s_min = q_j - q_j,min，分别对应于关节位置、速度和扭矩。

关节限制约束：q_j,min ≤ q_j ≤ q_j,max, q?_j,min ≤ q?_j ≤ q?_j,max, τ_min ≤ τ ≤ τ_max。

摩擦锥约束表示为：

√(f_x² + f_y²) ≤ μ_cf_z + ε。

其中μ_c表示摩擦系数，ε > 0是确保约束平滑性的参数。

自碰撞避免通过不等式距离约束实现：

d_k(q) ≥ ε_k。

其中d_k(q)表示第k个碰撞对之间的距离，ε_k表示该碰撞对的最小允许距离阈值。

等式约束

对于处于摆动相的腿，脚部接触力被约束为零。对于摆动腿，脚部速度被约束为与参考轨迹的表面法线方向对齐。

摆动腿接触力约束：f_c,i = 0。

摆动腿速度约束：v_foot,i · n_surface = v_ref,i · n_surface。

在支撑相期间，脚部需要保持与地面接触而不打滑；因此，其速度被约束为零。

支撑腿速度约束：v_foot,i = 0。

然后终端成本函数为：

Φ(x(T)) = (1/2)[α₁(x(T) - x_ref,s(T))^TW_S,f(x(T) - x_ref,s(T)) + α₂(x(T) - x_ref,d(T))^TW_D,f(x(T) - x_ref,d(T))]。

其中T是规划器的预测范围。W_S,f和W_D,f是半正定权重矩阵。

分层全身控制器

构建了一个分层全身控制器，以精确跟踪非线性规划器优化的参考运动轨迹。该控制器通过求解一系列具有任务优先级的二次规划问题来找到最优接触力和关节加速度。同时，在运动过程中考虑机器人的全身动力学模型、接触运动和其他约束，使机器人的运动控制更加鲁棒和稳定。一个运动任务T可以表示为解向量ν = [q?^T, f_c^T]^{T>上的一组线性等式和/或不等式约束的标准形式，表示为：}

A_eqν = b_eq, A_ineqν ≤ b_ineq, min(||A_taskν - b_task||² + w^TW w + ρ^TP ρ)。

其中w和ρ是引入的要最小化的松弛变量。优化在更高优先级任务的子空间内求解，确保最大限度地满足低优先级任务。

总结了SMAR-Q机器人的分层任务及其相应的优先级。为确保可行性和系统稳定性，动态一致性、摩擦锥和接触运动约束被赋予最高优先级，以防止打滑和不稳定。躯干质心位置和姿态跟踪任务被赋予次要优先级，以实现精确运动，同时满足关节空间限制。摆动腿跟踪和脚部接触力优化在运动过程中被视为较低优先级的任务。所有约束和控制目标都以方程中定义的标准形式统一表达。基于此公式，构建了一个基于分层二次规划的全身控制器，其中求解一系列二次规划问题以严格执行任务优先级排序。假设分层全身控制器中的优化变量是ν = [q?^T, f_c^T]^T，其中q?是广义加速度，f_c对应于脚部的接触力。机器人系统需要满足的不同任务要求如下。

动态一致性：作用在机器人上的力和加速度需要满足动态约束，以确保运动和力的可行性。

动态约束：M(q)q? + h(q, q?) = J_c^T(q)f_c + τ_a。

接触运动约束：由于接触腿在地面上必须保持静止，因此接触脚的加速度被约束为零。

接触脚加速度约束：J_c,iq? + J?_c,iq? = 0。

其中J_c,i是第i个接触脚的雅可比矩阵。

躯干质心轨迹跟踪：我们希望质心跟随精确运动规划器规划的轨迹。因此，设计了一个PD状态反馈控制器来跟踪质心的轨迹。

质心加速度期望：a_B,des = a_B,ref + K_p,com(r_B,ref - r_B) + K_d,com(v_B,ref - v_B)。

其中r_B,ref和v_B,ref分别表示相对于世界坐标系的期望躯干位置和速度，而r_B和v_B表示相应的反馈值。K_p,com和K_d,com是定义比例和微分增益的对角正定矩阵。

躯干姿态跟踪：PD状态反馈控制器也用于实现跟踪期望的躯干姿态。框减运算符?用于定义期望姿态R_B,ref和反馈姿态R_B之间的姿态误差。

姿态误差角加速度期望：ω?_B,des = ω?_B,ref + K_p,ori(R_B,ref ? R_B) + K_d,ori(ω_B,ref - ω_B)。

其中ω_B,ref和ω_B分别表示期望的姿态角速度和反馈的姿态角速度。K_p,ori和K_d,ori分别是躯干姿态跟踪的比例增益和微分增益。

摆动腿轨迹跟踪：使用状态反馈控制器实现摆动腿轨迹跟踪。

脚部加速度期望：a_foot,i,des = a_foot,i,ref + K_d,swing(v_foot,i,ref - v_foot,i) + K_p,swing(p_foot,i,ref - p_foot,i)。

其中J?_swing,i表示摆动腿雅可比矩阵的时间导数。a_foot,i,ref, v_foot,i,ref, 和p_foot,i,ref分别表示期望的脚部加速度、速度和位置。v_foot,i和p_foot,i表示反馈的脚部速度和位置。

接触力跟踪：脚部的接触力应与规划器计算的最优参考接触力一致。

接触力误差最小化：min ||f_c - f_c,ref||²。

其中f_c,ref表示规划器获得的参考脚部接触力。

摩擦锥约束：为防止脚部打滑，脚部接触力的切向和法向分量必须保持在摩擦锥内，这是通过线性不等式约束来执行的。

摩擦锥约束：A_FCf_c,i ≤ b_FC。

其中

A_FC = [1, 0, -μ; -1, 0, -μ; 0, 1, -μ; 0, -1, -μ], b_FC = [0; 0; 0; 0]。

μ是地面摩擦系数。

关节扭矩限制：为确保关节扭矩不超过关节执行器的最大限制，基于机器人的动力学模型制定不等式约束。

关节扭矩约束：τ_l,min ≤ S_j(M(q)q? + h(q, q?) - J_c^T(q)f_c) ≤ τ_l,max。

其中

τ = S_j(M(q)q? + h(q, q?) - J_c^T(q)f_c)。

这里，τ_l,min和τ_l,max分别表示腿部关节的最小和最大允许扭矩限制。

实验部分

为验证本文提出的控制框架的有效性，进行了一系列涉及不同构型下各种运动任务的实验。在类哺乳动物构型下，评估了机器人在两种运动场景中的表现：1) 穿越不规则砖地后爬坡；2) 跨越高障碍物。在类爬行动物构型下，考虑了另外两种场景：1) 导航球形护柱后穿越悬垂障碍物；2) 爬坡后通过带有间隙的双平台桥梁。根据地形和任务要求选择仿生构型以优化性能。

类哺乳动物构型粗糙地形运动实验

为展示所提出规划和控制算法的鲁棒性，设计了一项运动任务，要求机器人连续穿越两个具有挑战性的场景：不规则砖地 followed by 一个斜坡。如图所示，机器人在膝-肘构型下导航不规则砖地，然后切换到肘-肘构型爬上一个22°的斜坡。机器人通过这两个地形时躯干姿态角的变化如图所示。当机器人穿越非结构化砖地时，由于脚部打滑或部分失去支撑，其姿态角出现波动。具体而言，滚转角波动高达7°，横向速度变化约0.26 m/s，但两者都迅速恢复到稳定值。当机器人切换到肘-肘构型爬坡时，躯干姿态适应斜坡倾角，表现出良好的适应性，没有明显的振荡。机器人在斜坡和粗糙地形上表现出良好的适应性和鲁棒性。

如图所示，机器人爬上一个19厘米高的台阶，然后从肘-肘构型过渡到膝-膝构型以平稳下降。视频和图表均表明机器人在整个过程中避免了碰撞，并成功稳定高效地穿越了高障碍物。如图所示，机器人在保持脚部位置不变的情况下，顺利地从肘-肘构型过渡到膝-膝构型。在过渡开始时关节角度和脚部位置出现轻微波动，但迅速稳定下来。这种构型切换有效防止了小腿与台阶边缘碰撞，无需掉头，提高了