CN114179088A - 机器人负载补偿实现方法、装置及机器人 - Google Patents

Abstract

本申请实施例提供一种机器人负载补偿实现方法、装置及机器人，该方法包括：获取机器人各个关节的状态信息，其中，各个关节中包括由相应多维力传感器的安装部位等效得到的虚拟关节，且虚拟关节的受力状态信息通过相应多维力传感器直接测量得到；根据各个关节的状态信息基于包含虚拟关节在内构建的动量观测模型进行多维力传感器负载补偿，得到广义关节外力的估计值，进而获得机器人与环境的实际接触力以对机器人进行相应控制。本方法可以实现对机器人本体上位于不同安装部位的力传感器的负载补偿，以获得机器人与环境的真实接触外力用以机器人控制。

Description

机器人负载补偿实现方法、装置及机器人

技术领域

本申请涉及机器人控制技术领域，尤其涉及一种机器人负载补偿实现方法、装置及机器人。

背景技术

目前如六维力传感器等力传感器广泛安装于机器人本体上实现机器人与环境接触时的外力感知，以用于机器人安全协作和柔顺交互的力控制等。但无论是安装于机器人操作末端还是机器人底座等位置的力传感器，都会受到其连接的负载运动带来的动态效应的影响，也就是说由于负载的运动惯性存在，即使机器人没有与环境接触，该力传感器仍然会有动态变化的读数。若直接将该读数用于后续的力控制，会带来较大的误差。

然而在现有的解决方案中，不仅需要通过建立负载的动态方程，其中，随着安装部位的变化，动态方程的复杂度也随之变化，还需要计算传感器安装部位的加速度信息，而加速度信息往往无法直接获得。采用数值微分计算加速度，会放大数据噪声；而采用IMU(惯性测量单元)等传感器进行测量，又会增加成本，还需要改变机器人结构和增加数据处理环节等。可知，现有的解决方案在实际应用过程中仍存在很多的局限性。

发明内容

本申请实施例提供一种机器人负载补偿实现方法、装置及机器人，可以实现对机器人本体上位于不同安装部位的力传感器的负载补偿，而不需要计算加速度信息等，从而获得机器人与环境的真实接触外力，为机器人的力控制和安全协作等提供准确的感知信息等。

第一方面，本申请实施例提供一种机器人负载补偿实现方法，包括：

获取机器人各个关节的状态信息，其中，所述各个关节中包括由相应多维力传感器的安装部位等效得到的虚拟关节，所述虚拟关节的受力状态信息通过相应多维力传感器直接测量得到；

根据各个关节的所述状态信息基于包含所述虚拟关节在内构建的动量观测模型进行多维力传感器负载补偿，得到广义关节外力的估计值；

根据所述广义关节外力的估计值获得所述机器人与外界环境的实际接触力，并根据所述实际接触力对所述机器人进行相应控制。

在一些实施例中，所述动量观测模型通过预先构建得到，包括：

将机器人中各个所述多维力传感器的安装部位分别等效为具有相应自由度且位置固定的所述虚拟关节；

根据包含所述虚拟关节在内的所有关节的状态参数，构建所述机器人的增广动力学方程；

根据所述机器人的惯量矩阵和所有关节的关节速度，构建所述机器人的广义动量方程；

根据所述增广动力学方程和所述广义动量方程，构建动量观测模型，所述动量观测模型用于对所述机器人中的各个多维力传感器进行负载补偿。

在一些实施例中，所述虚拟关节的自由度与对应安装部位的所述多维力传感器的测量维度相等。

在一些实施例中，所述多维力传感器为六维力传感器，所述虚拟关节的自由度为六。

在一些实施例中，所述状态参数包括相应关节的运动状态参数和受力状态参数，所述根据包含所述虚拟关节在内的所有关节的状态参数，构建所述机器人的增广动力学方程，包括：

根据所述多维力传感器安装的总数量和每个所述多维力传感器的测量维度，确定所述机器人中的虚拟关节的总自由度，所述机器人中的所述虚拟关节的总自由度和真实关节的总自由度用于确定所述机器人的总自由度；

根据所有关节的运动状态参数，分别构建包含所述虚拟关节在内的增广惯量矩阵、增广科氏力和离心力矩阵、以及增广重力矢量；

将所述机器人的真实关节的驱动力矩映射到包含所述虚拟关节在内的广义关节力矩，得到第一映射矩阵，其中，所述广义关节力矩的维度等于所述机器人的总自由度；

