动基座机械臂末端负载质量在线估计方法

1.本发明涉及一种动基座机械臂末端负载质量在线估计方法，属于机器人控制技术领域。


背景技术：

2.随着工业4.0时代的开启，机器人产业的蓬勃发展为经济社会发展注入强劲动能，正极大改变着人类生产与生活方式。《中华人民共和国国民经济和社会发展第十四个五年规划和2035年远景目标纲要》指出，服务机器人、特种机器人在仓储物流、教育娱乐、清洁服务等领域将实现规模应用。而移动机械臂机器人与工业机械臂相比具有运动范围广、作业空间灵活的优势，成为下一代机器人发展的着力点。同时，作业空间复杂、作业任务多样化对机器人的控制提出了挑战。移动机械臂作业时，机械臂与外部环境的交互力为机器人控制器构造的重要输入项，应根据交互力的大小规划动基座的运动，实现作业机器人的稳定控制。因此，如何获取交互力的精确值成为移动机械臂控制的关键问题。
3.对于工业机械臂的末端力估计研究较多，最直观的方法是在末端安装力传感器进行交互力测量，但该方法具有局限性：第一，满足精度与可靠度要求的传感器成本过高；第二，力传感器的安装位置具有局限性，不能保证每次接触都能够产生正压力，从而获得可靠接触力数据；第三，力传感器通过自身形变对接触力进行估计，影响末端位置控制精度。因此，研究人员提出了多种力估计方法以替代力传感器。基于动力学模型的交互力估计方法如2021年发表于robotics and computer-integrated manufacturing的《sensorless forceestimation for industrial robots using disturbance observer and neural learningof friction approximation》(采用干扰观测器和摩擦近似神经学习的工业机器人无传感器力估计)采用神经网络学习方法对机器人的动力学模型误差进行补偿与矫正，然后构造了基于卡尔曼滤波的扰动观测器，实现对定基座机械臂的末端交互力估计。2020年发表于 ieee/asme transactions on mechatronics的《precision motion control of a 6-dofsindustrial robot with accurate payload estimation》(基于负载质量精确估计的六自由度工业机器人精确位置控制)提出一种基于广义动量的自适应控制率，实现了在线负载质量估计。
4.但对于动基座机械臂(轮式移动机械臂、人形双臂机器人、四足单臂\双臂机器人等) 而言，适用于工业机器人的末端负载质量估计方法无法直接进行迁移应用，同时，机械臂末端交互力直接影响动基座的控制，因此，对交互力的估计更加必要。意大利技术研究院研究团队在四足机器人上增加一条机械臂，以赋予hyq作业能力，发表于2016ieeeinternational conference on robotics and automation的《towards a multi-leggedmobile manipulator》(多腿移动机械臂)中为了实现对机械臂末端力的估计，假设躯干为固定基座，基于机械臂的精确动力学模型进行末端力估计。仍然延续了工业机器人的末端力估计方法。该方法存在缺点：忽略了末端力对基座运动的影响，因此造成末端力估计不准确。


技术实现要素：

