CN115903874A - 机器人平衡控制方法、装置、可读存储介质及机器人 - Google Patents
机器人平衡控制方法、装置、可读存储介质及机器人 Download PDFInfo
- Publication number
- CN115903874A CN115903874A CN202211617222.9A CN202211617222A CN115903874A CN 115903874 A CN115903874 A CN 115903874A CN 202211617222 A CN202211617222 A CN 202211617222A CN 115903874 A CN115903874 A CN 115903874A
- Authority
- CN
- China
- Prior art keywords
- robot
- wheel
- foot
- model
- control
- 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 49
- 238000003860 storage Methods 0.000 title claims abstract description 17
- 238000004458 analytical method Methods 0.000 claims abstract description 33
- 239000011159 matrix material Substances 0.000 claims description 42
- 238000005457 optimization Methods 0.000 claims description 28
- 230000006870 function Effects 0.000 claims description 26
- 230000001133 acceleration Effects 0.000 claims description 20
- 238000004590 computer program Methods 0.000 claims description 19
- 230000007704 transition Effects 0.000 claims description 12
- 238000012933 kinetic analysis Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 description 16
- 238000004422 calculation algorithm Methods 0.000 description 14
- 238000010586 diagram Methods 0.000 description 13
- 230000005484 gravity Effects 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000001364 causal effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 210000000115 thoracic cavity Anatomy 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/60—Other road transportation technologies with climate change mitigation effect
- Y02T10/72—Electric energy management in electromobility
Landscapes
- Manipulator (AREA)
Abstract
本申请属于机器人技术领域,尤其涉及一种机器人平衡控制方法、装置、计算机可读存储介质及机器人。所述方法包括:对所述双轮足机器人的腿部和躯干进行模型简化,建立所述双轮足机器人的双轮足飞轮倒立摆模型;对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式;根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡。通过上述方法,可以对建立的双轮足飞轮倒立摆模型进行动力学分析,得到动力学模型表达式,并根据该模型表达式对双轮足机器人进行模型预测控制,实现了对双轮足机器人的控制量约束,同时提升了对双轮足机器人平衡控制的鲁棒性。
Description
技术领域
本申请属于机器人技术领域,尤其涉及一种机器人平衡控制方法、装置、计算机可读存储介质及机器人。
背景技术
现有技术中对双轮足机器人的平衡控制通常采用结合比例、积分和微分三种环节于一体的控制算法(Proportion Integral Differential,PID)或者线性二次型调节器(Linear Quadratic Regulator,LQR)算法来实现。
然而,基于PID算法的平衡控制器涉及大量的参数调整过程,且算法的鲁棒性较差;而LQR算法虽然稳定性和鲁棒性有所提升,但算法的误差较大且对控制量无约束,可能会出现控制量超限的问题。
发明内容
有鉴于此,本申请实施例提供了一种机器人平衡控制方法、装置、计算机可读存储介质及机器人,以解决现有的平衡控制算法难以兼容鲁棒性和对双轮足机器人控制量约束的问题。
本申请实施例的第一方面提供了一种机器人平衡控制方法,应用于双轮足机器人,可以包括:
对所述双轮足机器人的腿部和躯干进行模型简化,建立所述双轮足机器人的双轮足飞轮倒立摆模型;
对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式;
根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡。
在第一方面的一种具体实现方式中,在所述双轮足机器人的双轮足飞轮倒立摆模型中,所述双轮足机器人的腿部等效为一个连杆,所述双轮足机器人的躯干等效为一个具有质量的刚体,所述刚体的几何中心作为所述刚体的质心,所述连杆的两端分别与所述刚体和所述双轮足机器人的轮足连接。
在第一方面的一种具体实现方式中,对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式,包括:
分别对所述双轮足飞轮倒立摆模型中的轮足、连杆和刚体进行动力学分析,确定所述双轮足机器人的动力学方程;
根据所述动力学方程确定所述双轮足机器人的动力学模型表达式。
在第一方面的一种具体实现方式中,所述动力学方程为:
其中,mf为所述双轮足飞轮倒立摆模型中刚体的质量,mp为所述双轮足飞轮倒立摆模型中连杆的质量,Lfw为刚体转轴到轮足转轴之间的距离,Lc为所述刚体的质心到所述轮足转轴之间的距离,为所述双轮足机器人向前的加速度,θp为所述连杆相对于竖直平面的夹角,为所述连杆相对于竖直平面的角加速度,Jp为所述刚体绕所述轮足轮轴的转动惯量,Jw为所述轮足绕所述轮足转轴的转动惯量,Jf为所述刚体绕所述刚体转轴的转动惯量,为所述刚体相对于水平面的角加速度,R为所述轮足的半径,D为所述双轮足机器人的宽度,Ip为所述连杆相对z轴的转动惯量,为所述连杆的偏航角加速度,τl为左轮足的关节输入力矩,τr为右轮足的关节输入力矩,τf为所述刚体的关节输入力矩。
在第一方面的一种具体实现方式中,根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡,包括:
根据所述动力学模型表达式确定所述双轮足机器人的离散化的状态转移矩阵表达式;
获取所述双轮足机器人的当前状态量,并根据所述当前状态量和所述状态转移矩阵表达式确定所述双轮足机器人在预定时长后的预测状态量;
获取所述双轮足机器人在所述预定时长后的期望状态量,并根据所述预测状态量和所述期望状态量对所述双轮足机器人的控制量进行优化控制,确定所述双轮足机器人的当前控制量;
按照所述当前控制量对所述双轮足机器人进行控制。
在第一方面的一种具体实现方式中,所述根据所述预测状态量和所述期望状态量对所述双轮足机器人的控制量进行优化控制,确定所述双轮足机器人的当前控制量,包括:
计算所述预测状态量和所述期望状态量之间的状态量误差;
根据所述状态量误差和所述双轮足机器人的控制量构建优化目标函数;
对所述优化目标函数进行二次规划求解,得到所述双轮足机器人在所述预定时长内的各个控制时间点的控制量;
将所述预定时长内的第一个控制时间点的控制量确定为所述双轮足机器人的当前控制量。
在第一方面的一种具体实现方式中,所述优化目标函数为:
s.t.hj(Xk)≤0,j∈{1,…,N}
本申请实施例的第二方面提供了一种机器人平衡控制装置,应用于双轮足机器人中,所述装置可以包括:
模型建立模块,用于对所述双轮足机器人的腿部和躯干进行模型简化,建立所述双轮足机器人的双轮足飞轮倒立摆模型;
动力学模型确定模块,用于对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式;
预测控制模块,用于根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡。
在第二方面的一种具体实现方式中,所述模型建立模块具体可以用于将所述双轮足机器人的腿部等效为一个连杆,将所述双轮足机器人的躯干等效为一个具有质量的刚体,所述刚体的几何中心作为所述刚体的质心,将所述连杆的两端分别与所述刚体和所述双轮足机器人的轮足连接。
在第二方面的一种具体实现方式中,所述动力学模型确定模块可以包括:
动力学方程确定单元,用于分别对所述双轮足飞轮倒立摆模型中的轮足、连杆和刚体进行动力学分析,确定所述双轮足机器人的动力学方程;
模型表达式确定单元,用于根据所述动力学方程确定所述双轮足机器人的动力学模型表达式。
在第二方面的一种具体实现方式中,所述动力学方程为:
其中,mf为所述双轮足飞轮倒立摆模型中刚体的质量,mp为所述双轮足飞轮倒立摆模型中连杆的质量,Lfw为刚体转轴到轮足转轴之间的距离,Lc为所述刚体的质心到所述轮足转轴之间的距离,为所述双轮足机器人向前的加速度,θp为所述连杆相对于竖直平面的夹角,为所述连杆相对于竖直平面的角加速度,Jp为所述刚体绕所述轮足轮轴的转动惯量,Jw为所述轮足绕所述轮足转轴的转动惯量,Jf为所述刚体绕刚体转轴的转动惯量,为所述刚体相对于水平面的角加速度,R为所述轮足的半径,D为所述双轮足机器人的宽度,Ip为所述连杆相对z轴的转动惯量,为所述连杆的偏航角加速度,τl为左轮足的关节输入力矩,τr为右轮足的关节输入力矩,τf为所述刚体的关节输入力矩。
在第二方面的一种具体实现方式中,所述预测控制模块可以包括:
矩阵表达式确定单元,用于根据所述动力学模型表达式确定所述双轮足机器人的离散化的状态转移矩阵表达式;
预测状态量确定单元,用于获取所述双轮足机器人的当前状态量,并根据所述当前状态量和所述状态转移矩阵表达式确定所述双轮足机器人在预定时长后的预测状态量;
当前控制量确定单元,用于获取所述双轮足机器人在所述预定时长后的期望状态量,并根据所述预测状态量和所述期望状态量对所述双轮足机器人的控制量进行优化控制,确定所述双轮足机器人的当前控制量;
机器人控制单元,用于按照所述当前控制量对所述双轮足机器人进行控制。
在第二方面的一种具体实现方式中,所述当前控制量确定单元可以包括:
状态量误差计算子单元,用于计算所述预测状态量和所述期望状态量之间的状态量误差;
优化目标函数构建子单元,用于根据所述状态量误差和所述双轮足机器人的控制量构建优化目标函数;
二次规划求解子单元,用于对所述优化目标函数进行二次规划求解,得到所述双轮足机器人在所述预定时长内的各个控制时间点的控制量;
当前控制量确定子单元,用于将所述预定时长内的第一个控制时间点的控制量确定为所述双轮足机器人的当前控制量。
在第二方面的一种具体实现方式中,所述目标优化函数为:
s.t.hj(Xk)≤0,j∈{1,…,N}
本申请实施例的第三方面提供了一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,所述计算机程序被处理器执行时实现上述任一种机器人平衡控制方法的步骤。
本申请实施例的第四方面提供了一种机器人,包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序,所述处理器执行所述计算机程序时实现上述任一种机器人平衡控制方法的步骤。
本申请实施例的第五方面提供了一种计算机程序产品,当计算机程序产品在机器人上运行时,使得机器人执行上述任一种机器人平衡控制方法的步骤。
本申请实施例与现有技术相比存在的有益效果是:本申请实施例对所述双轮足机器人的腿部和躯干进行模型简化,建立所述双轮足机器人的双轮足飞轮倒立摆模型;对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式;根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡。通过本申请实施例,可以对建立的双轮足飞轮倒立摆模型进行动力学分析,得到动力学模型表达式,并根据该模型表达式对双轮足机器人进行模型预测控制,实现了对双轮足机器人的控制量约束,同时提升了对双轮足机器人平衡控制的鲁棒性。
附图说明
为了更清楚地说明本申请实施例中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它的附图。
图1为本申请实施例中一种机器人平衡控制方法的一个实施例流程图;
图2为轮足飞轮倒立摆模型示意图;
图3为对双轮足飞轮倒立摆模型进行动力学分析的示意流程图;
图4为对左轮足进行受力分析的示意图;
图5为对连杆进行受力分析的示意图;
图6为对刚体进行受力分析的示意图;
图7为对双轮足机器人进行模型预测控制的示意流程图;
图8为对双轮足机器人的当前控制量的确定过程的示意流程图;
图9为本申请实施例中一种机器人平衡控制装置的一个实施例结构图;
图10为本申请实施例中一种机器人的示意框图。
具体实施方式
为使得本申请的发明目的、特征、优点能够更加的明显和易懂,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,下面所描述的实施例仅仅是本申请一部分实施例,而非全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本申请保护的范围。
应当理解,当在本说明书和所附权利要求书中使用时,术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在,但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解,在此本申请说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本申请。如在本申请说明书和所附权利要求书中所使用的那样,除非上下文清楚地指明其它情况,否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解,在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合,并且包括这些组合。
如在本说明书和所附权利要求书中所使用的那样,术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地,短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。
另外,在本申请的描述中,术语“第一”、“第二”、“第三”等仅用于区分描述,而不能理解为指示或暗示相对重要性。
现有技术中常采用PID算法或者LQR算法对双轮足机器人进行平衡控制。然而,PID算法鲁棒性较差,而LQR算法误差较大且无法考虑对控制量的约束。可见,目前缺少兼容鲁棒性和控制量约束的双轮足机器人平衡控制算法。
有鉴于此,本申请实施例提供了一种机器人平衡控制方法、装置、计算机可读存储介质及机器人,以解决现有的平衡控制算法难以兼容鲁棒性和对双轮足机器人控制量约束的问题。
请参阅图1,本申请实施例中一种机器人平衡控制方法的一个实施例可以包括:
步骤S101、对双轮足机器人的腿部和躯干进行模型简化,建立双轮足机器人的双轮足飞轮倒立摆模型。
具体地,在本申请实施例中,可以将双轮足机器人的腿部等效为一个连杆,将双轮足机器人的躯干等效为一个具有质量的刚体,将连杆的两端分别与刚体和双轮足机器人的轮足连接。
可以理解的是,双轮足机器人的轮足包含左轮足和右轮足,左轮足和右轮足为对称结构。为了便于叙述,此处以双轮足机器人中的左轮足组成的轮足飞轮倒立摆模型为例,对本申请实施例中的模型简化过程进行说明。请参阅图2所示的轮足飞轮倒立摆模型示意图,可以将双轮足机器人的躯干部分(包含胸腔等结构)近似看成一个有质量的刚体,将双轮足机器人的腿部近似看成一个连杆,并且腿部的两端分别与双轮足机器人的左轮足和刚体连接。同理,根据上述模型简化过程,可以得出右轮足的轮足飞轮倒立摆模型。
步骤S102、对双轮足飞轮倒立摆模型进行动力学分析,确定双轮足机器人的动力学模型表达式。
请参阅图3,步骤S102可以包括具体如下过程:
步骤S1021、分别对双轮足飞轮倒立摆模型中的轮足、连杆和刚体进行动力学分析,确定双轮足机器人的动力学方程。
在本申请实施例中,双轮足飞轮倒立摆模型由双轮足机器人的轮足、腿部和躯干经过模型简化得到,所以可以首先对双轮足飞轮倒立摆模型中的轮足、连杆和刚体进行动力学分析,确定双轮足机器人的动力学方程,再根据得出的动力学方程得到动力学模型表达式。
下面将分别对本申请实施例中的轮足、连杆和刚体的动力学分析过程进行说明。
在本申请实施例中,可以对轮足进行动力学分析。由于本申请实施例中的双轮足机器人的左轮足与右轮足为对称结构,所以此处以左轮足为例,对轮足的动力学分析过程进行说明。在本申请实施例中,可以将左轮足的几何中心作为左轮足的质心,左轮足的受力主要来自连杆、地面以及机器人关节的输入。具体地,可以参阅图4所示的左轮足受力分析示意图(图4中仅示出部分受力分析),其中,双轮足机器人的左轮足受到来自连杆的作用力可以被分解为x方向的作用力和z方向的作用力,即HLx和HLz;左轮足受到来自地面的作用力为FL,在本申请实施例中可以认为左轮足与地面之间仅存在滚动摩擦,而不存在滑动摩擦;Gw为左轮足所受重力;θL为左轮足的旋转角度;τl为左轮足的关节输入力矩,R为左轮足的半径。
可以理解的是,因为本申请实施例中的右轮足和左轮足为对称结构,所以右轮足的受力分析可以参考上述左轮足受力分析过程得到,此处便不再赘述。
在本申请实施例中,还可以对连杆进行动力学分析。在本申请实施例中,可以将连杆的几何中心作为连杆的质心,连杆的受力主要来自轮足和刚体,并且会与轮足和刚体发生相对转动。具体地,可以参考图5所示的连杆受力分析示意图(图5中仅示出部分受力分析)。HLx和HLz分别为连杆受到来自左轮足的作用力在x方向和z方向上的投影,连杆受到来自右轮足的作用力在x方向和z方向上的投影可以分别记为为HRx和HRz(图5中并未示出),则可以将连杆受到来自轮足(包括左轮足和右轮足)的作用力记为HWx和HWz,其中:
而连杆受到来自刚体的作用力在x方向和z方向上的投影分别为Hfx和Hfz;Gp为连杆所受重力;θp为连杆相对于竖直平面的夹角。
在本申请实施例中,还可以对刚体进行动力学分析。在本申请实施例中,可以将刚体的几何中心作为刚体的质心,刚体主要受到来自连杆的力和自身重力。可以参阅图6所示的刚体受力分析示意图,其中,Hfx和Hfz分别为刚体受到来自连杆的力在x方向和z方向上的投影;θf为刚体相对于水平面的夹角;Gf为刚体所受重力。
在本申请实施例中,在进行动力学分析后确定出的动力学方程可以为:
其中,mf为双轮足飞轮倒立摆模型中刚体的质量,mp为双轮足飞轮倒立摆模型中连杆的质量,Lfw为刚体转轴到轮足转轴之间的距离,Lc为刚体的之心到轮足转轴之间的距离,为双轮足机器人向前的加速度,为连杆相对于竖直平面的角加速度,Jp为刚体绕轮足转轴的转动惯量,Jf为刚体绕刚体转轴的转动惯量,Jw为轮足绕轮足转轴的转动惯量,为刚体相对于水平面的角加速度,D为双轮足机器人的宽度,Ip为连杆相对z轴的转动惯量,为连杆的偏航角加速度,τr为右轮足的关节输入力矩,τf为刚体的关节输入力矩。
步骤S1022、根据动力学方程确定双轮足机器人的动力学模型表达式。
在本申请实施例中,可以以双轮足机器人轮足向前的位移和速度、连杆相对于竖直平面的夹角、角速度、偏航角、偏航角速度、刚体相对于水平面的夹角、角速度为系统状态量,以左轮足的关节输入力矩、右轮足的关节输入力矩和刚体的关节输入力矩为控制量,得出动力学模型表达式。即可以记为系统状态量控制量u=[τl,τr,τf]T,并确定出如下所示的动力学模型表达式:
其中,A为双轮足机器人的状态矩阵,B为双轮足机器人的输入矩阵。
步骤S103、根据动力学模型表达式对双轮足机器人进行模型预测控制,以保持双轮足机器人的平衡。
模型预测控制(Model Predictive Control,MPC)是从当前时刻根据系统当前的状态以及未来的输入,预测系统后续一段时间内的系统状态,并通过优化未来的控制输入量,使得未来的系统状态能够达到期望值,本申请实施例采用模型预测控制的方法进行平衡控制,使得双轮足飞轮倒立摆模型能够保持躯干位置和姿态的平衡。
请参阅图7,步骤S103可以包括具体如下过程:
步骤S1031、根据动力学模型表达式确定双轮足机器人的离散化的状态转移矩阵表达式。
在本申请实施例中,假设预测双轮足机器人从当前时刻(记为k)到预定时长(记为Ts)之后的状态量,预测步数为N,则在预测范围内每一小段时间为Ts/N,其中的N个控制时间点分别简化记为k+1,k+2,…,k+N时刻,分别对应的控制时间点为k+Ts/N,k+2*Ts/N,…,k+Ts,每个控制时间点固定一个控制量,分别记为uk,uk+1,…,uk+N-1。由动力学模型表达式,可以确定离散化的状态转移矩阵表达式:
步骤S1032、获取双轮足机器人的当前状态量,并根据当前状态量和状态转移矩阵表达式确定双轮足机器人在预定时长后的预测状态量。
在本申请实施例中,可以将双轮足机器人的当前状态量记为xk,根据上述状态转移矩阵表达式,可以递推预测双轮足机器人在预设时长内的各个控制时间点的预测状态量分别为:
可以将上述预测状态量整合成矩阵的形式:
Xk=Ψxk+ΘUk
其中:
需要理解的是,上述式中的下三角形式直接反映了系统状态量在时间上的因果关系,即当前的系统状态量只受过去的输入的影响,而不受之后时刻的输入的影响,也即k时刻的系统状态量不受k+1时刻的输入的影响。
步骤S1033、获取双轮足机器人在预定时长后的期望状态量,并根据预测状态量和期望状态量对双轮足机器人的控制量进行优化控制,确定双轮足机器人的当前控制量。
在本申请实施例中,可以首先计算出预测状态量和期望状态量之间的状态量误差,然后以状态量误差和双轮足机器人的控制量构建优化目标函数,并对优化目标函数进行二次规划(Quadratic Program,QP)求解。
请参阅图8,步骤S1033可以包括具体如下过程:
步骤S1033a、计算预测状态量和期望状态量之间的状态量误差。
可以理解的是,在本申请实施例中,控制目标是使系统状态量在k+Ts时刻的预测值跟踪期望值,即k+Ts时刻的预测状态量尽可能接近期望状态量。若预测状态量和期望状态量之间的误差越小,则表示预测状态越接近期望状态。所以,在本申请实施例中,可以将预测状态量和期望状态量之间的误差作为优化目标函数的组成部分。
步骤S1033b、根据状态量误差和双轮足机器人的控制量构建优化目标函数。
可以理解的是,为了提升资源利用率,在保证状态量误差尽量小的同时,希望双轮足机器人的控制量也能够尽量小。因此,在本申请实施例中,对于每个预测时刻,可以将状态量误差最小化和双轮足机器人的控制量最小化作为优化目标,并写成二次型形式:
其中,为在预测时刻k的期望状态量,w1为第一系数,w2为第二系数,其维度与状态量和控制量的时间维度一一对应。将上述二次型对整个预测时域展开,即考虑k+1~k+N共N个预测时刻,将N个时刻的优化目标整合并形成整个预测时域的优化目标函数f(Uk),转化为QP的标准形式为:
s.t.hj(Xk)≤0,j∈{1,…,N}
其中,U为预测控制量,Uk为控制量矩阵,Xk为预测状态量矩阵,为期望状态量矩阵,Q为第一系数矩阵,W为第二系数矩阵,hj(Xk)为上述预定时长的内N个控制时间点的线性约束方程,上述各个矩阵在时间维度上一一对应。
需要说明的是,在本申请实施例中,约束条件可以包括控制量幅值约束和状态量极限约束(包括最大速度和位置),并可以通过状态空间方程写成线性约束方程的形式,即上述hj(Xk)。
步骤S1033c、对优化目标函数进行二次规划求解,得到双轮足机器人在预定时长内的各个控制时间点的控制量。
通过QP求解器求解上述优化问题,可以得到双轮足机器人在预定时长内的N个控制时间点的控制量,即U=[uk,uk+1,…,uk+N-1]T。
步骤S1033d、将预定时长内的第一个控制时间点的控制量确定为双轮足机器人的当前控制量。
在本申请实施例中,可以将预定时长内的第一个控制时间点的控制量uk确定为双轮足机器人的当前控制量。
步骤S1034、按照当前控制量对双轮足机器人进行控制。
在本申请实施例中,可以将确定出的双轮足机器人的当前控制量施加于轮足和刚体,而后在每个控制时间点均可以按照步骤S1032至步骤S1034进行滚动优化求解,即可实现对整个过程的优化控制。
综上所述,本申请实施例对所述双轮足机器人的腿部和躯干进行模型简化,建立所述双轮足机器人的双轮足飞轮倒立摆模型;对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式;根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡。通过上述方法,可以对建立的双轮足飞轮倒立摆模型进行动力学分析,得到动力学模型表达式,并根据该模型表达式对双轮足机器人进行模型预测控制,实现了对双轮足机器人的控制量约束,同时提升了对双轮足机器人平衡控制的鲁棒性。
应理解,上述实施例中各步骤的序号的大小并不意味着执行顺序的先后,各过程的执行顺序应以其功能和内在逻辑确定,而不应对本申请实施例的实施过程构成任何限定。
对应于上文实施例所述的一种机器人平衡控制方法,图9示出了本申请实施例提供的一种机器人平衡控制装置的一个实施例结构图,应用于双轮足机器人中。
本实施例中,一种应用于双轮足机器人的机器人平衡控制装置可以包括:
模型建立模块901,用于对所述双轮足机器人的腿部和躯干进行模型简化,建立所述双轮足机器人的双轮足飞轮倒立摆模型;
动力学模型确定模块902,用于对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式;
预测控制模块903,用于根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡。
在本申请实施例的一种具体实现方式中,所述模型建立模块具体可以用于将所述双轮足机器人的腿部等效为一个连杆,将所述双轮足机器人的躯干等效为一个具有质量的刚体,所述刚体的几何中心作为所述刚体的质心,将所述连杆的两端分别与所述刚体和所述双轮足机器人的轮足连接。
在本申请实施例的一种具体实现方式中,所述动力学模型确定模块可以包括:
动力学方程确定单元,用于分别对所述双轮足飞轮倒立摆模型中的轮足、连杆和刚体进行动力学分析,确定所述双轮足机器人的动力学方程;
模型表达式确定单元,用于根据所述动力学方程确定所述双轮足机器人的动力学模型表达式。
在本申请实施例的一种具体实现方式中,所述动力学方程为:
其中,mf为所述双轮足飞轮倒立摆模型中刚体的质量,mp为所述双轮足飞轮倒立摆模型中连杆的质量,Lfw为刚体转轴到轮足转轴之间的距离,Lc为所述刚体的质心到所述轮足转轴之间的距离,为所述双轮足机器人向前的加速度,θp为所述连杆相对于竖直平面的夹角,为所述连杆相对于竖直平面的角加速度,Jp为所述刚体绕所述轮足轮轴的转动惯量,Jw为所述轮足绕所述轮足转轴的转动惯量,Jf为所述刚体绕刚体转轴的转动惯量,为所述刚体相对于水平面的角加速度,R为所述轮足的半径,D为所述双轮足机器人的宽度,Ip为所述连杆相对z轴的转动惯量,为所述连杆的偏航角加速度,τl为左轮足的关节输入力矩,τr为右轮足的关节输入力矩,τf为所述刚体的关节输入力矩。
在本申请实施例的一种具体实现方式中,所述预测控制模块可以包括:
矩阵表达式确定单元,用于根据所述动力学模型表达式确定所述双轮足机器人的离散化的状态转移矩阵表达式;
预测状态量确定单元,用于获取所述双轮足机器人的当前状态量,并根据所述当前状态量和所述状态转移矩阵表达式确定所述双轮足机器人在预定时长后的预测状态量;
当前控制量确定单元,用于获取所述双轮足机器人在所述预定时长后的期望状态量,并根据所述预测状态量和所述期望状态量对所述双轮足机器人的控制量进行优化控制,确定所述双轮足机器人的当前控制量;
机器人控制单元,用于按照所述当前控制量对所述双轮足机器人进行控制。
在本申请实施例的一种具体实现方式中,所述当前控制量确定单元可以包括:
状态量误差计算子单元,用于计算所述预测状态量和所述期望状态量之间的状态量误差;
优化目标函数构建子单元,用于根据所述状态量误差和所述双轮足机器人的控制量构建优化目标函数;
二次规划求解子单元,用于对所述优化目标函数进行二次规划求解,得到所述双轮足机器人在所述预定时长内的各个控制时间点的控制量;
当前控制量确定子单元,用于将所述预定时长内的第一个控制时间点的控制量确定为所述双轮足机器人的当前控制量。
在本申请实施例的一种具体实现方式中,所述目标优化函数为:
s.t.hj(Xk)≤0,j∈{1,…,N}
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的装置,模块和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
在上述实施例中,对各个实施例的描述都各有侧重,某个实施例中没有详述或记载的部分,可以参见其它实施例的相关描述。
图10示出了本申请实施例提供的一种机器人的示意框图,为了便于说明,仅示出了与本申请实施例相关的部分。
如图10所示,该实施例的机器人10包括:处理器100、存储器101以及存储在所述存储器101中并可在所述处理器100上运行的计算机程序102。所述处理器100执行所述计算机程序102时实现上述各个机器人平衡控制方法实施例中的步骤,例如图1所示的步骤S101至步骤S103。或者,所述处理器100执行所述计算机程序102时实现上述各装置实施例中各模块/单元的功能,例如图9所示模块901至模块903的功能。
示例性的,所述计算机程序102可以被分割成一个或多个模块/单元,所述一个或者多个模块/单元被存储在所述存储器101中,并由所述处理器100执行,以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段,该指令段用于描述所述计算机程序102在所述机器人10中的执行过程。
本领域技术人员可以理解,图10仅仅是机器人10的示例,并不构成对机器人10的限定,可以包括比图示更多或更少的部件,或者组合某些部件,或者不同的部件,例如所述机器人10还可以包括输入输出设备、网络接入设备、总线等。
所述处理器100可以是中央处理单元(Central Processing Unit,CPU),还可以是其它通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器101可以是所述机器人10的内部存储单元,例如机器人10的硬盘或内存。所述存储器101也可以是所述机器人10的外部存储设备,例如所述机器人10上配备的插接式硬盘,智能存储卡(Smart Media Card,SMC),安全数字(Secure Digital,SD)卡,闪存卡(Flash Card)等。进一步地,所述存储器101还可以既包括所述机器人10的内部存储单元也包括外部存储设备。所述存储器101用于存储所述计算机程序以及所述机器人10所需的其它程序和数据。所述存储器101还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到,为了描述的方便和简洁,仅以上述各功能单元、模块的划分进行举例说明,实际应用中,可以根据需要而将上述功能分配由不同的功能单元、模块完成,即将所述装置的内部结构划分成不同的功能单元或模块,以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中,上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。另外,各功能单元、模块的具体名称也只是为了便于相互区分,并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
在上述实施例中,对各个实施例的描述都各有侧重,某个实施例中没有详述或记载的部分,可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中,应该理解到,所揭露的装置/机器人和方法,可以通过其它的方式实现。例如,以上所描述的装置/机器人实施例仅仅是示意性的,例如,所述模块或单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口,装置或单元的间接耦合或通讯连接,可以是电性,机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外,在本申请各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读存储介质中。基于这样的理解,本申请实现上述实施例方法中的全部或部分流程,也可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一计算机可读存储介质中,该计算机程序在被处理器执行时,可实现上述各个方法实施例的步骤。其中,所述计算机程序包括计算机程序代码,所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读存储介质可以包括:能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是,所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减,例如在某些司法管辖区,根据立法和专利实践,计算机可读存储介质不包括电载波信号和电信信号。
以上所述实施例仅用以说明本申请的技术方案,而非对其限制;尽管参照前述实施例对本申请进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围,均应包含在本申请的保护范围之内。
Claims (10)
1.一种机器人平衡控制方法,其特征在于,应用于双轮足机器人中,所述方法包括:
对所述双轮足机器人的腿部和躯干进行模型简化,建立所述双轮足机器人的双轮足飞轮倒立摆模型;
对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式;
根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡。
2.根据权利要求1所述的机器人平衡控制方法,其特征在于,在所述双轮足机器人的双轮足飞轮倒立摆模型中,所述双轮足机器人的腿部等效为一个连杆,所述双轮足机器人的躯干等效为一个具有质量的刚体,所述刚体的几何中心作为所述刚体的质心,所述连杆的两端分别与所述刚体和所述双轮足机器人的轮足连接。
3.根据权利要求1所述的机器人平衡控制方法,其特征在于,对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式,包括:
分别对所述双轮足飞轮倒立摆模型中的轮足、连杆和刚体进行动力学分析,确定所述双轮足机器人的动力学方程;
根据所述动力学方程确定所述双轮足机器人的动力学模型表达式。
4.根据权利要求3所述的机器人平衡控制方法,其特征在于,所述动力学方程为:
其中,mf为所述双轮足飞轮倒立摆模型中刚体的质量,mp为所述双轮足飞轮倒立摆模型中连杆的质量,Lfw为刚体转轴到轮足转轴之间的距离,Lc为所述刚体的质心到所述轮足转轴之间的距离,为所述双轮足机器人向前的加速度,θp为所述连杆相对于竖直平面的夹角,为所述连杆相对于竖直平面的角加速度,Jp为所述刚体绕所述轮足轮轴的转动惯量,Jw为所述轮足绕所述轮足转轴的转动惯量,Jf为所述刚体绕所述刚体转轴的转动惯量,为所述刚体相对于水平面的角加速度,R为所述轮足的半径,D为所述双轮足机器人的宽度,Ip为所述连杆相对z轴的转动惯量,为所述连杆的偏航角加速度,τl为左轮足的关节输入力矩,τr为右轮足的关节输入力矩,τf为所述刚体的关节输入力矩。
5.根据权利要求1至4中任一项所述的机器人平衡控制方法,其特征在于,根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡,包括:
根据所述动力学模型表达式确定所述双轮足机器人的离散化的状态转移矩阵表达式;
获取所述双轮足机器人的当前状态量,并根据所述当前状态量和所述状态转移矩阵表达式确定所述双轮足机器人在预定时长后的预测状态量;
获取所述双轮足机器人在所述预定时长后的期望状态量,并根据所述预测状态量和所述期望状态量对所述双轮足机器人的控制量进行优化控制,确定所述双轮足机器人的当前控制量;
按照所述当前控制量对所述双轮足机器人进行控制。
6.根据权利要求5所述的机器人平衡控制方法,其特征在于,所述根据所述预测状态量和所述期望状态量对所述双轮足机器人的控制量进行优化控制,确定所述双轮足机器人的当前控制量,包括:
计算所述预测状态量和所述期望状态量之间的状态量误差;
根据所述状态量误差和所述双轮足机器人的控制量构建优化目标函数;
对所述优化目标函数进行二次规划求解,得到所述双轮足机器人在所述预定时长内的各个控制时间点的控制量;
将所述预定时长内的第一个控制时间点的控制量确定为所述双轮足机器人的当前控制量。
8.一种机器人平衡控制装置,其特征在于,应用于双轮足机器人中,所述装置包括:
模型建立模块,用于对所述双轮足机器人的腿部和躯干进行模型简化,建立所述双轮足机器人的双轮足飞轮倒立摆模型;
动力学模型确定模块,用于对所述双轮足飞轮倒立摆模型进行动力学分析,确定所述双轮足机器人的动力学模型表达式;
预测控制模块,用于根据所述动力学模型表达式对所述双轮足机器人进行模型预测控制,以保持所述双轮足机器人的平衡。
9.一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的机器人平衡控制方法的步骤。
10.一种机器人,包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序,其特征在于,所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的机器人平衡控制方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211617222.9A CN115903874A (zh) | 2022-12-15 | 2022-12-15 | 机器人平衡控制方法、装置、可读存储介质及机器人 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211617222.9A CN115903874A (zh) | 2022-12-15 | 2022-12-15 | 机器人平衡控制方法、装置、可读存储介质及机器人 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115903874A true CN115903874A (zh) | 2023-04-04 |
Family
ID=86476660
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211617222.9A Pending CN115903874A (zh) | 2022-12-15 | 2022-12-15 | 机器人平衡控制方法、装置、可读存储介质及机器人 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115903874A (zh) |
-
2022
- 2022-12-15 CN CN202211617222.9A patent/CN115903874A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Liu et al. | Adaptive finite-time control for half-vehicle active suspension systems with uncertain dynamics | |
Dai et al. | Force control for path following of a 4WS4WD vehicle by the integration of PSO and SMC | |
CN109131351B (zh) | 基于随机时滞的车辆稳定性评价方法 | |
CN113408047B (zh) | 一种基于时滞反馈神经网络的车辆动力学预测模型、及训练数据获取方法、训练方法 | |
CN111730605A (zh) | 一种机器人姿态控制方法、装置、可读存储介质及机器人 | |
CN109270946A (zh) | 挠性航天器的姿态控制方法 | |
CN112792807A (zh) | 机器人控制方法、装置、计算机可读存储介质及机器人 | |
CN111185909B (zh) | 机器人运行工况获取方法、装置、机器人及存储介质 | |
Hidalgo et al. | Real-time dynamic simulations of large road vehicles using dense, sparse, and parallelization techniques | |
CN113927585B (zh) | 一种机器人平衡控制方法、装置、可读存储介质及机器人 | |
CN113656891B (zh) | 液体火箭动特性建模分析方法及终端设备 | |
Zeng et al. | Event-triggered controller design for active suspension systems: An adaptive backstepping method with error-dependent gain | |
CN112276952B (zh) | 一种多机器人系统的鲁棒同时镇定方法及系统 | |
CN112731951B (zh) | 一种机器人平衡控制方法、装置、可读存储介质及机器人 | |
CN118034284A (zh) | 多个机器人的避碰控制方法、装置、终端设备及存储介质 | |
WO2022105023A1 (zh) | 机器人控制方法、装置、计算机可读存储介质及机器人 | |
CN115903874A (zh) | 机器人平衡控制方法、装置、可读存储介质及机器人 | |
CN113806958A (zh) | 基于mpc算法的防侧倾控制方法、设备及存储介质 | |
CN111639388B (zh) | 一种汽车弹性元件参数仿真方法及系统 | |
CN112757299A (zh) | 质心轨迹的获取方法、装置、机器人及存储介质 | |
Callejo et al. | A hybrid direct‐automatic differentiation method for the computation of independent sensitivities in multibody systems | |
CN112720478B (zh) | 一种机器人力矩控制方法、装置、可读存储介质及机器人 | |
CN116279510A (zh) | 一种道路纵向坡度和横向坡度联合估计方法 | |
CN116047888A (zh) | 一种基于bp神经网络pid的自平衡车的控制方法 | |
CN116000922A (zh) | 机器人姿态控制方法、装置、可读存储介质及机器人 |
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 |