CN116880293B - 一种基于显式模型预测控制的四足机器人控制方法及控制终端 - Google Patents
一种基于显式模型预测控制的四足机器人控制方法及控制终端 Download PDFInfo
- Publication number
- CN116880293B CN116880293B CN202310892212.4A CN202310892212A CN116880293B CN 116880293 B CN116880293 B CN 116880293B CN 202310892212 A CN202310892212 A CN 202310892212A CN 116880293 B CN116880293 B CN 116880293B
- Authority
- CN
- China
- Prior art keywords
- robot
- state
- matrix
- control
- foot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 238000005457 optimization Methods 0.000 claims abstract description 43
- 230000006870 function Effects 0.000 claims abstract description 30
- 238000004364 calculation method Methods 0.000 claims abstract description 13
- 230000009194 climbing Effects 0.000 claims abstract description 8
- 238000001514 detection method Methods 0.000 claims abstract description 8
- 230000005021 gait Effects 0.000 claims abstract description 8
- 238000012544 monitoring process Methods 0.000 claims abstract description 8
- 238000011065 in-situ storage Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 90
- 210000002414 leg Anatomy 0.000 claims description 42
- 238000006243 chemical reaction Methods 0.000 claims description 26
- 230000008569 process Effects 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 15
- 238000005192 partition Methods 0.000 claims description 14
- 238000004458 analytical method Methods 0.000 claims description 9
- 230000017105 transposition Effects 0.000 claims description 9
- 230000000295 complement effect Effects 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 6
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 claims description 3
- 238000004590 computer program Methods 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- 210000000689 upper leg Anatomy 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims 1
- 230000007547 defect Effects 0.000 abstract description 2
- 238000005516 engineering process Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 238000007792 addition Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
- G05B19/042—Programme control other than numerical control, i.e. in sequence controllers or logic controllers using digital processors
- G05B19/0428—Safety, monitoring
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/20—Pc systems
- G05B2219/24—Pc safety
- G05B2219/24024—Safety, surveillance
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明提供了一种基于显式模型预测控制的四足机器人控制方法及控制终端，属于四足机器人智能控制技术领域，所述基于显式模型预测控制的四足机器人控制方法包括如下步骤：步骤一：确定四足机器人的功能模块；p1......pn表示四足机器人的功能模块，所述四足机器人的基本功能包括行走、奔跑、原地踏步、步态切换、跨越障碍，攀爬楼梯，可选功能包括气体检测、自主导航，远程监控；步骤二：建立四足机器人的优化问题；步骤三：以步骤二的优化问题为基础，建立控制输入的显示函数。本发明交叉融合了运动学理论和最优化理论，采用显式模型预测控制的控制方法，可以有效地解决模型预测控制在线计算量大、实时性低的缺点。
Description
技术领域
本发明涉及一种基于显式模型预测控制的四足机器人控制方法及控制终端，属于四足机器人智能控制技术领域。
背景技术
随着自动化理论、传感器技术、人工智能等技术的发展，推动着移动式机器人技术的不断进步，日益完善的性能使其在工业、农业、国防、救援等方面取得优异的成绩。移动机器人分为：轮式机器人、足式机器人、履带式机器人，其中足式机器人环境适应能力最强。足式机器人通常可以分为：单足、两足、四足、和多足，其中四足机器人相比于多足机器人结构简单，易于控制；相比于单足和双足，具有更好的稳定性和负载能力。对于四足机器人的研究重点是其对不同地形的适应能力和动态性能，其中，控制方法的优劣是其性能的关键所在。
目前，模型预测控制是四足机器人常用的控制方法之一，该方法首先建立单刚体动力学模型，通过动力学模型推导出预测方程，再建立一个优化问题和约束条件，通过在线求解有限时间段的最优化问题，找到一个最优的足端反作用力输入，在下一时刻重新求解优化过程，使得机器人状态有效跟随预定值。MPC需要反复的在线优化，因此，对于四足机器人这种多自由度的系统，该方法在线计算消耗的时间长，实时性差，对控制芯片要求过高，在实际应用中，若要实现在线求解优化问题，必须使用简化的模型以降低运算量，并且采用高性能的板卡提高运算效率。
发明内容
本发明要解决的技术问题，就是针对现有技术所存在的不足，而提供一种基于显式模型预测控制的四足机器人控制方法及控制终端，本发明交叉融合了运动学理论和最优化理论，采用显式模型预测控制的控制方法，可以有效地解决模型预测控制在线计算量大、实时性低的缺点。
一种基于显式模型预测控制的四足机器人控制方法，它包括如下步骤：
步骤一：确定四足机器人的功能模块；
p1,......pn表示四足机器人的功能模块，所述四足机器人的基本功能包括行走、奔跑、原地踏步、步态切换、跨越障碍，攀爬楼梯，可选功能包括气体检测、自主导航，远程监控；
步骤二：建立四足机器人的优化问题；
步骤三：以步骤二的优化问题为基础，建立控制输入的显示函数。
优选的，所述步骤二包括如下子步骤：
(1)基于动力学理论给出机器人状态与控制输入之间的关系；
(2)基于运动学理论给出足端力与关节力矩的关系；
(3)构建优化问题。
优选的，所述步骤三包括如下子步骤：
(a)基于多参数二次规划理论，离线划分状态区域；
(b)在线查找分区，得到最优控制输入。
优选的，所述步骤一中的n＝9，其中，p1表示四足机器人的行走，p2表示四足机器人的奔跑，p3表示四足机器人的原地踏步，p4表示四足机器人不同步态的切换，p5表示跨越障碍，p6表示攀爬楼梯，p7表示气体检测，p8表示自主导航，p9表示远程监控。
优选的，所述步骤二的子步骤(1)采用如下方法基于动力学理论给出机器人状态与控制输入之间的关系：
基于牛顿-欧拉法对四足机器人进行动力学分析，牛顿第二定律为：
根据该式，可以得到四足机器人的加速度与足端作用力的关系；
其中：为加速度，可以从惯性测量单元中得到；fi为第i条腿的足端所受地面反作用力；m为四足机器人的质量；g为重力加速度；
四足机器人本体坐标系中的惯性张量IB为常量，可以由四足机器人的质量m、长度w、宽度l、高度h得出：
当四足机器人运动时，世界坐标系中的惯性张量可以由下式给出：
I＝RIBRT
其中，R为旋转矩阵，RT为旋转矩阵的转置，四足机器人在稳定运动过程中，俯仰角和横滚角很小，可以把旋转矩阵表示为：其中，/>表示绕z轴旋转/>角度，旋转角度可以由关节电机上的编码器获得；
根据角动量定理，四足机器人转动惯量的矢量表达式为：
其中，I为四足机器人的惯性张量，ω为四足机器人的角速度，ri为质心指向第i条腿足底的向量，fi为第i条腿的足端所受地面反作用力，根据该式可以得到角速度与足端作用力的关系；
四足机器人的角速度ω与偏航角俯仰角θ、横滚角δ有关，本体坐标系转换到世界坐标系的角速度ω表示为：
欧拉角由四足机器人的偏航角/>俯仰角θ、横滚角δ组成，/>该式中θ≈0，δ≈0，便可以得到四足机器人欧拉角/>与角速度ω的关系：/>
把四足机器人的角速度ω、欧拉角质心坐标p、速度/>作为系统的状态变量，足端所受地面反作用力fi作为系统输入，将上述公式整合成矩阵的形式，可以得到系统的动力学方程为：
其中，03为3×3的零矩阵；I3为三阶的单位矩阵；g为重力加速度；m为四足机器人质量；表示绕z轴旋转/>角度；I-1为四足机器人惯性张量的逆矩阵；ri为质心指向第i条腿足底的向量，其计算方法为：由状态估计器得到质心位置，再减去步骤二的子步骤(2)得到的第i条腿足端位置pi；通过该方程可以按照期望的状态给定控制输入的大小。
优选的，所述步骤二的子步骤(2)采用如下方法基于运动学理论给出足端力与关节力矩的关系：
为了将控制输入映射到关节电机从而输出力矩，需要对四足机器人进行运动学分析，利用几何关系，可以得到本体坐标系下第i条腿足端位置与关节角度的关系：
其中，px、py、pz分别代表足端位置在本体坐标系中x、y、z轴的位置；s代表sin，c代表cos，ln代表腿的第n节的长度；hx、hy分别代表质心在x、y轴的坐标；αi1、αi2、αi3分别表示第i条腿依次绕机身关节、大腿关节、小腿关节转动的角度，其转动角度可以从编码器中获得，对该式求偏导可以得到足端速度与关节角速度的关系，该式可以整理为如下形式：
其中，Ji表示四足机器人第i条腿的单腿雅可比矩阵，为求导的的第一部分，根据力学原理可以得到关节转矩为：
其中，τi为第i条腿的关节力矩，表示四足机器人第i条腿的单腿雅可比矩阵的转置，/>为第i条腿的本体坐标系转换到世界坐标系的旋转矩阵的转置，fi为第i条腿的足端所受地面反作用力，可由步骤二的子步骤(1)中系统的动力学方程计算得出，通过该式可以把足端所受的地面反作用力转化为三个关节的力矩，即关节电机的输出力矩，并驱动四足机器人运动。
优选的，所述步骤二的子步骤(3)采用如下方法构建优化问题：
基于上述子步骤(1)的动力学方程，把重力加速度写入系数矩阵，可以将该方程整合为状态空间模型：
其中，为上述子步骤(1)的动力学方程的系数矩阵，01×3为一行三列的零矩阵，03×1为三行一列的零矩阵，/>代表机器人t时刻的状态，u(t)代表系统t时刻的控制输入u＝(f1，...，f4)即足端受到的地面反作用力，由于机器人的状态是随着时间变化的连续量，需要将上式的状态方程离散化为数值求解的形式，得到离散时间单刚体运动方程为：
其中，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，为离散后的系数矩阵，T为离散时间步长，τ为积分时间常数，e为自然对数底数，由该式可知，系统可通过当前状态和控制输入，预测系统未来状态序列和控制输入序列；
通过建立最小成本的优化问题，保证机器人状态x(k)发生变化时得到最优的足端力输入，目标函数直接反映了预测控制对系统性能要求，选取二次型作为系统目标函数，约束x，y轴方向上的接触力小于或等于最大静摩擦力，基于上述分析，构建的优化问题如下：
其中，J为优化的目标函数，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，为离散后的系数矩阵，μ为滑动摩擦系数，fx、fy分别为摩擦力在x、y轴上的分量，xref为期望状态，由遥控手柄或者电脑给出，Q和R为权值矩阵，QT和RT为权值矩阵的转置，Q＝QT≥0、R＝RT≥0，Q值越大、误差越小，R值越大、控制输入u就越小。
优选的，所述步骤三的子步骤(a)采用如下方法基于多参数二次规划理论，离线划分状态区域：
步骤二的优化问题求解为在线优化过程，需要将该问题过渡到离线阶段求解，线性系统的性质有：
其中，k代表系统k时刻，i代表系统i时刻，Ak代表系数矩阵A的K次方，Ai代表系数矩阵A的i次方，该式表明，四足机器人任意时刻的状态x(k+t)都可以由初始状态x(0)和控制输入u来表示，初始状态x(0)通过状态估计器获取，将该式带入优化问题，把优化问题转换为关于初始状态的形式：
其中Y、H、F、G、W、E是只与机器人参数有关的常数矩阵，将四足机器人参数带入优化问题J，并整理为关于初始状态x(0)的形式均可得到常数矩阵Y、H、F、G、W、E的固定值，忽略D(x(0))的常数项并令Z＝u+H-1FTx(0)，xT(0)代表初始状态的转置，H-1代表常数矩阵H的逆矩阵，FT代表常数矩阵F的转置，可以把上式中关于初始状态的形式转化为如下形式：
把Vz(x)作为目标函数，Z作为决策变量，x(0)作为参数矢量，则该式为标准的多参数二次规划问题，其中H、G、S、W均为常数矩阵，ZT为决策变量Z的转置，利用KKT条件可以求解该问题，其对应的KKT条件为：
其中，H、G、W、S均为上述中的常数矩阵，Z为决策变量，x(0)为参数矢量即机器人初始状态，λ为拉格朗日乘子，下标i代表矩阵的第i行，GT为常数矩阵G的转置，由该式的前两个等式约束可以得到互补松弛条件：
λ(-GH-1GTλ-W-Sx(0))＝0
当拉格朗日乘数对应于无效约束时λ＝0；当对应有效约束时λ≠0，且
GiZ-Wi-Six(0)＝0
则有：
其中，为常数矩阵G转置的第i行；
将该式代入KKT条件的不等式组可以得到：
由该式的不等式可得到四足机器人的初始状态x(0)关于第一个临界域的不等式表达式，将Z＝u+H-1FTx(0)代入互补松弛条件，可以得到控制输入u关于状态x的显式表达式
由此，可以得到第一个临界域及四足机器人的状态关于输入的显示控制规律，为了求解的准确性，只取u的第一项作用于系统，对于剩余的分区，可以通过相同的步骤获得每个分区上的控制规律，当没有新的有效约束时，停止计算，即可以得到控制序列u关于状态x的分段仿射系统；
在实际应用中，将四足机器人的实际参数带入A、B矩阵，使用matlab的MPT工具箱或者c++的qpOASES库求解得到该式的显示关系，并将它们存于表中供在线阶段使用。
优选的，所述步骤三的子步骤(b)采用如下方法在线查找分区，得到最优控制输入：
通过上述离线阶段的求解，得到了四足机器人所有的状态分区以及对应的显式函数关系，那么显式模型预测控制的在线计算过程就转化为简单的查表过程：在某一时刻，通过检测四足机器人的当前状态，判断当前的状态分区，同时调用控制输入u＝(f1，...，f4)的函数，并求得足端所受地面反作用力，将足端所受地面反作用力带入关节转矩，驱动机器人运动，之后更新机器人的状态；在下一时刻，再次检测四足机器人的状态，获得的新状态作为四足机器人的初始状态，重复上述的查表过程，就可以实现四足机器人的显式模型预测控制。
本发明还提供了一种控制终端，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述基于显式模型预测控制的四足机器人控制方法的步骤。
本发明的有益效果：本发明通过对四足机器人控制系统的研究，基于动力学、运动学、多参数二次规划理论等，提出了一种新的基于显式模型预测控制的四足机器人控制方法及控制终端，该控制方法主要包括三个阶段，第一阶段是确定四足机器人的功能模块(包括基本功能：行走、奔跑、踏步、步态切换、跨越障碍、攀爬楼梯，可选功能：气体检测、自主导航，远程监控等)；第二阶段是建立四足机器人的优化问题，在该阶段使用动力学和运动学理论，建立四足机器人控制系统的预测方程和约束条件；第三阶段是以第二阶段的优化问题为基础，构建出多参数二次规划问题，通过离线划分四足机器人状态区域，建立最优控制量(四足机器人的足端反作用力)和初始状态(四足机器人的位置、线速度、角速度、欧拉角)之间显示关系，在线查找状态分区，得到最优控制量。本发明交叉融合了运动学理论和最优化理论，采用显式模型预测控制的控制方法，使用显式模型预测控制控制器的四足机器人，把模型预测控制的在线求解优化过程放到离线阶段，能够有效地减少计算时间，提高机器人的动态性能，降低对硬件性能的需求，可以有效地解决模型预测控制在线计算量大、实时性低的缺点。由此可见，本发明与现有技术相比，具有突出的实质性特点和显著的进步，其实施的有益效果也是显而易见的。
附图说明
图1为本发明中基于显式模型预测控制的四足机器人控制方法的流程图。
图2为本发明中在线查找流程图。
具体实施方式
为能清楚说明本方案的技术特点，下面通过具体实施方式，并结合其附图，对本方案进行阐述。
一种基于显式模型预测控制的四足机器人控制方法，它包括如下步骤：
步骤一：确定四足机器人的功能模块；
p1,......pn表示四足机器人的功能模块，所述四足机器人的基本功能包括行走、奔跑、原地踏步、步态切换、跨越障碍，攀爬楼梯，可选功能包括气体检测、自主导航，远程监控；
优选n＝9，其中，p1表示四足机器人的行走，p2表示四足机器人的奔跑，p3表示四足机器人的原地踏步，p4表示四足机器人不同步态的切换，p5表示跨越障碍，p6表示攀爬楼梯，p7表示气体检测，p8表示自主导航，p9表示远程监控。
步骤二：建立四足机器人的优化问题；
该步骤二包括如下子步骤：
(1)基于动力学理论给出机器人状态与控制输入之间的关系；
具体采用如下方法基于动力学理论给出机器人状态与控制输入之间的关系：
基于牛顿-欧拉法对四足机器人进行动力学分析，牛顿第二定律为：
根据该式，可以得到四足机器人的加速度与足端作用力的关系；
其中：为加速度，可以从惯性测量单元中得到；fi为第i条腿的足端所受地面反作用力；m为四足机器人的质量；g为重力加速度；
四足机器人本体坐标系中的惯性张量IB为常量，可以由四足机器人的质量m、长度w、宽度l、高度h得出：
当四足机器人运动时，世界坐标系中的惯性张量可以由下式给出：
I＝RIBRT
其中，R为旋转矩阵，RT为旋转矩阵的转置，四足机器人在稳定运动过程中，俯仰角和横滚角很小，可以把旋转矩阵表示为：其中，/>表示绕z轴旋转/>角度，旋转角度可以由关节电机上的编码器获得；
根据角动量定理，四足机器人转动惯量的矢量表达式为：
其中，I为四足机器人的惯性张量，ω为四足机器人的角速度，ri为质心指向第i条腿足底的向量，fi为第i条腿的足端所受地面反作用力，根据该式可以得到角速度与足端作用力的关系；
四足机器人的角速度ω与偏航角俯仰角θ、横滚角δ有关，本体坐标系转换到世界坐标系的角速度ω表示为：
欧拉角由四足机器人的偏航角/>俯仰角θ、横滚角δ组成，/>该式中θ≈0，δ≈0，便可以得到四足机器人欧拉角/>与角速度ω的关系：/>
把四足机器人的角速度ω、欧拉角质心坐标p、速度/>作为系统的状态变量，足端所受地面反作用力fi作为系统输入，将上述公式整合成矩阵的形式，可以得到系统的动力学方程为：
其中，03为3×3的零矩阵；I3为三阶的单位矩阵；g为重力加速度；m为四足机器人质量；表示绕z轴旋转/>角度；I-1为四足机器人惯性张量的逆矩阵；ri为质心指向第i条腿足底的向量，其计算方法为：由状态估计器得到质心位置，再减去步骤二的子步骤(2)得到的第i条腿足端位置pi；通过该方程可以按照期望的状态给定控制输入的大小。
(2)基于运动学理论给出足端力与关节力矩的关系；
具体采用如下方法基于运动学理论给出足端力与关节力矩的关系：
为了将控制输入映射到关节电机从而输出力矩，需要对四足机器人进行运动学分析，利用几何关系，可以得到本体坐标系下第i条腿足端位置与关节角度的关系：
其中，px、py、pz分别代表足端位置在本体坐标系中x、y、z轴的位置；s代表sin，c代表cos，ln代表腿的第n节的长度；hx、hy分别代表质心在x、y轴的坐标；αi1、αi2、αi3分别表示第i条腿依次绕机身关节、大腿关节、小腿关节转动的角度，其转动角度可以从编码器中获得，对该式求偏导可以得到足端速度与关节角速度的关系，该式可以整理为如下形式：
其中，Ji表示四足机器人第i条腿的单腿雅可比矩阵，为求导的的第一部分，根据力学原理可以得到关节转矩为：
其中，τi为第i条腿的关节力矩，表示四足机器人第i条腿的单腿雅可比矩阵的转置，/>为第i条腿的本体坐标系转换到世界坐标系的旋转矩阵的转置，fi为第i条腿的足端所受地面反作用力，可由步骤二的子步骤(1)中系统的动力学方程计算得出，通过该式可以把足端所受的地面反作用力转化为三个关节的力矩，即关节电机的输出力矩，并驱动四足机器人运动。
(3)构建优化问题：
具体采用如下方法构建优化问题：
基于上述子步骤(1)的动力学方程，把重力加速度写入系数矩阵，可以将该方程整合为状态空间模型：
其中，为上述子步骤(1)的动力学方程的系数矩阵，01×3为一行三列的零矩阵，03×1为三行一列的零矩阵，/>代表机器人t时刻的状态，u(t)代表系统t时刻的控制输入u＝(f1，...，f4)即足端受到的地面反作用力，由于机器人的状态是随着时间变化的连续量，需要将上式的状态方程离散化为数值求解的形式，得到离散时间单刚体运动方程为：
其中，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，为离散后的系数矩阵，T为离散时间步长，τ为积分时间常数，e为自然对数底数，由该式可知，系统可通过当前状态和控制输入，预测系统未来状态序列和控制输入序列；
通过建立最小成本的优化问题，保证机器人状态x(k)发生变化时得到最优的足端力输入，目标函数直接反映了预测控制对系统性能要求，选取二次型作为系统目标函数，约束x，y轴方向上的接触力小于或等于最大静摩擦力，基于上述分析，构建的优化问题如下：
其中，J为优化的目标函数，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，为离散后的系数矩阵，μ为滑动摩擦系数，fx、fy分别为摩擦力在x、y轴上的分量，xref为期望状态，由遥控手柄或者电脑给出，Q和R为权值矩阵，QT和RT为权值矩阵的转置，Q＝QT≥0、R＝RT≥0，Q值越大、误差越小，R值越大、控制输入u就越小。在实际应用时可以根据需求，在保证系统稳定的前提下调节权值矩阵的大小，但为了避免过大扭矩对电机关节和机械结构造成伤害，需要控制输入量做到最小化。
步骤三：以步骤二的优化问题为基础，建立控制输入的显示函数；
该步骤三包括如下子步骤：
(a)基于多参数二次规划理论，离线划分状态区域；
具体采用如下方法基于多参数二次规划理论，离线划分状态区域：
步骤二的优化问题求解为在线优化过程，需要将该问题过渡到离线阶段求解，线性系统的性质有：
其中，k代表系统k时刻，i代表系统i时刻，Ak代表系数矩阵A的K次方，Ai代表系数矩阵A的i次方，该式表明，四足机器人任意时刻的状态x(k+t)都可以由初始状态x(0)和控制输入u来表示，初始状态x(0)通过状态估计器获取，将该式带入优化问题，把优化问题转换为关于初始状态的形式：
其中Y、H、F、G、W、E是只与机器人参数有关的常数矩阵，将四足机器人参数带入优化问题J，并整理为关于初始状态x(0)的形式均可得到常数矩阵Y、H、F、G、W、E的固定值，忽略D(x(0))的常数项并令Z＝u+H-1FTx(0)，xT(0)代表初始状态的转置，H-1代表常数矩阵H的逆矩阵，FT代表常数矩阵F的转置，可以把上式中关于初始状态的形式转化为如下形式：
把Vz(x)作为目标函数，Z作为决策变量，x(0)作为参数矢量，则该式为标准的多参数二次规划问题，其中H、G、S、W均为常数矩阵，ZT为决策变量Z的转置，利用KKT条件可以求解该问题，其对应的KKT条件为：
其中，H、G、W、S均为上述中的常数矩阵，Z为决策变量，x(0)为参数矢量即机器人初始状态，λ为拉格朗日乘子，下标i代表矩阵的第i行，GT为常数矩阵G的转置，由该式的前两个等式约束可以得到互补松弛条件：
λ(-GH-1GTλ-W-Sx(0))＝0
当拉格朗日乘数对应于无效约束时λ＝0；当对应有效约束时λ≠0，且
GiZ-Wi-Six(0)＝0
则有：
其中，为常数矩阵G转置的第i行；
将该式代入KKT条件的不等式组可以得到：
由该式的不等式可得到四足机器人的初始状态x(0)关于第一个临界域的不等式表达式，将Z＝u+H-1FTx(0)代入互补松弛条件，可以得到控制输入u关于状态x的显式表达式
由此，可以得到第一个临界域及四足机器人的状态关于输入的显示控制规律，为了求解的准确性，只取u的第一项作用于系统，对于剩余的分区，可以通过相同的步骤获得每个分区上的控制规律，当没有新的有效约束时，停止计算，即可以得到控制序列u关于状态x的分段仿射系统；
在实际应用中，将四足机器人的实际参数带入A、B矩阵，使用matlab的MPT工具箱或者c++的qpOASES库求解得到该式的显示关系，并将它们存于表中供在线阶段使用。
(b)在线查找分区，得到最优控制输入；
具体采用如下方法在线查找分区，得到最优控制输入：
通过上述离线阶段的求解，得到了四足机器人所有的状态分区以及对应的显式函数关系，那么显式模型预测控制的在线计算过程就转化为简单的查表过程：在某一时刻，通过检测四足机器人的当前状态，判断当前的状态分区，同时调用控制输入u＝(f1,…,f4)的函数，并求得足端所受地面反作用力，将足端所受地面反作用力带入关节转矩，驱动机器人运动，之后更新机器人的状态；在下一时刻，再次检测四足机器人的状态，获得的新状态作为四足机器人的初始状态，重复上述的查表过程，就可以实现四足机器人的显式模型预测控制。
本发明还提供了一种控制终端，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述基于显式模型预测控制的四足机器人控制方法的步骤。
本发明中未经描述的技术特征可以通过现有技术实现，在此不再赘述。本发明并不仅限于上述具体实施方式，本领域普通技术人员在本发明的实质范围内做出的变化、改型、添加或替换，也应属于本发明的保护范围。
Claims (8)
1.一种基于显式模型预测控制的四足机器人控制方法，其特征是，它包括如下步骤：
步骤一：确定四足机器人的功能模块；
p1,......pn表示四足机器人的功能模块，所述四足机器人的基本功能包括行走、奔跑、原地踏步、步态切换、跨越障碍，攀爬楼梯，可选功能包括气体检测、自主导航，远程监控；
步骤二：建立四足机器人的优化问题；
步骤三：以步骤二的优化问题为基础，建立控制输入的显式函数；
所述步骤三包括如下子步骤：
(a)基于多参数二次规划理论，离线划分状态区域；
(b)在线查找分区，得到最优控制输入；
所述步骤三的子步骤(a)采用如下方法基于多参数二次规划理论，离线划分状态区域：
步骤二的优化问题求解为在线优化过程，需要将该问题过渡到离线阶段求解，线性系统的性质有：
其中，k代表系统k时刻，i代表系统i时刻，Ak代表系数矩阵A的K次方，Ai代表系数矩阵A的i次方，该式表明，四足机器人任意时刻的状态x(k+t)都由初始状态x(0)和控制输入u来表示，初始状态x(0)通过状态估计器获取，将该式带入优化问题，把优化问题转换为关于初始状态的形式：
其中Y、H、F、G、W、E是只与机器人参数有关的常数矩阵，将四足机器人参数带入优化问题J，并整理为关于初始状态x(0)的形式均可得到常数矩阵Y、H、F、G、W、E的固定值，忽略D(x(0))的常数项并令Z＝u+H-1FTx(0)，xT(0)代表初始状态的转置，H-1代表常数矩阵H的逆矩阵，FT代表常数矩阵F的转置，把上式中关于初始状态的形式转化为如下形式：
把Vz(x)作为目标函数，Z作为决策变量，x(0)作为参数矢量，则该式为标准的多参数二次规划问题，其中H、G、S、W均为常数矩阵，ZT为决策变量Z的转置，利用KKT条件求解该问题，其对应的KKT条件为：
其中，H、G、W、S均为上述中的常数矩阵，Z为决策变量，x(0)为参数矢量即机器人初始状态，λ为拉格朗日乘子，下标i代表矩阵的第i行，GT为常数矩阵G的转置，由该式的前两个等式约束得到互补松弛条件：
λ(-GH-1GTλ-W-Sx(0))＝0
当拉格朗日乘数对应于无效约束时λ＝0；当对应有效约束时λ≠0，且
GiZ-Wi-Six(0)＝0
则有：
其中，为常数矩阵G转置的第i行；
将该式代入KKT条件的不等式组得到：
由该式的不等式可得到四足机器人的初始状态x(0)关于第一个临界域的不等式表达式，将Z＝u+H-1FTx(0)代入互补松弛条件，得到控制输入u关于状态x的显式表达式
由此，得到第一个临界域及四足机器人的状态关于输入的显式控制规律，为了求解的准确性,只取u的第一项作用于系统，对于剩余的分区，通过相同的步骤获得每个分区上的控制规律，当没有新的有效约束时，停止计算，即可得到控制序列u关于状态x的分段仿射系统；
将四足机器人的实际参数带入A、B矩阵，使用matlab的MPT工具箱或者c++的qpOASES库求解得到该式的显式关系，并将它们存于表中供在线阶段使用。
2.根据权利要求1所述的基于显式模型预测控制的四足机器人控制方法，其特征是，所述步骤二包括如下子步骤：
(1)基于动力学理论给出机器人状态与控制输入之间的关系；
(2)基于运动学理论给出足端力与关节力矩的关系；
(3)构建优化问题。
3.根据权利要求1-2任一项所述的基于显式模型预测控制的四足机器人控制方法，其特征是，所述步骤一中的n＝9，其中，p1表示四足机器人的行走，p2表示四足机器人的奔跑，p3表示四足机器人的原地踏步，p4表示四足机器人不同步态的切换，p5表示跨越障碍，p6表示攀爬楼梯，p7表示气体检测，p8表示自主导航，p9表示远程监控。
4.根据权利要求3所述的基于显式模型预测控制的四足机器人控制方法，其特征是，所述步骤二的子步骤(1)采用如下方法基于动力学理论给出机器人状态与控制输入之间的关系：
基于牛顿-欧拉法对四足机器人进行动力学分析，牛顿第二定律为：
根据该式，得到四足机器人的加速度与足端作用力的关系；
其中：为加速度，从惯性测量单元中得到；fi为第i条腿的足端所受地面反作用力；m为四足机器人的质量；g为重力加速度；
四足机器人本体坐标系中的惯性张量IB为常量，由四足机器人的质量m、长度w、宽度l、高度h得出：
当四足机器人运动时，世界坐标系中的惯性张量由下式给出：
I＝RIBRT
其中，R为旋转矩阵，RT为旋转矩阵的转置，四足机器人在稳定运动过程中，俯仰角和横滚角很小，把旋转矩阵表示为：其中，/>表示绕z轴旋转/>角度，旋转角度由关节电机上的编码器获得；
根据角动量定理，四足机器人转动惯量的矢量表达式为：
其中，I为四足机器人的惯性张量，ω为四足机器人的角速度，ri为质心指向第i条腿足底的向量，fi为第i条腿的足端所受地面反作用力，根据该式得到角速度与足端作用力的关系；
四足机器人的角速度ω与偏航角俯仰角θ、横滚角δ有关，本体坐标系转换到世界坐标系的角速度ω表示为:
欧拉角由四足机器人的偏航角/>俯仰角θ、横滚角δ组成，/>该式中θ≈0，δ≈0，便得到四足机器人欧拉角/>与角速度ω的关系：/>
把四足机器人的角速度ω、欧拉角质心坐标p、速度/>作为系统的状态变量，足端所受地面反作用力fi作为系统输入，将上述公式整合成矩阵的形式，得到系统的动力学方程为：
其中，03为3×3的零矩阵；I3为三阶的单位矩阵；g为重力加速度；m为四足机器人质量；表示绕z轴旋转/>角度；I-1为四足机器人惯性张量的逆矩阵；ri为质心指向第i条腿足底的向量，其计算方法为：由状态估计器得到质心位置，再减去步骤二的子步骤(2)得到的第i条腿足端位置pi；通过该方程按照期望的状态给定控制输入的大小。
5.根据权利要求4所述的基于显式模型预测控制的四足机器人控制方法，其特征是，所述步骤二的子步骤(2)采用如下方法基于运动学理论给出足端力与关节力矩的关系：
为了将控制输入映射到关节电机从而输出力矩，需要对四足机器人进行运动学分析，利用几何关系，得到本体坐标系下第i条腿足端位置与关节角度的关系：
其中，px、py、pz分别代表足端位置在本体坐标系中x、y、z轴的位置；s代表sin，c代表cos，ln代表腿的第n节的长度；hx、hy分别代表质心在x、y轴的坐标；αi1、αi2、αi3分别表示第i条腿依次绕机身关节、大腿关节、小腿关节转动的角度，其转动角度从编码器中获得，对该式求偏导得到足端速度与关节角速度的关系，该式整理为如下形式：
其中，Ji表示四足机器人第i条腿的单腿雅可比矩阵，为求导的的第一部分，根据力学原理得到关节转矩为：
其中，τi为第i条腿的关节力矩，表示四足机器人第i条腿的单腿雅可比矩阵的转置，为第i条腿的本体坐标系转换到世界坐标系的旋转矩阵的转置，fi为第i条腿的足端所受地面反作用力，可由步骤二的子步骤(1)中系统的动力学方程计算得出，通过该式把足端所受的地面反作用力转化为三个关节的力矩，即关节电机的输出力矩，并驱动四足机器人运动。
6.根据权利要求5所述的基于显式模型预测控制的四足机器人控制方法，其特征是，所述步骤二的子步骤(3)采用如下方法构建优化问题：
基于上述子步骤(1)的动力学方程，把重力加速度写入系数矩阵，将该方程整合为状态空间模型：
其中，为上述子步骤(1)的动力学方程的系数矩阵，01×3为一行三列的零矩阵，03×1为三行一列的零矩阵，/>代表机器人t时刻的状态，u(t)代表系统t时刻的控制输入u＝(f1，...，f4)即足端受到的地面反作用力，由于机器人的状态是随着时间变化的连续量，需要将上式的状态方程离散化为数值求解的形式，得到离散时间单刚体运动方程为：
其中，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，为离散后的系数矩阵，T为离散时间步长，τ为积分时间常数，e为自然对数底数，由该式可知，系统可通过当前状态和控制输入，预测系统未来状态序列和控制输入序列；
通过建立最小成本的优化问题，保证机器人状态x(k)发生变化时得到最优的足端力输入，目标函数直接反映了预测控制对系统性能要求，选取二次型作为系统目标函数，约束x，y轴方向上的接触力小于或等于最大静摩擦力,基于上述分析，构建的优化问题如下：
其中，J为优化的目标函数，x(k)代表机器人k时刻的状态，x(k+1)代表机器人k+1时刻的状态，u(k)代表系统k时刻的控制输入，为离散后的系数矩阵，μ为滑动摩擦系数，fx、fy分别为摩擦力在x、y轴上的分量，xref为期望状态，由遥控手柄或者电脑给出，Q和R为权值矩阵，QT和RT为权值矩阵的转置，Q＝QT≥0、R＝RT≥0，Q值越大、误差越小，R值越大、控制输入u就越小。
7.根据权利要求6所述的基于显式模型预测控制的四足机器人控制方法，其特征是，所述步骤三的子步骤(b)采用如下方法在线查找分区，得到最优控制输入：
通过上述离线阶段的求解，得到了四足机器人所有的状态分区以及对应的显式函数关系，那么显式模型预测控制的在线计算过程就转化为简单的查表过程：在某一时刻，通过检测四足机器人的当前状态，判断当前的状态分区，同时调用控制输入u＝(f1，...，f4)的函数，并求得足端所受地面反作用力，将足端所受地面反作用力带入关节转矩，驱动机器人运动，之后更新机器人的状态；在下一时刻，再次检测四足机器人的状态，获得的新状态作为四足机器人的初始状态，重复上述的查表过程，就实现四足机器人的显式模型预测控制。
8.一种控制终端，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7任一项所述基于显式模型预测控制的四足机器人控制方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310892212.4A CN116880293B (zh) | 2023-07-20 | 2023-07-20 | 一种基于显式模型预测控制的四足机器人控制方法及控制终端 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310892212.4A CN116880293B (zh) | 2023-07-20 | 2023-07-20 | 一种基于显式模型预测控制的四足机器人控制方法及控制终端 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116880293A CN116880293A (zh) | 2023-10-13 |
| CN116880293B true CN116880293B (zh) | 2023-12-26 |
Family
ID=88256591
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310892212.4A Active CN116880293B (zh) | 2023-07-20 | 2023-07-20 | 一种基于显式模型预测控制的四足机器人控制方法及控制终端 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116880293B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2002091572A (ja) * | 2000-07-14 | 2002-03-29 | Daihen Corp | ロボットシステムの制御方法及び制御装置 |
| KR20100015121A (ko) * | 2008-08-04 | 2010-02-12 | 주식회사 스페이스솔루션 | 제어기의 성능을 시험하기 위한 시험장치 |
| CN108303882A (zh) * | 2018-01-22 | 2018-07-20 | 浙江工业大学之江学院 | 三自由度直升机的多胞体近似显式模型预测控制方法 |
| WO2020031718A1 (ja) * | 2018-08-08 | 2020-02-13 | ソニー株式会社 | 制御装置、制御方法、およびプログラム |
| CN111891252A (zh) * | 2020-08-06 | 2020-11-06 | 齐鲁工业大学 | 一种四足仿生机器人的身体姿态斜坡自适应控制方法 |
| CN114933243A (zh) * | 2022-05-25 | 2022-08-23 | 山东大学 | 基于多层感知机网络的桥式起重机安全控制方法及系统 |
| CN115098907A (zh) * | 2022-05-07 | 2022-09-23 | 济南大学 | 基于虚拟现实的变电站4d-bim可视化方法及系统 |
| CN115202259A (zh) * | 2022-07-22 | 2022-10-18 | 同济大学 | 一种四足机器人的cpg控制系统及其参数整定方法 |
| CN115761787A (zh) * | 2022-09-27 | 2023-03-07 | 西北工业大学 | 一种融合约束的手部姿态测量方法 |
| CN116237943A (zh) * | 2023-02-28 | 2023-06-09 | 杭州云深处科技有限公司 | 一种结合地形约束的四足机器人控制方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080109115A1 (en) * | 2006-11-03 | 2008-05-08 | Michael Zin Min Lim | Dynamic force controller for multilegged robot |
-
2023
- 2023-07-20 CN CN202310892212.4A patent/CN116880293B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2002091572A (ja) * | 2000-07-14 | 2002-03-29 | Daihen Corp | ロボットシステムの制御方法及び制御装置 |
| KR20100015121A (ko) * | 2008-08-04 | 2010-02-12 | 주식회사 스페이스솔루션 | 제어기의 성능을 시험하기 위한 시험장치 |
| CN108303882A (zh) * | 2018-01-22 | 2018-07-20 | 浙江工业大学之江学院 | 三自由度直升机的多胞体近似显式模型预测控制方法 |
| WO2020031718A1 (ja) * | 2018-08-08 | 2020-02-13 | ソニー株式会社 | 制御装置、制御方法、およびプログラム |
| CN111891252A (zh) * | 2020-08-06 | 2020-11-06 | 齐鲁工业大学 | 一种四足仿生机器人的身体姿态斜坡自适应控制方法 |
| CN115098907A (zh) * | 2022-05-07 | 2022-09-23 | 济南大学 | 基于虚拟现实的变电站4d-bim可视化方法及系统 |
| CN114933243A (zh) * | 2022-05-25 | 2022-08-23 | 山东大学 | 基于多层感知机网络的桥式起重机安全控制方法及系统 |
| CN115202259A (zh) * | 2022-07-22 | 2022-10-18 | 同济大学 | 一种四足机器人的cpg控制系统及其参数整定方法 |
| CN115761787A (zh) * | 2022-09-27 | 2023-03-07 | 西北工业大学 | 一种融合约束的手部姿态测量方法 |
| CN116237943A (zh) * | 2023-02-28 | 2023-06-09 | 杭州云深处科技有限公司 | 一种结合地形约束的四足机器人控制方法 |
Non-Patent Citations (1)
| Title |
|---|
| 四足机器人动力学控制的研究;尹佳新;《中国优秀硕士学位论文全文数据库信息科技辑》(第2023年02期);第4-50页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116880293A (zh) | 2023-10-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Shiriaev et al. | Can we make a robot ballerina perform a pirouette? Orbital stabilization of periodic motions of underactuated mechanical systems | |
| Halme et al. | WorkPartner: interactive human-like service robot for outdoor applications | |
| Yamamoto et al. | Survey on model-based biped motion control for humanoid robots | |
| Liu et al. | Static force analysis of foot of electrically driven heavy-duty six-legged robot under tripod gait | |
| Kolvenbach et al. | Efficient gait selection for quadrupedal robots on the moon and mars | |
| Peng et al. | Stability criteria of balanced and steppable unbalanced states for full-body systems with implications in robotic and human gait | |
| CN116880293B (zh) | 一种基于显式模型预测控制的四足机器人控制方法及控制终端 | |
| Herzog et al. | Experiments with a hierarchical inverse dynamics controller on a torque-controlled humanoid | |
| CN114986526A (zh) | 机器人运动控制方法、装置、机器人及存储介质 | |
| Ryu et al. | Preview control-based online walking pattern generation for biped robots with vertical center-of-mass motion | |
| Zhuang et al. | Power consumption characteristics research on mobile system of electrically driven large-load-ratio six-legged robot | |
| Ando et al. | Master-slave bipedal walking and semi-automatic standing up of humanoid robots | |
| Khusainov et al. | Kinematic and dynamic approaches in gait optimization for humanoid robot locomotion | |
| Ding et al. | Dynamic modeling and locomotion control for quadruped robots based on center of inertia on SE (3) | |
| Kim et al. | Assessing whole-body operational space control in a point-foot series elastic biped: Balance on split terrain and undirected walking | |
| Lee et al. | Balance Recovery Based on Whole-Body Control Using Joint Torque Feedback for Quadrupedal Robots | |
| Yoo et al. | Advantages of vertical pelvic movement in bipedal gaits for increasing stride length and reducing actuator power requirements | |
| Nguyen et al. | Hierarchical sliding mode algorithm for athlete robot walking | |
| Heyu et al. | Impedance control method with reinforcement learning for dual-arm robot installing slabstone | |
| Li et al. | External force observer aided push recovery for torque-controlled biped robots | |
| Sun et al. | Hierarchical Trajectory Optimization for Humanoid Robot Jumping Motion | |
| EP4109198A1 (en) | Method and apparatus for controlling movement of legged robot, and device, medium and program | |
| YAT | Robust Walking and Running Gaits for Biped Robots with a QP-Based Whole-Body Controller | |
| Jing | Application of variable zero-moment point in walking control of the biped robot | |
| Gor et al. | Bond graph Modeling and Control of Compliant Legged Quadruped Robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |