一种基于显式模型预测控制的四足机器人控制方法及控制终端

摘要

本发明提供了一种基于显式模型预测控制的四足机器人控制方法及控制终端，属于四足机器人智能控制技术领域，所述基于显式模型预测控制的四足机器人控制方法包括如下步骤：步骤一：确定四足机器人的功能模块；p

说明书

技术领域

本发明涉及一种基于显式模型预测控制的四足机器人控制方法及控制终端，属于四足机器人智能控制技术领域。

背景技术

随着自动化理论、传感器技术、人工智能等技术的发展，推动着移动式机器人技术的不断进步，日益完善的性能使其在工业、农业、国防、救援等方面取得优异的成绩。移动机器人分为：轮式机器人、足式机器人、履带式机器人，其中足式机器人环境适应能力最强。足式机器人通常可以分为：单足、两足、四足、和多足，其中四足机器人相比于多足机器人结构简单，易于控制；相比于单足和双足，具有更好的稳定性和负载能力。对于四足机器人的研究重点是其对不同地形的适应能力和动态性能，其中，控制方法的优劣是其性能的关键所在。

目前，模型预测控制是四足机器人常用的控制方法之一，该方法首先建立单刚体动力学模型，通过动力学模型推导出预测方程，再建立一个优化问题和约束条件，通过在线求解有限时间段的最优化问题，找到一个最优的足端反作用力输入，在下一时刻重新求解优化过程，使得机器人状态有效跟随预定值。MPC需要反复的在线优化，因此，对于四足机器人这种多自由度的系统，该方法在线计算消耗的时间长，实时性差，对控制芯片要求过高，在实际应用中，若要实现在线求解优化问题，必须使用简化的模型以降低运算量，并且采用高性能的板卡提高运算效率。

发明内容

本发明要解决的技术问题，就是针对现有技术所存在的不足，而提供一种基于显式模型预测控制的四足机器人控制方法及控制终端，本发明交叉融合了运动学理论和最优化理论，采用显式模型预测控制的控制方法，可以有效地解决模型预测控制在线计算量大、实时性低的缺点。

一种基于显式模型预测控制的四足机器人控制方法，它包括如下步骤：

步骤一：确定四足机器人的功能模块；

p

步骤二：建立四足机器人的优化问题；

步骤三：以步骤二的优化问题为基础，建立控制输入的显示函数。

优选的，所述步骤二包括如下子步骤：

(1)基于动力学理论给出机器人状态与控制输入之间的关系；

(2)基于运动学理论给出足端力与关节力矩的关系；

(3)构建优化问题。

优选的，所述步骤三包括如下子步骤：

(a)基于多参数二次规划理论，离线划分状态区域；

(b)在线查找分区，得到最优控制输入。

优选的，所述步骤一中的n＝9，其中，p

优选的，所述步骤二的子步骤(1)采用如下方法基于动力学理论给出机器人状态与控制输入之间的关系：

基于牛顿-欧拉法对四足机器人进行动力学分析，牛顿第二定律为：

根据该式，可以得到四足机器人的加速度与足端作用力的关系；

其中：

四足机器人本体坐标系中的惯性张量I

当四足机器人运动时，世界坐标系中的惯性张量可以由下式给出：

I＝RI

其中，R为旋转矩阵，R

根据角动量定理，四足机器人转动惯量的矢量表达式为：

其中，I为四足机器人的惯性张量，ω为四足机器人的角速度，r

四足机器人的角速度ω与偏航角

欧拉角

把四足机器人的角速度ω、欧拉角

其中，0

优选的，所述步骤二的子步骤(2)采用如下方法基于运动学理论给出足端力与关节力矩的关系：

为了将控制输入映射到关节电机从而输出力矩，需要对四足机器人进行运动学分析，利用几何关系，可以得到本体坐标系下第i条腿足端位置与关节角度的关系：

其中，p

其中，J

其中，τ

优选的，所述步骤二的子步骤(3)采用如下方法构建优化问题：

基于上述子步骤(1)的动力学方程，把重力加速度写入系数矩阵，可以将该方程整合为状态空间模型：

其中，

其中，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，

通过建立最小成本的优化问题，保证机器人状态x(k)发生变化时得到最优的足端力输入，目标函数直接反映了预测控制对系统性能要求，选取二次型作为系统目标函数，约束x，y轴方向上的接触力小于或等于最大静摩擦力，基于上述分析，构建的优化问题如下：