5.本发明的目的是为了克服现有技术的上述不足，提供一种动基座机械臂末端负载质量在线估计方法。
6.当动基座机械臂末端受到外界交互力时，为了抵抗外部力以及机械臂杆件的重力、惯性力、惯性力矩等，躯干需要为其提供合适的反向广义力。动基座机械臂的推力的根本来源是足-地接触所产生的交互力，将动基座机械臂的腿足式底盘对躯干的推动广义力映射为躯干质心处的六维广义力w
leg
，同时，躯干还受到与机械臂的交互力，wa基于单刚体的牛顿
ꢀ‑
欧拉公式，将躯干的动力学模型采用解析解的形式进行表示：
[0007][0008]
其中，mb为躯干惯性矩阵，pb为躯干六维位姿，因此，为躯干位姿加速度，gb为躯干重力矩阵，ja为机械臂-躯干交互力向躯干质心映射的力雅克比矩阵。
[0009]
而wa可表示为与机械臂杆件质量ma、负载质量m
obj
呈线性关系的形式：
[0010][0011]
其中，分别为末端负载质量与机械臂杆件质量施加于机械臂
‑ꢀ
躯干交互力影响的线性回归解析式。q、分别为机械臂关节的空间位置、速度与加速度。
[0012]
将公式(2)带入公式(1)，便得到与机械臂杆件质量、负载质量相关的躯干动力学模型：
[0013][0014]
基于公式(3)构造动基座推力控制模型，使得腿足式底盘根据躯干的当前运动状态进行推力的调节。该处采可以采用虚拟模型控制方法，以躯干的期望位姿与实际位姿误差，构造躯干质心处的六维虚拟广义力wd：
[0015][0016]
其中，wd为六维虚拟广义力，k
p
为正定的比例增益矩阵，kd为正定的微分增益矩阵，pd为躯干的期望位姿，为躯干的期望速度，pb为躯干的实际位姿，为躯干的实际速度， gb为躯干重力矩阵，ja为机械臂-躯干交互力向躯干质心映射的力雅克比矩阵，为末端负载质量的估计值对于机械臂-躯干交互力的线性回归解析式，为机械臂杆件质量对于机械臂-躯干交互力的线性回归解析式，为负载质量的估计值，ma为机械臂杆件质量。
[0017]
可见，当不等于m
obj
分增时，直接影响到躯干位姿的控制。将基于负载质量的估计值计算得到的推力记为u
fb
＝wd。如果当施加推力u
fb
于躯干时，躯干应沿期望轨迹运行，如果躯干实际运动偏离期望轨迹，则负载质量的估计值不准确。
[0018]
为了对躯干的实际受力进行估计，首先，对躯干的状态量即躯干的实时六维位姿x
fb
进行取回，由于躯干的运动是波动的，取回的实时位姿存在严重的噪声干扰，从而导致躯
干实际受力的估计存在误差。因此，将运动状态采样频率设置为7.69hz(即负载质量的估计值的更新频率为7.69hz)，有效滤除了高频扰动。通过微分得到躯干速度与加速度基于公式(3)所示的动力学模型，可得到下式：
[0019][0020]
其中，ud为负载质量误差(负载质量的估计值与实际值之间的差)造成的输入构造误差。此时，ud与负载质量的估计误差存在以下关系：
[0021][0022]
根据公式(5)，构造负载质量自适应迭代速度滑模控制模型为：
[0023][0024]
其中，为负载质量的自适应迭代速度，为负载质量的估计值对于机械臂
ꢀ‑
躯干交互力的线性回归解析式，为的倒数，diag表示将括号内的列向量元素排列为对角阵，ud为负载质量误差造成的输入构造误差，u
fb
为推力，u
fb
＝wd，表示求括号内向量各个元素的平均数，sat为饱和函数。sat饱和函数能够有效抑制的大幅度抖动，sat的具体形式为：
[0025][0026]
此处，其中，为负载质量自适应迭代的阈值。负载质量的估计值通过以下公式进行更新，并渐近稳定在实际值m
obj
处。迭代后的负载质量的估计值用于下一时刻的躯干位姿控制模型构造，计算wd。
[0027][0028]
本发明采用下述技术方案：
[0029]
一种动基座机械臂末端负载质量在线估计方法，包括以下步骤：
[0030]
s1.获取初始数值作为负载质量的估计值；
[0031]
s2.根据动基座推力控制模型对负载质量的估计值进行处理，获得六维虚拟广义力；
[0032]
s3.实施推力进行相应动作，所述推力等于六维虚拟广义力；
[0033]
s4.采集躯干六维位姿反馈信号；
[0034]
s5.基于躯干受力估计模型对躯干六维位姿反馈信号进行处理，获得躯干实际受力；
[0035]
s6.记录本次执行的推力为反馈受力；将反馈受力与躯干实际受力进行比对，获得反馈受力与躯干实际受力的比对关系：
[0036]
s7.根据反馈受力与躯干实际受力的比对关系对负载质量的估计值进行处理：
[0037]
当躯干实际受力与反馈受力相等时，当前负载质量的估计值即为负载质量的实际值；
[0038]
当躯干实际受力值小于或大于反馈受力时，采用负载质量自适应迭代速度滑模控制模型对当前负载质量的估计值进行处理，获取新的负载质量的估计值，继续执行s2-s7，直至躯干实际受力与反馈受力相等。
[0039]
其中，步骤s1所述的初始数值可以由机器人的控制中心随机分配，可以是任意值，例如是0。步骤s4中采样频率可以设置为7.69hz。
[0040]
步骤s2中所述动基座推力控制模型为：
[0041][0042]
其中，wd为六维虚拟广义力，k
p
为正定的比例增益矩阵，kd为正定的微分增益矩阵，pd为躯干的期望位姿，为躯干的期望速度，pb为躯干的实际位姿，为躯干的实际速度， gb为躯干重力矩阵，ja为机械臂-躯干交互力向躯干质心映射的力雅克比矩阵，为末端负载质量的估计值对于机械臂-躯干交互力的线性回归解析式，为机械臂杆件质量对于机械臂-躯干交互力的线性回归解析式，q、分别为机械臂的空间位置、速度与加速度，为负载质量的估计值，ma为机械臂杆件质量。
[0043]
步骤s5所述躯干受力估计模型为：
[0044][0045]
其中，mb为躯干实际受力，为躯干位姿加速度，gb为躯干重力矩阵，w
leg
为腿部施加于躯干的力，ja为机械臂-躯干交互力向躯干质心映射的力雅克比矩阵，为负载质量的估计值对于机械臂-躯干交互力的线性回归解析式，为机械臂杆件质量对于机械臂-躯干交互力的线性回归解析式，m
obj
取负载质量的估计值ma为机械臂杆件质量，q、分别为机械臂的空间位置、速度与加速度。
[0046]
步骤s7中所述负载质量自适应迭代速度滑模控制模型包括负载质量自适应迭代速度公式和负载质量更新公式；
[0047]
负载质量自适应迭代速度公式为：其中，为负载质量的自适应迭代速度，q、分别为机械臂的空间位置、速度与加速度；为负载质量的估计值对于机械臂-躯干交互力的线性回归解析式，为的倒数，diag表示将括号内的列向量元素排列为对角阵，ud为负载质量误差造成的输入构造误差，u
fb
为推力，u
fb
＝wd，表示求括号内向量各个元素的平均数，sat为饱和函数；sat的具体形式为：
[0048][0049]
此处，其中，为负载质量的估计值自适应迭代的阈值；
[0050]
负载质量更新公式：其中，公式左侧的为新的
[0051]
该方法的前提是假设动基座移动方式(轮式、履带式或腿足式等)在无负载质量或者精确获取负载质量的前提下，能够驱动躯干精确伺服期望位姿。该方法的假设条件易于满足，且简单易懂，可操作性强。
[0052]
动基座机械臂的机械臂推力的根本来源是足-地接触所产生的交互力，将腿足式底盘与机械臂末端输出力同时映射到躯干力空间，在该空间进行输入与状态量的比较分析，极大简化了算法流程。其算法流程分为执行-观测-校正三部分；具体流程图如图1所示；具体交互过程如图2所示。
[0053]
本发明的有益效果是：
[0054]
本发明的动基座机械臂末端负载质量在线估计方法，将机械臂杆件、机械臂末端负载质量、动态运动所产生的，施加于躯干的力矢量整合为躯干-机械臂交互力，并描述为与机械臂杆件、末端负载质量线性相关的形式，避免了整体动力学模型繁杂的解析公式带来的实时计算压力。
[0055]
本发明的动基座机械臂末端负载质量在线估计方法，以躯干的位姿反馈值，对躯干所受合力进行估计，以描述基座主动推力、机械臂交互合力对躯干产生的总影响，而不需要面向基座与机械臂来计算其对躯干产生的交互力，大大简化了该交互力的估计复杂度。
[0056]
本发明的动基座机械臂末端负载质量在线估计方法，构造了负载质量自适应迭代速度滑模控制模型对负载质量的迭代速度进行更新，保证了预测值平滑趋近于实际值。
[0057]
本发明以通过对机械臂负载质量进行精确估计，确保动基座运动控制的可靠性。而机械臂负载质量的不精确与实时变化为动基座的位姿控制产生影响，基于机械臂负载质量对位姿控制的干扰，解决机械臂负载质量无法精确获取的问题。该方法简单易懂，且适用于多种类型动基座机械臂。
[0058]
本发明的动基座机械臂末端负载质量在线估计方法，能够基于当前参数所构造并施加的动基座推力与躯干实际推力预测值对机械臂末端负载质量进行实时估计，以调整控制器模型参数，不断改善系统控制性能。
附图说明
[0059]
图1是本发明动基座机械臂末端负载质量在线估计方法的流程图；
[0060]
图2是本发明动基座机械臂末端负载质量在线估计方法的交互过程图；
[0061]
图3是本发明具体实施方式中所涉及的搭载一条三自由度机械臂的四足机器人拓扑结构示意图；
[0062]
图4是将腿足式底盘对躯干的作用转化为躯干所受六维广义力后，图2所示机器人
的躯干受力示意图。
具体实施方式
[0063]
下面结合附图和实施例对本发明进一步说明。
[0064]
本说明书附图所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容涵盖的范围内。同时，本说明书中所引用的如“上”、“下”、“左”、“右”、“中间”及“一”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。
[0065]
以搭载一条三自由度机械臂的动基座四足机器人(以下简称机器人)为例详细阐述动基座机械臂末端负载质量在线估计方法。该搭载一条三自由度机械臂的四足机器人拓扑结构示意图如图3所示；将腿足式底盘对躯干的作用转化为躯干所受六维广义力后，图3所示四足机器人的躯干受力示意图如图4所示。
[0066]
给予机器人的机械臂末端以质量为xkg的负载。
[0067]
s1.机器人首先获得一随机数值作为负载质量的估计值；该随机数值即初始数值；初始数值可以为0。
[0068]
s2.机器人根据动基座推力控制模型对负载质量的估计值(初始数值)进行处理，获得六维虚拟广义力wd。所述动基座推力控制模型为：
[0069][0070]
其中，wd为六维虚拟广义力，k
p
为正定的比例增益矩阵，kd为正定的微分增益矩阵，pd为躯干的期望位姿，为躯干的期望速度，pb与分别为躯干的实际位姿与实际速度，通过实时测量得到。gb为躯干重力矩阵，ja为机械臂-躯干交互力向躯干质心映射的力雅克比矩阵，为末端负载质量的估计值对于机械臂-躯干交互力的线性回归解析式，为机械臂杆件质量对于机械臂-躯干交互力的线性回归解析式，以上四部分均为与机器人物理参数、关节运动状态相关的解析式，其中，物理参数可在建模时通过测量得到，关节运动状态q、分别为机械臂空间位置、速度与加速度，通过实时测量得到。为负载质量的估计值，ma为机械臂杆件质量。
[0071]
s3.实施推力进行相应动作，所述推力等于六维虚拟广义力。即，机器人将获得的六维虚拟广义力wd作为推力，机器人执行该推力进行动作。机器人的躯干六维位姿发生变化。
[0072]
s4.实时采集躯干六维位姿反馈信号，包括躯干的三维线加速度与三维角加速度。
[0073]
s5.基于躯干受力估计模型对躯干六维位姿反馈信号进行处理，获得躯干实际受力；
[0074]
步骤s5所述躯干受力估计模型为：
[0075][0076]
其中，mb为躯干实际受力，为躯干位姿加速度，gb为躯干重力矩阵，w
leg
为腿部施加于躯干的力，ja为机械臂-躯干交互力向躯干质心映射的力雅克比矩阵，为负载质量的估计值对于机械臂-躯干交互力的线性回归解析式，为机械臂杆件质量对于机械臂-躯干交互力的线性回归解析式，m
obj
取负载质量的估计值ma为机械臂杆件质量，q、分别为机械臂的空间位置、速度与加速度。
[0077]
为负载质量的估计值对于机械臂-躯干交互力的线性回归解析式，为机械臂杆件质量对于机械臂-躯干交互力的线性回归解析式。机器人构型不同，具体解析式的形式不同；线性回归解析式可以通过动力学模型整理整理得出，属于本领域技术人员的公知常识，此处不再赘述。
[0078]
s6.记录本次执行的推力wd为反馈受力，即反馈受力等于wd；将反馈受力wd与躯干实际受力mb进行比对，获得反馈受力与躯干实际受力的比对关系：
[0079]
s7.根据反馈受力与躯干实际受力的比对关系对负载质量的估计值进行处理：
[0080]
当躯干实际受力mb与反馈受力wd相等时，当前负载质量的估计值即为负载质量的实际值m
obj
；
[0081]
当躯干实际受力mb小于或大于反馈受力wd时，采用负载质量自适应迭代速度滑模控制模型对当前负载质量的估计值进行处理，获取新的负载质量的估计值继续执行 s2-s7，直至躯干实际受力mb与反馈受力wd相等。
[0082]
步骤s7中所述负载质量自适应迭代速度滑模控制模型包括负载质量自适应迭代速度公式和负载质量更新公式；即，先根据负载质量自适应迭代速度公式获得负载质量的自适应迭代速度，然后将负载质量的自适应迭代速度带入到负载质量更新公式中；根据当前负载质量的估计值和负载质量的自适应迭代速度计算得出新的负载质量的估计值。然后根据新的负载质量的估计值重复步骤s2-s7直至躯干实际受力mb与反馈受力wd相等。当躯干实际受力mb与反馈受力wd相等时，负载质量的估计值则等于负载质量的真实值。
[0083]
负载质量自适应迭代速度公式为：其中，为负载质量的自适应迭代速度，q、分别为机械臂的空间位置、速度与加速度；为负载质量的估计值对于机械臂-躯干交互力的线性回归解析式，为的倒数，diag表示将括号内的列向量元素排列为对角阵，ud为负载质量误差造成的输入构造误差，u
fd
为推力，u
fd
＝wd，表示求求括号内向量各个元素的平均数，sat为饱和函数；sat的具体形式为：
[0084]
[0085]
此处，其中，为负载质量的估计值自适应迭代的阈值；
[0086]
负载质量更新公式：其中，公式左侧的为新的其中，负载质量的估计值的更新频率设置为7.69hz。
[0087]
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可作出的各种修改或变形仍在本发明的保护范围以内。