一种基于改进模型预测控制的四足机器人稳定运动方法 |
|||||||
| 申请号 | CN202311670365.0 | 申请日 | 2023-12-07 | 公开(公告)号 | CN117970948A | 公开(公告)日 | 2024-05-03 |
| 申请人 | 河北工业大学; | 发明人 | 刘今越; 张相波; 唐春瑞; 姜兆发; 李铁军; | ||||
| 摘要 | 本 发明 提出了一种基于改进模型预测控制的四足 机器人 持续稳定跟随期望身体轨迹的运动方法，包括以下步骤：步骤一：建立四足机器人系统 状态方程 ；步骤二：根据机器人系统状态方程建立优化足端 力 以及机器人状态的代价函数和地面约束条件；步骤三：根据四足机器人各部分的 质量 、尺寸以及惯量，利用CasADi库生成其动力学模型，降低因模型简化带来的误差；步骤四：将机器人系统状态以及约束方程编写成C++代码，并利用IPOPT非线性求解器求解离散的最优MPC控制率。该控制方法使四足机器人在每一个步态周期中以最优足端力运动，大大降低了地面反力对机器人运动 稳定性 的影响。 | ||||||
| 权利要求 | 1.一种基于改进模型预测控制的复杂环境下四足机器人稳定运动方法，其特征在于： |
||||||
| 说明书全文 | 一种基于改进模型预测控制的四足机器人稳定运动方法[0002] 四足机器人由于其特殊的腿部结构可以轻松跨越障碍，实现轮式或履带式机器人无法完成的任务。然而目前四足机器人运动场景受限，在遇到地面存在坑洞或者遇到阶梯时难以实现稳定性运动。而四足机器人在运动过程中的稳定性完全依靠触地腿的支撑，所以获得一种能够使其在复杂崎岖地面上稳定运动的算法至关重要。 [0003] 目前，四足机器人通过状态估计器估计机器人机身状态结合IMU信息实现其稳定性运动的方法可大致分为两类： [0004] (1)利用离散卡尔曼滤波器优化机器人在下一个步态的周期中的机器人状态，使其与期望的机器人状态之间的误差的方差最小，实现其跟踪期望轨迹稳定运动的目标。使用这种算法存在建模和离散化的误差，后续需要调节测量噪声协方差矩阵和输入量产生的过程协方差中的参数，调节不当导致机器人状态发生漂移，运行轨迹不准确。 [0005] (2)通过模型预测控制算法(MPC)优化机器人未来多个步态周期中足端触地时的足端位置以及机器人状态。机器人状态包括机身质心位置、速度、俯仰角度。模型预测控制器可以在未来的预测时域内在一定的状态轨迹和足端力输入的约束下，找到控制输入的最优序列和相应的状态轨迹。但是这种方法难以实时准确预测未来多个周期内的足端力，从而无法有效减弱触地瞬间足端冲击对机器人稳定运动的影响。 [0006] 近年来，逐渐出现了一些功能强大的非线性求解器，比如Gurobi、IPOPT和CasADi等。这些非线性求解的出现可以使的包含地面反力和摩擦锥约束的系统状态方程得到快速求解，也使得MPC控制器可以得到频率较高的离散控制率。 发明内容[0007] 本部分主要目的是介绍提出的能够实时可执行的四足机器人稳定运动控制方法，详细说明了所提出方法中包含的基本概念和控制流程。同时也介绍了在真实机器人中实现算法的例子。在本部分及本申请的说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。 [0008] 针对四足机器人在复杂地面无法保持稳定可靠的运动，本发明提出了基于非线性模型预测控制的机器人运动控制算法，预测未来多个步态周期足端力以及机器人系统状态，包括机器人质心位置、速度、加速度以及俯仰角度。在输入状态和摩擦约束下通过反复迭代只将下一个采样间隔的第一个时间步的控制信号输入给系统。可以使机器人在触地瞬间足端力冲击较小的同时，按照期望系统状态运动。 [0009] 本发明解决技术问题所采用的技术方案是基于非线性模型预测控制的四足机器人运动控制方法，包括以下步骤：第一步：建立四足机器人系统状态方程；第二步：建立优化足端力以及机器人状态的代价函数和约束方程；第三步：根据四足机器人的腿部和机身的质量以及惯量，利用CasADi库生成其动力学模型；第四步：将机器人系统状态以及约束方程编写成C++代码并利用IPOPT非线性求解器求解离散的MPC控制率。 [0010] 进一步地，步骤一包括：本发明的研究对象是四足机器人在复杂环境下实现稳定运动的问题，其中步骤一建立的四足机器人系统状态方程如下所示： [0011] x(k+1)＝Ax(k)+Bu(k) [0012] [0013] 其中θ表示机器人机身在世界坐标系下的x，y，z方向上的位移；p表示机器人俯仰角度(pitch)，为机身在x，y，z方向上的速度；为机器人机身俯仰角速度；Rz(Φ)为绕z轴旋转的旋转矩阵；ri表示从机器人质心到足端距离向量；fi为足端触地时所受到的足端反力。 [0014] 进一步地，步骤二包括：结合四足机器人系统状态方程建立其摩擦约束关系、最大足端力约束关系以及优化足端力和机器人状态的代价函数。后续利用非线性求解器求解出来满足约束条件的MPC控制率即最优的足端力，并利用逆动力学求出离散的关节力矩，即可实现四足机器人的稳定运动。本发明提出了通过以下优化问题来求解离散的MPC控制率。 [0015] 本发明将优化过程归结为以下优化问题： [0016] [0017] [0018] 其中N为预测区间，Q为系统状态的对角权重矩阵，R为系统输入的对角权重矩阵，x(k)为系统状态，x(k+i|k)为在k时刻预测的第k+i时刻的系统状态，x(k|k)ref＝x(k)为期望的机器人状态，u(k)＝u(k|k)ref为期望足端力。 [0019] 通过求解上述代价函数可以确定最优地面反力，从而更好的跟踪期望身体轨迹。 [0020] 进一步地，步骤三包括：提出的算法中所用的计算过程中所用的机器人动力学模型由CasADi库自动生成。生成机器人动力学模型时需要机器人各部分长度、质量以及惯性质量。机器人模型参数如附图2所示。然后将CasADi库中的文件导入MATLAB中，并编写代码将机器人模型参数填进去，生成了对应的机器人动力学模型。 [0021] 进一步地，步骤四包括：将上述非线性模型预测控制优化问题(NMPC优化问题)编写成C++代码与IPOPT接口结合，在ROS中进行验证算法的有效性，并在真实机器人Unitree Go1应用本发明提出的算法，实验结果表明该优化过程可以使机器人在复杂地面环境中实现稳定运动，如附图3所示。 [0022] 本发明的有益效果是，提出了一种基于改进模型预测控制四足机器人在复杂环境下稳定运动的控制方法，解决了四足机器人在无深度相机和激光雷达等辅助设备下难以实现持续稳定运动的问题，此方法可以避免机身IMU和状态估计器估计机身状态不准而导致足端位置和机身状态漂移的问题，使得四足机器人在各种复杂地面中能够稳定安全的作业，具有一定的推广意义。 附图说明[0023] 为了更清楚地说明本发明中提出的算法的具体流程和在真实机器人中的适用，下面将对本发明中提出的算法技术路线或现有技术描述中所需要使用的附图作简单地介绍，下面描述性中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。 [0024] 图1为应用本发明算法控制四足机器人运动流程图； [0025] 图2为本发明四足机器人模型参数图； [0026] 图3为本发明中所提出算法在真实机器人中验证效果图； [0027] 图4为本发明使用提出的算法的仿真验证； [0028] 通过真实机器人运动效果验证，此方法可以避免由于状态估计器估计误差和IMU漂移导致的足端位置和机身状态漂移的问题，使得四足机器人在各种复杂地面中能够稳定安全的运动，从而实现高效作业。 [0029] 以上所述，仅为本发明的具体实施例，不能以其限定发明实施的范围，所以其等同组件的置换，或依本发明专利保护范围所作的等同变化与修饰，都应仍属于本专利涵盖的范畴。另外，本发明中的技术特征与技术特征之间、技术特征与技术方案之间、技术方案与技术方案之间均可以自由组合使用。 具体实施方式[0030] 参照图1，用户通过手柄或者PC给机器人发布线速度或者角速度指令，然后机器人在世界坐标系下沿x，y，z方向上速度或者角速度会传递给NMPC控制器，NMPC控制器使机器人可以基于足端接触时序预测未来多个时间步的可行落足点，然后结合机器人动力学模型优化出足端接触时的足端力，并采用最优的关节力矩发送给足端控制器，控制摆动腿沿着期望的足端轨迹曲线运动。 [0031] 参照图2，在ROS中进行四足机器人仿真运动，仿真结果显示，机器人可以在崎岖地形中保持稳定运动。 [0032] 参照图3，在真实机器人Unitree Go1中部署该控制算法，发现本发明中所提出的算法能够使机器人跟随目标运动轨迹，进行可靠地动态运动，在未来能够大大延伸四足机器人的适用场景，提高四足机器人作业效率。 |
||||||