将所述虚拟关节的受力映射到所述广义关节力矩，得到第二映射矩阵；

根据所述第二映射矩阵和所有关节的关节速度，建立所述虚拟关节的约束等式；

基于所有关节的运动状态参数和受力状态参数、所述第一映射矩阵、所述第二映射矩阵和所述约束等式，得到所述机器人的增广动力学方程。

在一些实施例中，所述根据所述增广动力学方程和所述广义动量方程，构建动量观测模型，包括：

根据所述增广惯量矩阵、所述增广科氏力和离心力矩阵、以及所述增广重力矢量，构建所述动量观测模型的状态矩阵；

根据所述机器人的真实关节的驱动力矩和各个多维力传感器的力矩测量值，构建所述动量观测模型的观测力矩；

根据所述状态矩阵、所述观测力矩和所述机器人的广义动量的导数，构建所述动量观测模型的表达式，其中，所述动量观测模型的输出量被定义为所述广义关节外力的估计值。

在一些实施例中，所述增广动力学方程的表达式如下：

其中，

式中，

和

分别表示包含所述虚拟关节在内的增广关节位置、增广关节速度和增广关节加速度；

分别表示所述增广惯量矩阵、所述增广科氏力和离心力矩阵、以及所述增广重力矢量；H表示所述第一映射矩阵；τ表示所述真实关节的驱动力矩；

表示广义关节外力；λ表示拉格朗日乘子矢量，

表示所述第二映射矩阵。

在一些实施例中，所述动量观测模型的表达式如下：

其中，

式中，

表示所述广义动量的导数

的观测值，

表示所述观测力矩，

和

分别表示包含所述虚拟关节在内的增广关节位置和增广关节速度，

表示所述增广惯量矩阵的导数，

表示所述增广科氏力和离心力矩阵，

表示所述增广重力矢量，

表示所述动量观测模型的输出值

的导数，

表示所述动量观测模型的增益矩阵。

第二方面，本申请实施例提供一种机器人负载补偿实现装置，包括：

获取模块，用于获取机器人各个关节的状态信息，其中，所述各个关节中包括由相应多维力传感器的安装部位等效得到的虚拟关节，所述虚拟关节的受力状态信息通过相应多维力传感器直接测量得到；

补偿模块，用于根据各个关节的所述状态信息基于包含所述虚拟关节在内构建的动量观测模型进行多维力传感器负载补偿，得到广义关节外力的估计值；

控制模块，用于根据所述广义关节外力的估计值获得所述机器人与外界环境的实际接触力，并根据所述实际接触力对所述机器人进行相应控制。

第三方面，本申请实施例提供一种机器人，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的机器人负载补偿实现方法。

第四方面，本申请实施例提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上执行时，实施上述的机器人负载补偿实现方法。

本申请的实施例具有如下有益效果：

本申请实施例的机器人负载补偿实现方法通过预先将机器人中的各个多维力传感器的安装部位分别等效为具有相应自由度的虚拟关节；并根据包含虚拟关节在内的所有关节构建增广的动力学方程及广义动量方程，以进一步构建得到能够用于负载补偿的动量观测模型，由于该动量观测模型充分考虑了多维力传感器的安装部位的状态信息，因此在实际运动过程中，通过该动量观测模型进行实时外力估计时，可以补偿与力传感器连接的负载的运动所带来的动态效应，从而获得机器人与环境的真实接触外力，而这真实接触外力可为后续用于机器人的如碰撞检测、与外界的交互等控制过程提供准确的感知信息，以保证控制的可靠性。此外，该方法不需要计算加速度等信息及复杂运算，且对所有的机器人系统具有普适性，易于推广应用。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本申请实施例的机器人的一种结构示意图；

图2示出了本申请实施例的将六维力传感器安装在底座的示意图；

图3示出了本申请实施例的机器人负载补偿实现方法的第一流程图；

图4示出了本申请实施例的机器人的动量观测模型构建的流程图；

图5示出了本申请实施例的机器人的增广动力学方程的构建的流程图；

图6示出了本申请实施例的动量观测模型构建的另一流程图；

图7示出了本申请实施例的机器人负载补偿实现装置的结构示意图；

图8示出了本申请实施例的一种负载补偿模型构建模块的结构示意图。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

在下文中，可在本申请的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本申请的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本申请的各种实施例中被清楚地限定。

