CN115857354A - 一种四足机器人优化足底力分配和轨迹跟踪的方法 - Google Patents
一种四足机器人优化足底力分配和轨迹跟踪的方法 Download PDFInfo
- Publication number
- CN115857354A CN115857354A CN202211592400.7A CN202211592400A CN115857354A CN 115857354 A CN115857354 A CN 115857354A CN 202211592400 A CN202211592400 A CN 202211592400A CN 115857354 A CN115857354 A CN 115857354A
- Authority
- CN
- China
- Prior art keywords
- phase
- virtual
- robot
- swing phase
- force
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 30
- 230000001133 acceleration Effects 0.000 claims abstract description 29
- 230000003044 adaptive effect Effects 0.000 claims abstract description 7
- 230000006641 stabilisation Effects 0.000 claims abstract description 7
- 238000011105 stabilization Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 31
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 230000001105 regulatory effect Effects 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 4
- 238000013507 mapping Methods 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 2
- 230000011218 segmentation Effects 0.000 abstract 1
- 230000005021 gait Effects 0.000 description 6
- 230000001276 controlling effect Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012913 prioritisation Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
Abstract
一种四足机器人优化足底力分配和轨迹跟踪的方法,(1)在四足机器人支撑相期间,将躯干虚拟力、虚拟扭矩和足端给定力之间的力分配问题转化成分层二次规划问题,对最优解进行处理,并在支撑相分段切换的瞬间实现不同任务之间优先级对调,在支撑相前半段和后半段分别着重实现机器人姿态稳定和质心轨迹跟踪;(2)在四足机器人摇摆相期间,建立摆动相虚拟弹簧刚度与摆动相各点加速度之间的数值关系,实现四足机器人摆动相的虚拟刚度加速度适应控制,结合摆动相的关节角度比例微分控制,减小摆动相轨迹跟踪误差。本发明优化了四足机器人的足底力分配和轨迹跟踪性能,提升了整个控制过程的平滑度,使四足机器人行进过程能够保持良好的动态稳定性。
Description
技术领域
本发明涉及一种用于四足机器人足底力的分配和轨迹跟踪方法,该方法基于虚拟模型控制进行优化,属于四足机器人的控制技术领域。
背景技术
四足机器人是近年来发展较快的一种腿足式机器人,与履带式或轮式机器人相比,在面对复杂环境条件时有着巨大的优势,在近几年得到了广泛的发展。四足机器人常见的步态有trot、bounding和gallop等步态,足底力分配和轨迹跟踪性能直接影响到四足机器人的稳定。
虚拟模型控制的原理是将四足机器人的每条腿虚拟为弹簧和阻尼器,通过分析机器人足端所受的虚拟力通过雅可比矩阵建立虚拟力和关节力的映射关系。在控制上只需要调整虚拟元件对应的参数就可以实现控制对象预期的运动特性。虚拟模型控制可以将四足机器人本体控制简化为对虚拟刚体高度、速度和姿态的独立控制,由于虚拟模型控制不用考虑动力学复杂的计算关系即可实现良好的力控特性,通过这种方法可以实现四足机器人的多种稳定步态。四足机器人可通过二次规划来求解最优足底力,但是无法应对存在多个任务需求的情况。在分层二次优化(HQP)中,多任务控制问题被重新定义为分层优化问题,分层目标的每个层次都被建模为二次规划,HQP能够通过向较低优先级目标添加额外的线性约束来解决等式和不等式的层次结构,使低优先级目标的解属于高优先级空间的解空间来实现目标侧重。
基于虚拟模型控制的四足机器人trot步态分为支撑相控制和摇摆相控制。对于目前的四足机器人虚拟模型控制技术,存在以下问题:
在支撑相期间仅通过传统的虚拟模型控制算法求解躯干虚拟力,使得机器人姿态角和支撑腿的地面反作用力的波动范围较大,使四足机器人丧失了一定的行走稳定性和鲁棒性;在摇摆相期间受虚拟弹簧模型的影响,轨迹跟踪误差较大,进而影响相位切换时的落足点控制,使四足机器人无法达到预期运动速度。
发明内容
本发明针对现有四足机器人行走过程中的足力分配和轨迹跟踪技术存在的不足,基于虚拟模型控制,提出一种优化的四足机器人的足底力分配和轨迹跟踪方法,使四足机器人行进过程能够保持良好的动态稳定性。
本发明的四足机器人优化足底力分配和轨迹跟踪的方法,采用以下方案:
(1)在四足机器人支撑相期间,将机器人支撑相分为支撑相前半段和支撑相后半段,应用分层二次规划(HQP),结合虚拟模型控制算法,将支撑相的姿态稳定和质心轨迹跟踪两个主要任务都转换为二次规划问题并求解,通过零空间投影得到最终优化足底力,并在支撑相分段切换瞬间,实现不同任务之间优先级对调,在支撑相前半段着重调节躯干虚拟扭矩,实现机器人着地后的机体稳定性能,在支撑相后半段着重调节躯干虚拟力,实现机器人离地前的质心轨迹跟踪。
首先给定四足机器人支撑相的质心轨迹和预期RPY角,通过虚拟模型控制算法求解躯干虚拟力和虚拟扭矩,并将躯干虚拟力、虚拟扭矩和足端给定力之间的力分配问题转化成分层二次规划(HQP)问题,对机器人机体姿态稳定和机体质心轨迹跟踪两个支撑相主要任务转换为两个QP问题,通过零空间投影法对最优解进行处理,并在支撑相分段切换的瞬间实现不同任务之间优先级对调,以此实现不同阶段调节量之间的侧重,同时防止过大的足端虚拟力;利用qpOASES对转换成的两个二次优化问题进行求解,得到最优足端虚拟力,并通过雅可比矩阵映射到各关节扭矩。
所述支撑相前半段分层二次规划(HQP)的最优解为:
所述支撑相后半段分层二次规划(HQP)的最优解为:
(2)在四足机器人摇摆相期间,对预先得到的摆动相预期轨迹进行二次求导,得到摆动相各点加速度关于时间t的表达式,建立摆动相虚拟弹簧刚度与摆动相各点加速度之间的数值关系,实现四足机器人摆动相的虚拟刚度加速度适应控制,结合摆动相的关节角度比例微分控制,减小摆动相轨迹跟踪误差。
具体是在四足机器人摇摆相期间,首先根据机体各方向预期速度,引入实际速度与目标速度之间误差的比例控制,计算足端与地面各方向零度角,并依据各方向零度角通过插值多项式得到平滑的摆动相预期轨迹,实现摆动相期间足端的平滑运动;然后求解摆动相预期轨迹的二次导函数,得到摆动相各点加速度关于时间t的表达式,将某时刻加速度与后一时刻加速度之比定义为当前时刻刚度比β,建立摆动相虚拟弹簧刚度与摆动相各点加速度之间的数值关系,实现四足机器人摆动相的虚拟刚度加速度适应控制;在此基础上,根据位置控制的思想通过逆运动学解算各关节的预期角度,并与各关节实际角度相比较,实现摆动相的关节角度比例微分控制。
所述摆动相虚拟弹簧刚度与摆动相各点加速度之间的数值关系(摆动相某一时刻的虚拟弹簧刚度关于时间t的表达式)为:
本发明通过控制算法的改进优化了四足机器人的足底力分配和轨迹跟踪性能,同时提升了整个控制过程的平滑度,使四足机器人行进过程能够保持良好的动态稳定性。具有以下特点:
1.应用启发式方法分析四足机器人支撑相功能,应用分层二次规划(HQP),将支撑相的姿态稳定和质心轨迹跟踪两个主要任务都转换为二次规划问题并求解,通过零空间投影得到最终优化足底力,并在支撑相分段切换瞬间,实现不同任务之间优先级对调,在支撑相前半段着重实现机器人姿态稳定,支撑相后半段着重实现机器人质心轨迹跟踪。
2.求解摆动相预期轨迹关于时间t的二阶分析模型,得到摆动相各点关于时间t的加速度表达式,将某时刻加速度与后一时刻加速度之比定义为当前时刻刚度比β,建立摆动相虚拟弹簧刚度与摆动相各点加速度之间的数值关系,实现四足机器人摆动相的虚拟刚度加速度适应控制,并结合关节角度比例微分控制优化摆动相的轨迹跟踪性能。
附图说明
图1是本发明四足机器人控制过程框图。
图2是四足机器人躯干的虚拟弹簧模型示意图。
图3是分层二次规划中应用零空间投影的优先级排序示意图。
图4是四足机器人摆动足的虚拟弹簧模型示意图。
具体实施方式
四足机器人整个运动阶段被分为支撑相和摆动相,这两个阶段由状态机交替切换。本发明分别对四足机器人支撑相和摇摆相进行控制,控制架构如图1所示。
在支撑相期间,为了跟踪躯干的期望运动和姿态,将躯干和支撑足看作一个虚拟弹簧模型,如图2。由于腿部质量相较于躯干质量很小,所以在控制过程中忽略腿部质量。对支撑足进行受力分析,躯干的所受虚拟力和虚拟力矩由下式可得:
其中,Fb和Tb分别是机器人躯干所受虚拟力和虚拟力矩,fF和fH分别是前支撑足和后支撑足足端受力,bPF和bPH分别是前、后支撑足在躯干坐标系下的位置。根据虚拟模型控制,躯干所受虚拟力和虚拟力矩由下式可得:
其中,Fx、Fy、Fz、Tx、Ty和Tz分别是应用虚拟模型控制后质心所受各方向虚拟力和虚拟力矩,stiffness和damping分别是所应用虚拟弹簧模型的虚拟弹簧系数和虚拟阻尼系数,xd、yd、zd、αd、βd和γd分别是机器人质心各方向期望位置和期望姿态角,x、y、z、α、β和γ分别是质心各方向实时位置和实时姿态角,和分别是质心各方向期望速度和期望姿态角速度,和分别是质心各方向实际速度和实际姿态角速度。
四足机器人切换成支撑相后需要实现机体稳定和质心轨迹跟踪。在四足机器人支撑相期间,本发明将支撑相分为支撑相前半段和支撑相后半段,在不同分段着重实现不同的性能要求;在支撑相前半段着重调节躯干虚拟扭矩,实现机器人着地后的机体稳定性能;在支撑相后半段着重调节躯干虚拟力,实现机器人离地前的质心轨迹跟踪。
为了将躯干所受虚拟力和虚拟力矩求解足底力分别转换成两个二次规划(QP)问题,实现分层二次规划(HQP),结合上式可得:
其中,[Fx Fy Fz]T和[Tx Ty Tz]T分别是QPs问题中的B1、B2阵,和分别是QPs问题中的A1和A2阵,[fFx fFy fFz fHxfHy fHz]T是提出的分层二次规划问题所求解x。
为了简化重力的影响,假设机器人躯干的横滚角和俯仰角都很小,则在垂直方向虚拟力上引入重力项。
经上式定义,本发明将根据期望躯干虚拟力和虚拟力矩求解最优足底力转化成HQP问题,HQP使用零空间投影对最终优化解进行处理,使最终解更侧重于完成其中一项任务,来实现支撑相不同阶段躯干虚拟力和虚拟力矩的优先级控制。可得:
其中,xi是所有QPs的相同决策变量向量,代表前后支撑足各方向受力。下标i表示任务的优先级。Q∈R3×3是权重对角矩阵,用来控制足底力对躯干虚拟力和虚拟力矩的影响大小。W∈R6×6是惩罚对角矩阵,可以防止过大的足底力。为了防止脚步滑动,增加了摩擦锥约束,其中μ是摩擦系数,同时添加垂直力大于O的约束。
本发明在四足机器人支撑相由于仅有两个任务优先级,并且在支撑相前半段和后半段切换瞬间,对姿态稳定和跟踪质心轨迹两个任务进行优先级对调。所以在支撑相前半段,为了减小机器人机体姿态的变化,使机器人在着地后机体姿态迅速达到期望状态,故实现机器人姿态稳定性能具有更高的优先级,则支撑相前半段HQP的最优解为:
在支撑相后半段,为了减小机器人质心轨迹的跟踪误差,使机器人在离地前到达期望位置并具备离地的加速度,故实现机器人质心轨迹跟踪具有更高的优先级,则支撑相后半段的最优解为:
其中是关于轨迹跟踪QP问题的解,是关于姿态稳定QP问题的解,表示矩阵Ai的零空间投影矩阵。本发明使用矩阵的伪逆来构成零空间投影矩阵,即Ni=Ii-其中Ii是单位矩阵,是矩阵Ai的伪逆。应用零空间投影对解进行处理的过程如图3。根据τ=-JTx*,将最终求得的最优支撑足力通过雅可比矩阵可求得支撑足各关节的最佳输入扭矩。
四足机器人摇摆相的主要任务是通过控制虚拟弹簧落地时与地面的角度来控制机体的速度,所以对于摇摆相来说足端轨迹的精确跟踪显得尤为重要。在四足机器人摇摆相期间,由于虚拟弹簧的滞后特性,跟踪足端轨迹的效果较差。
在摇摆相期间,将摆动足看作一个虚拟模型,如图4。首先通过落足点控制来控制机器人各方向速度,同时引入实际速度与目标速度之间误差的比例控制,得到落足点的实际坐标。为了得到摆动相的平滑轨迹曲线,对摆动相足端轨迹施加状态约束,可得摆动相各方向平滑的多项式插值足端轨迹。
根据虚拟模型控制,摆动腿的虚拟力由下可得:
其中,Fx、Fy和Fz分别是应用虚拟模型控制后足端所受各方向虚拟力stiffness、damping分别是所应用虚拟弹簧模型的虚拟弹簧系数和虚拟阻尼系数,xd、yd和zd是分别是足端各方向期望位置,x、y和z分别是足端各方向实时位置,和分别是足端各方向期望速度,和分别是足端各方向实际速度。
由于在摆动相期间,恒定的弹簧系数难以保证精确的摆动相轨迹跟踪。为此本发明在四足机器人摆动相引入虚拟刚度加速度适应控制,实现摆动相足端虚拟弹簧系数随足端各时刻加速度改变而改变。首先,由于虚拟模型中虚拟阻尼系数很小,且假设实现轨迹跟踪后期望位置和实际位置的误差很小,以x方向为例,即xd-x=ε,ε是一个极小的常数。则摆动腿各方向虚拟力可简化为仅与虚拟弹簧系数有关的表达式:
Fx≈stiffnessx*ε,
Fy≈stiffnessy*ε,
Fz≈stiffnessz*ε。
其中m*为关于摆动足质量的参数,则摆动相各方向t时刻刚度为:
可知摆动相各方向虚拟弹簧系数正比于各方向加速度大小。令摆动相各方向t时刻刚度与(t+1)时刻刚度相比可得:
其中βx、βy和βz是定义的摆动相各时刻刚度比,给定摆动相初始虚拟弹簧系数为stiffnessx(0)、stiffnessy(0)和stiffnessz(0),则摆动相某一时刻的虚拟弹簧刚度关于时间t的表达式为:
由于摆动相各方向加速度在t≥0时有零值,因此虚拟弹簧刚度可能会出现为零或者过大的情况,故添加如下约束:
0.1*stiffnessx(0)≤stiffnessx(t)≤10*stiffnessx(0),
0.1*stiffnessy(0)≤stiffnessy(t)≤10*stiffnessy(0),
0.1*stiffnessz(0)≤stiffnessz(t)≤10*stiffnessz(0)。
经过引入摆动相的虚拟刚度加速度适应控制,摆动相虚拟力变为:
得到摆动相各时刻足端虚拟力后,通过雅可比矩阵可求得摆动足各关节的输入扭矩。此外,为了进一步精确轨迹跟踪性能,根据位置控制的思想通过逆运动学解算摆动足每一时刻各关节的预期角度,并与摆动足每一时刻各关节实际角度相比较,实现摆动相的关节角度比例微分控制。则摆动相各关节最终输入扭矩为:
Claims (5)
1.一种四足机器人优化足底力分配和轨迹跟踪的方法,其特征是:
(1)在四足机器人支撑相期间,将机器人支撑相分为支撑相前半段和支撑相后半段,应用分层二次规划,结合虚拟模型控制算法,将支撑相的姿态稳定和质心轨迹跟踪两个主要任务都转换为二次规划问题并求解,通过零空间投影得到最终优化足底力,并在支撑相分段切换瞬间,实现不同任务之间优先级对调,在支撑相前半段着重调节躯干虚拟扭矩,实现机器人着地后的机体稳定性能,在支撑相后半段着重调节躯干虚拟力,实现机器人离地前的质心轨迹跟踪;
(2)在四足机器人摇摆相期间,对预先得到的摆动相预期轨迹进行二次求导,得到摆动相各点加速度关于时间t的表达式,建立摆动相虚拟弹簧刚度与摆动相各点加速度之间的数值关系,实现四足机器人摆动相的虚拟刚度加速度适应控制,结合摆动相的关节角度比例微分控制,减小摆动相轨迹跟踪误差。
2.根据权利要求1所述的四足机器人优化足底力分配和轨迹跟踪的方法,其特征是:所述(1)的具体过程是:
首先给定四足机器人支撑相的质心轨迹和预期RPY角,通过虚拟模型控制算法求解躯干虚拟力和虚拟扭矩,并将躯干虚拟力、虚拟扭矩和足端给定力之间的力分配问题转化成分层二次规划问题,对机器人机体姿态稳定和机体质心轨迹跟踪两个支撑相主要任务转换为两个QP问题,通过零空间投影法对最优解进行处理,并在支撑相分段切换的瞬间实现不同任务之间优先级对调,以此实现不同阶段调节量之间的侧重,同时防止过大的足端虚拟力;对转换成的两个二次优化问题进行求解,得到最优足端虚拟力,并通过雅可比矩阵映射到各关节扭矩。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211592400.7A CN115857354A (zh) | 2022-12-09 | 2022-12-09 | 一种四足机器人优化足底力分配和轨迹跟踪的方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211592400.7A CN115857354A (zh) | 2022-12-09 | 2022-12-09 | 一种四足机器人优化足底力分配和轨迹跟踪的方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115857354A true CN115857354A (zh) | 2023-03-28 |
Family
ID=85672241
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211592400.7A Pending CN115857354A (zh) | 2022-12-09 | 2022-12-09 | 一种四足机器人优化足底力分配和轨迹跟踪的方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115857354A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117731276A (zh) * | 2024-02-19 | 2024-03-22 | 天津大学 | 一种信息处理方法和可穿戴设备 |
-
2022
- 2022-12-09 CN CN202211592400.7A patent/CN115857354A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117731276A (zh) * | 2024-02-19 | 2024-03-22 | 天津大学 | 一种信息处理方法和可穿戴设备 |
CN117731276B (zh) * | 2024-02-19 | 2024-04-19 | 天津大学 | 一种信息处理方法和可穿戴设备 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP2024512235A (ja) | 脚車輪型ロボットの制御方法、装置、脚車輪型ロボット及び機器 | |
CN105608309B (zh) | 一种双足机器人步行规划和控制方法 | |
JP5232124B2 (ja) | 脚式移動ロボットの制御装置 | |
CN100589937C (zh) | 腿式移动机器人及其控制程序 | |
CN108549237A (zh) | 基于深度增强学习的预观控制仿人机器人步态规划方法 | |
CN113485398B (zh) | 一种轮式双足机器人姿态控制方法 | |
CN101068663B (zh) | 腿式移动机器人及其控制方法 | |
CN111679579A (zh) | 针对传感器和执行器故障系统的滑模预测容错控制方法 | |
CN101068662B (zh) | 腿式移动机器人及其控制方法 | |
WO2024146206A1 (zh) | 一种应用于双足机器人快速行走的全身柔顺控制方法 | |
CN115857354A (zh) | 一种四足机器人优化足底力分配和轨迹跟踪的方法 | |
CN114397810A (zh) | 基于自适应虚拟模型控制的四足机器人运动控制方法 | |
CN112606843A (zh) | 一种基于Lyapunov-MPC技术的智能车辆路径跟踪控制方法 | |
CN113341698A (zh) | 基于神经网络调节双腿关节角的仿人机器人步行控制方法 | |
CN116237943A (zh) | 一种结合地形约束的四足机器人控制方法 | |
Dong et al. | On-line gait adjustment for humanoid robot robust walking based on divergence component of motion | |
Ficht et al. | Fast whole-body motion control of humanoid robots with inertia constraints | |
CN114700955B (zh) | 双轮腿-臂机器人全身运动规划与控制方法 | |
CN116859969A (zh) | 一种足式机器人的扰动抑制与稳定控制方法及装置 | |
CN114734445B (zh) | 一种四足机器人动态适应负载的控制方法 | |
Luo | Balance control based on six-dimensional spatial mechanics and velocity adjustment through region intervention and foot landing for quadruped robot | |
CN114063490B (zh) | 一种智能仿生足式机器人控制系统及方法 | |
Naksuk et al. | Humanoid trajectory generation: an iterative approach based on movement and angular momentum criteria | |
CN113467246B (zh) | 一种双足机器人偏摆力矩补偿方法 | |
Geng et al. | Optimal Control of Quadruped Robot Using HQP-Based Virtual Model Control |
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 |