其中，J为优化的目标函数，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，

优选的，所述步骤三的子步骤(a)采用如下方法基于多参数二次规划理论，离线划分状态区域：

步骤二的优化问题求解为在线优化过程，需要将该问题过渡到离线阶段求解，线性系统的性质有：

其中，k代表系统k时刻，i代表系统i时刻，A

其中Y、H、F、G、W、E是只与机器人参数有关的常数矩阵，将四足机器人参数带入优化问题J，并整理为关于初始状态x(0)的形式均可得到常数矩阵Y、H、F、G、W、E的固定值，忽略D(x(0))的常数项

把Vz(x)作为目标函数，Z作为决策变量，x(0)作为参数矢量，则该式为标准的多参数二次规划问题，其中H、G、S、W均为常数矩阵，Z

其中，H、G、W、S均为上述中的常数矩阵，Z为决策变量，x(0)为参数矢量即机器人初始状态，λ为拉格朗日乘子，下标i代表矩阵的第i行，G

λ(-GH

当拉格朗日乘数对应于无效约束时λ＝0；当对应有效约束时λ≠0，且

G

则有：

其中，

将该式代入KKT条件的不等式组可以得到：

由该式的不等式可得到四足机器人的初始状态x(0)关于第一个临界域的不等式表达式，将Z＝u+H

由此，可以得到第一个临界域及四足机器人的状态关于输入的显示控制规律，为了求解的准确性，只取u的第一项作用于系统，对于剩余的分区，可以通过相同的步骤获得每个分区上的控制规律，当没有新的有效约束时，停止计算，即可以得到控制序列u关于状态x的分段仿射系统；

在实际应用中，将四足机器人的实际参数带入A、B矩阵，使用matlab的MPT工具箱或者c++的qpOASES库求解得到该式的显示关系，并将它们存于表中供在线阶段使用。

优选的，所述步骤三的子步骤(b)采用如下方法在线查找分区，得到最优控制输入：

通过上述离线阶段的求解，得到了四足机器人所有的状态分区以及对应的显式函数关系，那么显式模型预测控制的在线计算过程就转化为简单的查表过程：在某一时刻，通过检测四足机器人的当前状态，判断当前的状态分区，同时调用控制输入u＝(f

本发明还提供了一种控制终端，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述基于显式模型预测控制的四足机器人控制方法的步骤。

本发明的有益效果：本发明通过对四足机器人控制系统的研究，基于动力学、运动学、多参数二次规划理论等，提出了一种新的基于显式模型预测控制的四足机器人控制方法及控制终端，该控制方法主要包括三个阶段，第一阶段是确定四足机器人的功能模块(包括基本功能：行走、奔跑、踏步、步态切换、跨越障碍、攀爬楼梯，可选功能：气体检测、自主导航，远程监控等)；第二阶段是建立四足机器人的优化问题，在该阶段使用动力学和运动学理论，建立四足机器人控制系统的预测方程和约束条件；第三阶段是以第二阶段的优化问题为基础，构建出多参数二次规划问题，通过离线划分四足机器人状态区域，建立最优控制量(四足机器人的足端反作用力)和初始状态(四足机器人的位置、线速度、角速度、欧拉角)之间显示关系，在线查找状态分区，得到最优控制量。本发明交叉融合了运动学理论和最优化理论，采用显式模型预测控制的控制方法，使用显式模型预测控制控制器的四足机器人，把模型预测控制的在线求解优化过程放到离线阶段，能够有效地减少计算时间，提高机器人的动态性能，降低对硬件性能的需求，可以有效地解决模型预测控制在线计算量大、实时性低的缺点。由此可见，本发明与现有技术相比，具有突出的实质性特点和显著的进步，其实施的有益效果也是显而易见的。

附图说明

图1为本发明中基于显式模型预测控制的四足机器人控制方法的流程图。

图2为本发明中在线查找流程图。

具体实施方式

为能清楚说明本方案的技术特点，下面通过具体实施方式，并结合其附图，对本方案进行阐述。

一种基于显式模型预测控制的四足机器人控制方法，它包括如下步骤：

步骤一：确定四足机器人的功能模块；

p

优选n＝9，其中，p

步骤二：建立四足机器人的优化问题；

该步骤二包括如下子步骤：

(1)基于动力学理论给出机器人状态与控制输入之间的关系；

具体采用如下方法基于动力学理论给出机器人状态与控制输入之间的关系：

基于牛顿-欧拉法对四足机器人进行动力学分析，牛顿第二定律为：

根据该式，可以得到四足机器人的加速度与足端作用力的关系；

其中：

四足机器人本体坐标系中的惯性张量I

当四足机器人运动时，世界坐标系中的惯性张量可以由下式给出：

I＝RI

其中，R为旋转矩阵，R

根据角动量定理，四足机器人转动惯量的矢量表达式为：

其中，I为四足机器人的惯性张量，ω为四足机器人的角速度，r

四足机器人的角速度ω与偏航角

欧拉角

把四足机器人的角速度ω、欧拉角

其中，0

(2)基于运动学理论给出足端力与关节力矩的关系；

具体采用如下方法基于运动学理论给出足端力与关节力矩的关系：

为了将控制输入映射到关节电机从而输出力矩，需要对四足机器人进行运动学分析，利用几何关系，可以得到本体坐标系下第i条腿足端位置与关节角度的关系：

其中，p

其中，J

其中，τ

(3)构建优化问题：

具体采用如下方法构建优化问题：

基于上述子步骤(1)的动力学方程，把重力加速度写入系数矩阵，可以将该方程整合为状态空间模型：

其中，

其中，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，

通过建立最小成本的优化问题，保证机器人状态x(k)发生变化时得到最优的足端力输入，目标函数直接反映了预测控制对系统性能要求，选取二次型作为系统目标函数，约束x，y轴方向上的接触力小于或等于最大静摩擦力，基于上述分析，构建的优化问题如下：

其中，J为优化的目标函数，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，

步骤三：以步骤二的优化问题为基础，建立控制输入的显示函数；

该步骤三包括如下子步骤：

(a)基于多参数二次规划理论，离线划分状态区域；

具体采用如下方法基于多参数二次规划理论，离线划分状态区域：

步骤二的优化问题求解为在线优化过程，需要将该问题过渡到离线阶段求解，线性系统的性质有：

其中，k代表系统k时刻，i代表系统i时刻，A

其中Y、H、F、G、W、E是只与机器人参数有关的常数矩阵，将四足机器人参数带入优化问题J，并整理为关于初始状态x(0)的形式均可得到常数矩阵Y、H、F、G、W、E的固定值，忽略D(x(0))的常数项

把V

其中，H、G、W、S均为上述中的常数矩阵，Z为决策变量，x(0)为参数矢量即机器人初始状态，λ为拉格朗日乘子，下标i代表矩阵的第i行，G

λ(-GH

当拉格朗日乘数对应于无效约束时λ＝0；当对应有效约束时λ≠0，且

G

则有：

其中，

将该式代入KKT条件的不等式组可以得到：

由该式的不等式可得到四足机器人的初始状态x(0)关于第一个临界域的不等式表达式，将Z＝u+H

由此，可以得到第一个临界域及四足机器人的状态关于输入的显示控制规律，为了求解的准确性，只取u的第一项作用于系统，对于剩余的分区，可以通过相同的步骤获得每个分区上的控制规律，当没有新的有效约束时，停止计算，即可以得到控制序列u关于状态x的分段仿射系统；

在实际应用中，将四足机器人的实际参数带入A、B矩阵，使用matlab的MPT工具箱或者c++的qpOASES库求解得到该式的显示关系，并将它们存于表中供在线阶段使用。

(b)在线查找分区，得到最优控制输入；

具体采用如下方法在线查找分区，得到最优控制输入：

通过上述离线阶段的求解，得到了四足机器人所有的状态分区以及对应的显式函数关系，那么显式模型预测控制的在线计算过程就转化为简单的查表过程：在某一时刻，通过检测四足机器人的当前状态，判断当前的状态分区，同时调用控制输入u＝(f

本发明还提供了一种控制终端，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述基于显式模型预测控制的四足机器人控制方法的步骤。

本发明中未经描述的技术特征可以通过现有技术实现，在此不再赘述。本发明并不仅限于上述具体实施方式，本领域普通技术人员在本发明的实质范围内做出的变化、改型、添加或替换，也应属于本发明的保护范围。