在机器人运动过程中，由于机器人末端所持有的物体或其底座连接的结构等存在运动惯性，导致机器人本体上的力传感器在没有与外界接触时仍会有动态变化的读数。为此，本申请实施例提出一个统一的多维力传感器补偿框架，其通过将安装在机器人本体上的多维力传感器的安装部位等效为位置固定的虚拟关节，并基于虚拟关节及机器人的真实关节来构建机器人增广的动力学方程，进而结合动量观测来得到对各多维力传感器进行负载补偿后的真实外力估计，该方法不仅可以避免加速度信息的计算，也不需要增加额外的器件以改变机器人的结构等。

下面结合附图，对本申请的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互结合。

请参照图1，其为本申请实施例提供的机器人10的一种结构示意图。在本实施例中，该机器人10包括处理器11、存储器12和感知单元13等，其中，存储器12和感知单元13均与处理器11连接，存储器12存储有相应的计算机程序，处理器11用于执行该计算机程序以实施本申请实施例中的机器人负载补偿实现方法，以获得机器人与环境的真实接触外力，进而为机器人的力控制和安全协作等提供准确的感知信息等。

其中，处理器11可以是一种具有信号的处理能力的集成电路芯片。处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、图形处理器(GraphicsProcessing Unit，GPU)及网络处理器(Network Processor，NP)、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件中的至少一种。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。

其中，存储器12可以但不限于是，随机存取存储器(Random Access Memory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(Programmable Read-OnlyMemory，PROM)，可擦除只读存储器(Erasable Programmable Read-Only Memory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory，EEPROM)等。其中，存储器用于存储计算机程序，处理器在接收到执行指令后，可相应地执行计算机程序。

其中，感知单元13主要指在机器人身上设置的传感器，这些传感器用于将必要的外部信息以及自身状态信息传递给机器人的控制系统，从而为机器人的决策提供必要的条件。在一些情况下，这些传感器可分为内部传感器和外部传感器，其中，内部传感器用于测量机器人自身状态的功能元件，其功能是测量运动学量和力学量，用于机器人感知自身的运动状态，使得机器人可以按照规定的位置、轨迹和速度等参数运动。例如，内部传感器包括但不限于，设于相应关节或躯干处的位置或角度传感器、六维力传感器、压力传感器等。而外部传感器主要是感知机器人自身和环境之间的相互信息，例如包括视觉等信息，具体可包括但不限于接近传感器、视觉传感器等。应当明白的是，上述列举的这些传感器可根据机器人的实际应用场景而选择性设置。

在本申请实施例中，该机器人10可以如仿人机器人、三足或三足以上的机器人、工业用机械臂、单机械臂手术机器人、或多机械臂的手术机器人等，这些机器人可在相应的底座和/或末端等位置安装有至少一个多维力传感器，如三维力传感器或六维力传感器等，以用于感知与外界环境的接触情况，对于安装的这些多维力传感器，均可以采用本申请实施例的负载补偿方法来补偿负载的动态效应，以获得机器人与环境的真实接触外力，进而可实现如碰撞检测、交互力控制等的精准控制。

基于上述的机器人结构，下面对本申请实施例的机器人负载补偿实现方法进行具体阐述。这里先对本申请实施例中提及的相关术语进行说明。

真实关节，是一种用于构成机械臂的真实存在的关节，其需要通过关节电机进行驱动以产生相应的力或力矩。通常地，真实关节的数量与相应机械臂的自由度相等，例如，一个机械臂的自由度为3时，则表明该机械臂包括3个真实关节；而一个冗余机械臂的自由度为6，则表明该冗余机械臂包括6个真实关节等。

虚拟关节，与上述的真实关节不同，其不是真实存在的关节，可视为一种等效关节，只产生运动而不能输出实际的力或力矩。在本申请实施例中，为实现负载补偿，将一个多维力传感器的安装部位等效为一个具有相应自由度的串联型虚拟固定关节，以便将多维力传感器融入机器人本体结构，这样在进行实时外力估计时，可以将多维力传感器连接的负载的动态影响考虑在内。可以理解，该串联型虚拟固定关节由多个位置固定的虚拟关节串联形成，其中，这里的位置固定是指该虚拟关节不会与相邻连接的虚拟关节或真实关节产生相对运动。并且，该串联型虚拟固定关节的自由度与对应安装部位的多维力传感器的测量维度相等。例如，若该多维力传感器为六维力传感器，则该串联型虚拟固定关节的自由度为6，如图2所示，可将一六维力传感器安装在一机械臂的底座位置，则可将该底座等效为一个六自由度的虚拟关节。又或者，若该多维力传感器为三维力传感器，则该串联型虚拟固定关节的自由度为3等。

此外，对于本申请实施例中出现的诸如增广动力学方程、广义动量方程、增广惯量矩阵、增广科氏力和离心力矩阵、以及增广重力矢量、增广关节位置、增广关节速度、增广关节加速度等，其中的“广义”或“增广”的描述，均表示其是基于包含机器人的真实关节和等效的虚拟关节在内的所有关节而构建得到的，以此区分仅考虑机器人的真实关节而构建得到的动力学方程、动量方程、惯量矩阵、科氏力和离心力矩阵、重力矢量、关节位置、关节速度、关节加速度等。

请参照图3，为本申请实施例的机器人负载补偿实现方法的第一流程图。示范性地，该机器人负载补偿实现方法包括步骤S110～S130：

S110，获取机器人各个关节的状态信息，其中，各个关节中包括由相应多维力传感器的安装部位等效得到的虚拟关节，而虚拟关节的受力状态信息通过相应多维力传感器直接测量得到。

其中，上述的多维力传感器是一种安装在机器人本体上的感知单元，例如，可以是三维力传感器、六维力传感器等，用于在安装部位进行力或力矩的测量。可以理解的是，当多维力传感器安装在机器人的不同部位时，其可以对机器人与外界环境是否接触的感知范围及负载情况也会存在差异。

例如，若一多维力传感器位于机械臂的腕关节处，则其可检测从机器人的腕关节到末端执行器这一区域是否与外界环境存在接触，此时的负载主要指外界施加的外力等。若一多维力传感器位于机械臂的肩关节处，则其可检测从机器人的大臂、小臂以及末端执行器整个区域是否与外界环境存在接触，此时的负载可包括从底座到机械臂末端的所有关节连杆以及末端所持物体等。因此，为对设置的每个多维力传感器进行相应的负载补偿，在本申请实施例中，可将每个多维力传感器的安装部位等效为一具有相应自由度的串联型虚拟关节。

在本实施例中，该多维力传感器的安装部位并不限定，具体可根据实际运用场景来选取，例如，可以是如机器人的底座处、或末端等任何可能与外界接触的部位。具体地，在某种应用场景中，机器人的底座与移动小车上的支撑平台连接，则可在底座处设置一个多维力传感器；又例如，对于工业机械臂、手术机器人而言，在末端需要进行物体夹持的情况下，同样可在末端处设置相应的多维力传感器，用以感知外界接触时的作用力。

在本实施例中，上述的各个关节包括该机器人的所有真实关节以及由相应多维力传感器的安装部位等效得到的所有虚拟关节。其中，每个关节的状态信息包括该关节的运动状态信息以及受力状态信息，例如，关节的运动状态信息可包括但不限于包括关节的位置、速度、加速度等中的一种或多种，而受力状态信息可包括关节受到的力、力矩等中的一种或组合。可以理解，这里获取的每个关节的状态信息与后文中描述的用于构建动量观测模型的各个关节的状态参数是相对应的。

其中，各个真实关节的状态信息可通过机器人的相应感知单元直接测量或间接计算得到，例如，对于真实关节的关节位置，可采用如位置编码器、位移传感器等直接测量得到，进而，通过微分处理，可得到关节速度以及关节加速度等。而对于真实关节的电机驱动力矩，可通过如电机电流计算得到或设于相应关节处的力矩传感器等测量得到，这里不作限定。

而对于各个虚拟关节的状态参数，主要与对应多维力传感器的安装部位有关，例如，当机器人的底座安装有一多维力传感器时，则该底座等效的各个虚拟关节的受力状态信息可根据该多维力传感器直接测量得到的力或力矩示数获得，而其运动状态信息可根据该底座在机器人模型中的表示来计算得到。例如，若多维力传感器设置机械臂的末端，则该末端等效得到的各个虚拟关节的运动状态与在对应虚拟关节之前依次连接的所有关节的运动状态有关。同理，若机器人的底座安装有一多维力传感器，则与该虚拟关节之后依次连接的所有关节的运动状态有关。

举例说明，对于一个六自由度的机械臂，若一个六维力传感器设置在末端，则将该末端等效为6个串联的虚拟关节，其中，对于虚拟关节1的关节位置，可根据从其之前连接的6个真实关节的关节位置通过链乘法则计算得到，而对于虚拟关节2的关节位置，则可根据之前连接的6个真实关节和虚拟关节1的关节位置共同计算得到，如此类推，从而可以计算出每个虚拟关节的关节位置。

S120，根据各个关节的状态信息基于包含虚拟关节在内构建的动量观测模型进行多维力传感器负载补偿，得到广义关节外力的估计值。

其中，动量观测模型用于对机器人中的各个多维力传感器进行负载补偿，并计算得到负载补偿后的实时外力估计。可以理解，该动量观测模型基于各个关节的状态参数预先构建得到，示范性地，对于上述步骤S120，通过将获得的各个关节的状态信息代入预先构建的动量观测模型的表达式中，从而得到该动量观测模型的输出值，即上述的广义关节外力的估计值。其中，该广义关节外力是指包含虚拟关节在内的各个关节受到的外力或外力矩。

在一种实施方式中，如图4所示，上述的动量观测模型可通过如下方式预先构建得到，包括步骤S210～S230：

S210，将机器人中的各个多维力传感器的安装部位分别等效为具有相应自由度且位置固定的虚拟关节。

在本实施例中，每个安装部位均可等效为一个串联型虚拟关节，且这些虚拟关节的关节位置是固定的。为了方便理解，这里主要以六维力传感器进行示例说明。其中，六维力传感器，又称六轴力传感器、六向力传感器或六维力/力矩传感器，是一种可以同时检测X、Y和Z方向上的3个力分量和3个力矩分量的力传感器。具体地，某一安装部位等效的串联型虚拟关节具体包括3个虚拟的平动关节和3个虚拟的转动关节，这些虚拟关节之间的位置是固定的。

S220，根据包含虚拟关节在内的所有关节的状态参数，构建机器人的增广动力学方程。

本实施例中，该增广动力学方程是指进行关节增广后的机器人的刚体动力学方程。依据机器人的刚体动力学原理，在一种实施方式中，如图5所示，该机器人的增广动力学方程的具体构建包括子步骤S221～S225：

S221，根据多维力传感器安装的总数量和每个多维力传感器的测量维度，确定机器人中的虚拟关节的总自由度。其中，机器人中的虚拟关节的总自由度和真实关节的总自由度用于确定机器人的总自由度。

其中，机器人的自由度与关节数量相等。不同自由度的关节所能实现的功能可能不同，对于存在冗余的自由度的机器人，其往往可以实现更多的任务。本实施例中，通过确定等效的所有虚拟关节的数量，并结合真实关节的数量，可以确定进行关节增广后的机器人的总关节数量，以便构建相应维度的矩阵。

例如，若机器人的真实关节的自由度为n，其末端安装了一个六维力传感器，则进行关节扩展后的该机器人的总自由度为n+6，即将其末端位置等效为一个6自由度的串联型虚拟关节。进一步地，若在底座同时也有安装一六维力传感器，则该机器人的总自由度为n+12。

S222，根据所有关节的运动状态参数，分别构建包含虚拟关节在内的增广惯量矩阵、增广科氏力和离心力矩阵、以及增广重力矢量。

其中，惯量矩阵用于描述机器人刚体的旋转惯性，其与各个关节的关节位置有关。可知，在已知包含虚拟关节在内的各个关节的关节位置的情况下，可以计算出对应的增广惯量矩阵，这里记为

其中，

为机器人的总自由度，

表示包含虚拟关节在内的各个关节的关节位置。

而科氏力和离心力矩阵是一个非线性项矩阵，可根据各个关节的关节位置、关节速度等运动状态信息计算得到，这里记为

重力矢量由各个关节连杆受到的重力构成的

维列向量，这里记为

S223，将机器人的真实关节的驱动力矩映射到包含虚拟关节在内的广义关节力矩，得到第一映射矩阵。

其中，广义关节力矩的维度等于该机器人的总自由度。在本申请实施例中，由于真实关节的驱动力矩的维度与机器人的总自由度不相等，为方便进行矩阵运算，这里将通过一映射矩阵

将真实关节的驱动力矩转换到包含虚拟关节在内的广义关节力矩，其中，n为机器人的真实关节的总自由度，且

k为虚拟关节的总自由度。

S224，将虚拟关节的受力映射到广义关节力矩，得到第二映射矩阵，并根据第二映射矩阵和所有关节的关节速度，建立虚拟关节的约束等式。

同理，对于上述的虚拟关节的受力，需要将其映射到广义关节力矩，于是依据拉格朗日算法构建动力学方程，则通过该虚拟关节的速度雅克比矩阵将其映射到广义关节力矩，此时的速度雅克比矩阵即作为上述的第二映射矩阵，这里记为

可以理解，对于本申请实施例中的各个虚拟关节，由于安装部位是固定的，故这些虚拟关节的位置也是固定的，相应地，可建立如下约束条件：

式中，

表示虚拟关节的速度雅克比矩阵，即第二映射矩阵；

表示包含虚拟关节在内的各个关节的速度，也称增广关节速度。

S225，基于所有关节的运动状态参数和受力状态参数、第一映射矩阵、第二映射矩阵和约束等式，得到机器人的增广动力学方程。

基于上述的各个矩阵及各个关节的状态参数，可建立如下增广动力学方程：

其中，

式中，

和

分别表示包含虚拟关节在内的增广关节位置、增广关节速度和增广关节加速度；H表示第一映射矩阵；τ表示真实关节的驱动力矩；

表示广义关节外力；λ表示拉格朗日乘子矢量。

例如，以底座和末端均安装有一六维力传感器为例，上述的

具体可表示为

q_b,q_e∈R⁶分别表示底座和末端虚拟关节的关节位置；而

具体可表示为

分别表示底座和末端与环境接触的力和力矩。

可以理解，通过上式将相应的多维力传感器融合进机器人的刚体动力学模型，得到统一的力传感器负载补偿框架。在实际应用时，可灵活地调整机器人本体安装的多维力传感器的个数和安装部位，这里不作限定。

S230，根据机器人的惯量矩阵和所有关节的关节速度，构建机器人的广义动量方程。其中，该广义动量方程是指包含虚拟关节在内构建得到的动量方程。示范性地，该广义动量方程

可表示为：

S240，根据该增广动力学方程和广义动量方程，构建动量观测模型，以用于对机器人中的各个多维力传感器进行负载补偿。

在得到增广动力学方程和广义动量方程后，可联立两个方程，构建出该机器人的动量观测模型。在一种实施方式中，如图6所示，步骤S240包括子步骤S241～S243：

S241，根据增广惯量矩阵、增广科氏力和离心力矩阵、以及增广重力矢量，构建动量观测模型的状态矩阵。

具体地，该动量观测模型的状态矩阵

的表达式如下：

S241，根据机器人的真实关节的驱动力矩和各个多维力传感器的力矩测量值，构建动量观测模型的观测力矩。

具体地，可驱动力矩和多维力传感器的力矩测量值构建得到观测力矩

可以理解，这里的力矩测量值即为相应力传感器直接读取到的示数。

例如，以底座和末端安装有一六维力传感器为例，则有：

其中，τ_b,τ,τ_e分别表示机器人底座的多维力传感器的力矩测量值、机器人本体的关节力矩、以及末端的多维力传感器的力矩测量值。

S243，根据状态矩阵、观测力矩和机器人的广义动量的导数，构建动量观测模型的表达式。其中，动量观测模型的输出量被定义为广义关节外力的估计值。

示范性地，该动量观测模型的表达式如下：

式中，

表示广义动量的导数

的观测值，

表示增广惯量矩阵的导数，

表示动量观测模型的输出值

的导数。

进一步地，可根据上述动量观测模型的表达式进行变换及离散化处理，得到该动量观测模型的输出值

的表达式，如下：

式中，

和

表示初始时刻和t时刻的广义动量。本实施例中，动量观测模型的输出值

被定义为广义关节外力

的估计值。

可以理解，由于该动量观测模型将多维力传感器融入，即充分考虑了多维力传感器的安装部位的状态信息，因此，在该动量观测模型的计算过程中，即补偿了与力传感器连接的负载的运动所带来的动态效应，此时输出的即为进行负载补偿后的机器人与环境的真实接触外力。

S130，根据广义关节外力的估计值获得机器人与外界环境的实际接触力，并根据该实际接触力对机器人进行相应控制。

其中，计算得到的动量观测模型的输出值

即为广义关节外力的估计值，由广义关节外力的定义可知，其包括了各个安装部位与环境接触的力和力矩。示范性地，可从计算的该广义关节外力的估计值中得到相应多维力传感器经负载补偿后的实际读数，并将该实际读数作为机器人与外界接触的实际接触力。例如，仍以上述的底座和末端为例，则可从输出值

中获得底座和末端的六维力传感器经过负载补偿后的实际读数

和

而该实际读数

和

即分别为机器人底座和末端与环境的实际接触力。

最后，可根据应用场景，将该实际接触力用于后续的机器人控制中，例如，可包括但不限为，进行精准力控制、柔顺交互、碰撞检测等。例如，以碰撞检测为例，若机器人的末端设有一六维力传感器，在利用本实施例的方法计算得到该末端的六维力传感器与环境的实际接触力

后，可判断该实际接触力是否大于或等于一个预设阈值，若大于或等于，则表明发生碰撞，若小于该预设阈值，则表明未发生碰撞。

本申请实施例中，通过将机器人中的各个多维力传感器的安装部位分别等效为具有相应自由度且位置固定的虚拟关节；并根据包含虚拟关节在内的所有关节构建增广的动力学方程及广义动量方程，以构建得到能够用于负载补偿的动量观测模型。通过该动量观测模型可以补偿与力传感器连接的负载因运动惯性带来的动态效应，从而获得机器人与环境的真实接触外力，这为机器人的力控制和安全协作等控制提供了准确的感知信息，从而保证控制系统的可靠性等。

请参照图7，基于上述实施例的机器人稳定控制方法，本申请实施例还提出一种机器人稳定控制装置100，下面对本申请实施例提供的机器人稳定控制装置100的具体组成进行相应描述。

获取模块110，用于获取机器人各个关节的状态信息，其中，各个关节中包括由相应多维力传感器的安装部位等效得到的虚拟关节，而虚拟关节的受力状态信息通过相应多维力传感器直接测量得到。

补偿模块120，用于根据各个关节的状态信息基于包含虚拟关节在内构建的动量观测模型进行多维力传感器负载补偿，得到广义关节外力的估计值；

控制模块130，用于根据广义关节外力的估计值获得所述机器人与外界环境的实际接触力，并根据该实际接触力对机器人进行相应控制。

作为一种可选的方案，该机器人稳定控制装置100还可包括模型构建模块140，用于构建上述的动量观测模型。如图8所示，该模型构建模块140包括虚拟关节等效子模块141、动力学方程构建子模块142、动量方程子模块143和观测模型构建子模块144。当然，上述的动量观测模型也可以是预先构建得到并存储在补偿模块120中以便直接调用，这里不作限定。

具体地，虚拟关节等效子模块141用于将机器人中的各个多维力传感器的安装部位分别等效为具有相应自由度且位置固定的虚拟关节。动力学方程构建子模块142用于根据包含虚拟关节在内的所有关节的状态参数，构建机器人的增广动力学方程。动量方程子模块143用于根据机器人的惯量矩阵和所有关节的关节速度，构建机器人的广义动量方程。观测模型构建子模块144用于根据增广动力学方程和广义动量方程，构建机器人的动量观测模型。本申请实施例中，该动量观测模型用于对机器人中的各个多维力传感器进行负载补偿。

可以理解，本实施例的机器人负载补偿实现装置对应于上述实施例的机器人负载补偿实现方法，上述实施例中的可选项同样适用于本实施例，故在此不再重复描述。

可选地，本申请还提供了一种终端设备，如计算机等，示范性地，该终端设备包括处理器和存储器，其中，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使终端设备执行上述模型构建模块中各个子模块的功能。

本申请还提供了一种可读存储介质，用于储存上述机器人或上述终端设备中使用的所述计算机程序。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。

Claims (11)

1.一种机器人负载补偿实现方法，其特征在于，包括：

获取机器人各个关节的状态信息，其中，所述各个关节中包括由相应多维力传感器的安装部位等效得到的虚拟关节，所述虚拟关节的受力状态信息通过相应多维力传感器直接测量得到；

根据各个关节的所述状态信息基于包含所述虚拟关节在内构建的动量观测模型进行多维力传感器负载补偿，得到广义关节外力的估计值；

根据所述广义关节外力的估计值获得所述机器人与外界环境的实际接触力，并根据所述实际接触力对所述机器人进行相应控制。

2.根据权利要求1所述的机器人负载补偿实现方法，其特征在于，所述动量观测模型通过预先构建得到，包括：

将机器人中各个所述多维力传感器的安装部位分别等效为具有相应自由度且位置固定的所述虚拟关节；

根据包含所述虚拟关节在内的所有关节的状态参数，构建所述机器人的增广动力学方程；

根据所述机器人的惯量矩阵和所有关节的关节速度，构建所述机器人的广义动量方程；

根据所述增广动力学方程和所述广义动量方程，构建动量观测模型，所述动量观测模型用于对所述机器人中的各个多维力传感器进行负载补偿。

3.根据权利要求2所述的机器人负载补偿实现方法，其特征在于，所述虚拟关节的自由度与对应安装部位的所述多维力传感器的测量维度相等。

4.根据权利要求3所述的机器人负载补偿实现方法，其特征在于，所述多维力传感器为六维力传感器，所述虚拟关节的自由度为六。

5.根据权利要求2至3中任一项所述的机器人负载补偿实现方法，其特征在于，所述状态参数包括相应关节的运动状态参数和受力状态参数，所述根据包含所述虚拟关节在内的所有关节的状态参数，构建所述机器人的增广动力学方程，包括：

根据所述多维力传感器安装的总数量和每个所述多维力传感器的测量维度，确定所述机器人中的虚拟关节的总自由度，所述机器人中的所述虚拟关节的总自由度和真实关节的总自由度用于确定所述机器人的总自由度；

根据所有关节的运动状态参数，分别构建包含所述虚拟关节在内的增广惯量矩阵、增广科氏力和离心力矩阵、以及增广重力矢量；

将所述机器人的真实关节的驱动力矩映射到包含所述虚拟关节在内的广义关节力矩，得到第一映射矩阵，其中，所述广义关节力矩的维度等于所述机器人的总自由度；

将所述虚拟关节的受力映射到所述广义关节力矩，得到第二映射矩阵，并根据所述第二映射矩阵和所有关节的关节速度，建立所述虚拟关节的约束等式；

基于所有关节的运动状态参数和受力状态参数、所述第一映射矩阵、所述第二映射矩阵和所述约束等式，得到所述机器人的增广动力学方程。

6.根据权利要求5所述的机器人负载补偿实现方法，其特征在于，所述根据所述增广动力学方程和所述广义动量方程，构建动量观测模型，包括：

根据所述增广惯量矩阵、所述增广科氏力和离心力矩阵、以及所述增广重力矢量，构建所述动量观测模型的状态矩阵；

根据所述机器人的真实关节的驱动力矩和各个多维力传感器的力矩测量值，构建所述动量观测模型的观测力矩；

根据所述状态矩阵、所述观测力矩和所述机器人的广义动量的导数，构建所述动量观测模型的表达式，其中，所述动量观测模型的输出量被定义为所述广义关节外力的估计值。

7.根据权利要求5所述的机器人负载补偿实现方法，其特征在于，所述增广动力学方程的表达式如下：

其中，

式中，

和

分别表示包含所述虚拟关节在内的增广关节位置、增广关节速度和增广关节加速度；

分别表示所述增广惯量矩阵、所述增广科氏力和离心力矩阵、以及所述增广重力矢量；H表示所述第一映射矩阵；τ表示所述真实关节的驱动力矩；

表示广义关节外力；λ表示拉格朗日乘子矢量，

表示所述第二映射矩阵。

8.根据权利要求6所述的机器人负载补偿实现方法，其特征在于，所述动量观测模型的表达式如下：

其中，

式中，

表示所述广义动量的导数

的观测值，

表示所述观测力矩，

和

分别表示包含所述虚拟关节在内的增广关节位置和增广关节速度，

表示所述增广惯量矩阵的导数，

表示所述增广科氏力和离心力矩阵，

表示所述增广重力矢量，

表示所述动量观测模型的输出值

的导数，

表示所述动量观测模型的增益矩阵。

9.一种机器人负载补偿实现装置，其特征在于，包括：

获取模块，用于获取机器人各个关节的状态信息，其中，所述各个关节中包括由相应多维力传感器的安装部位等效得到的虚拟关节，所述虚拟关节的受力状态信息通过相应多维力传感器直接测量得到；

补偿模块，用于根据各个关节的所述状态信息基于包含所述虚拟关节在内构建的动量观测模型进行多维力传感器负载补偿，得到广义关节外力的估计值；

控制模块，用于根据所述广义关节外力的估计值获得所述机器人与外界环境的实际接触力，并根据所述实际接触力对所述机器人进行相应控制。

10.一种机器人，其特征在于，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-8中任一项所述的机器人负载补偿实现方法。

11.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上执行时，实施权利要求1-8中任一项所述的机器人负载补偿实现方法。

Images