CN111993416B - 一种控制机械臂运动的方法、设备、系统以及装置 - Google Patents

Abstract

本申请公开了一种控制机械臂运动的方法、设备、系统以及装置，该方法包括：获取机械臂的当前角度与目标角度之间的第一差值；将第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数；获得当前控制参数和第一差值下的控制器所输出的控制指令，并根据控制指令获得机械臂能够运动的角度值；若角度值与第一差值之间的第二差值大于等于阈值，则将角度值输入至神经网络模型中，以调整神经网络模型中的权系数，并返回至将第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数的步骤；否则，按照控制指令控制机械臂运动方法和装置。通过上述方式，本申请能够减少机械臂震荡，控制机械臂稳定运动。

Description

一种控制机械臂运动的方法、设备、系统以及装置

技术领域

本申请涉及自动控制技术领域，特别是涉及一种控制机械臂运动领域。

背景技术

随着科技的发展，机械臂在工业制造、医学治疗、娱乐服务、军事、太空探索以及水下探索等领域均有广泛的应用。尤其在水下探索领域，水下机械臂作为人类开发水下世界的有力工具，正在水下工程等方面发挥着不可替代的作用。在一些情况下，水下机械臂需要携带一些作业工具下水，如检测水下大坝时需要在机械臂安装特定的摄像机用以观察大坝情况；又例如水下考古时也需要用到携带摄像机的水下机械臂。

为了工作或工程的顺利完成，需要确保机械臂的运动的可靠性。本申请的发明人在长期的研发过程中，发现机械臂运动的控制方法是决定机械臂运动可靠性的重要因素。

发明内容

本申请主要解决的技术问题是提供一种控制机械臂运动的方法，能够减少机械臂震荡，控制机械臂稳定运动。

为解决上述技术问题，本申请采用的一个技术方案是：提供一种控制机械臂运动的方法，该方法包括：获取机械臂的当前角度与目标角度之间的第一差值；将第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数；获得当前控制参数和第一差值下的控制器所输出的控制指令，并根据控制指令获得机械臂能够运动的角度值；若角度值与第一差值之间的第二差值大于等于阈值，则将角度值输入至神经网络模型中，以调整神经网络模型中的权系数，并返回至将第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数的步骤；否则，按照控制指令控制机械臂运动。

为解决上述技术问题，本申请采用的另一个技术方案是：提供一种机械臂控制设备，包括：输入单元和处理器；输入单元用于获取机械臂的目标角度；处理器与输入单元耦接，用于接收目标角度，并执行指令以实现上述的控制机械臂运动的方法。

为解决上述技术问题，本发明采用的另一个技术方案是：提供一种机械臂系统，包括上述的机械臂控制设备以及机械臂。

为解决上述技术问题，本发明采用的另一个技术方案是：提供一种具有存储功能的装置，装置存储有程序，程序被执行时能够实现上述的控制机械臂运动的方法。

本申请的有益效果是：区别于现有技术的情况，本申请可以基于神经网络模型获取控制器的控制参数，神经网络可以根据机械臂的实际情况自适应调整权系数，从而使得所获得的控制参数可以更为准确，控制器更加精准。减少了由于控制指令不准确而引起的机械臂震荡，提高了机械臂运动的稳定性。

附图说明

图1是根据本申请一实施方式的控制机械臂运动方法的流程示意图；

图2是根据本申请另一实施方式的控制机械臂运动方法的流程示意图；

图3是根据本申请一实施方式的BP神经网络模型的结构示意图；

图4是根据本申请一实施方式的机械臂动力学模型的构建方法的流程示意图；

图5是根据本申请一实施方式的机械臂模型的示意图；

图6是根据本申请一实施方式的机械臂控制系统的结构示意图；

图7是根据本申请一实施方式的机械臂控制设备的结构示意图；

图8是根据本申请一实施方式的机械臂系统的结构示意图；

图9是根据本申请一实施方式的具有存储功能的装置的结构示意图。

具体实施方式

为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。

本申请的提供一种控制机械臂运动的方法，该方法基于神经网络模型获得控制器的控制参数从而对控制器进行调节，能够实现控制器的控制参数的实时调节，增加了控制精度，使得机械臂可以稳定运动。本申请的实施例可以应用于在任意场景下的控制机械臂运动，例如水下挖掘、水下探测等。应当理解的是，本申请的系统及方法的应用场景仅仅是本申请的一些示例或实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其他类似情景。

参阅图1，图1是根据本申请一实施方式的控制机械臂运动方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图1所示的流程顺序为限。如图1所示，该方法包括：

步骤S110：获取机械臂的当前角度与目标角度之间的第一差值。

在一实施例中，可以获取机械臂经过运动后需要达到的目标角度。计算机械臂当前角度与目标角度之间的第一差值，即机械臂需要运动的角度。具体的，机械臂可以包括多个关节，可以根据机械臂需要完成的工作，确定机械臂需要运动的关节以及各关节需要运动的角度。

在一实施例中，可以基于设置在机械臂上的角度传感器获取机械臂当前的角度。具体的，可以获取机械臂需要运动的关节的当前角度。

步骤S120：将第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数。

在一实施例中，神经网络模型可以是任意适用的神经网络模型。例如：BP神经网络模型，深度学习神经网络模型等。仅为了便于说明，下面以BP神经网络模型为例进行说明。

BP神经网络的权系数可以自适应调整。将第一差值输入BP神经网络模型后，基于BP神经网络模型当前的权系数，可以获得该当前权系数所对应的控制器对应的当前控制参数。BP神经网络模型当前的权系数可以是预设定的初始权系数，也可以是经过预训练后获得的初始权系数，也可以是已经经过若干次调整的权系数。

在一实施例中，控制机械臂的控制器可以是任意使用的控制器。例如，PID控制器，PD控制器等。控制器的控制参数是可以进行调整的。例如，控制器为PID控制器时，控制参数为Kp，Ki，Kd。控制器为PD控制器时，控制参数为Kp，Kd。

步骤S130：获得当前控制参数和第一差值下的控制器所输出的控制指令，并根据控制指令获得机器臂能够运动的角度值。

在一实施例中，将第一差值输入控制器，控制器基于当前控制参数可以输出控制指令。以PID控制器为例，PID控制器的控制算法可以为，如式(1)所示：

u(k)＝u(k-1)+K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)] (1)

其中，u(k)为控制指令，e(k)为第一差值，Kp，Ki，Kd为控制参数，k表述当前时刻，k-1为当前时刻的前一时刻，k-2为前一时刻的前一时刻。即将第一差值e(k)输入PID控制器中，获得控制指令u(k)。

在一实施例中，控制指令u(k)可以用于控制施加在机械臂上的力矩，可以计算机械臂在该力矩下运动的角度值。

在一实施例中，若角度值与第一差值之间的第二差值大于等于第一阈值，则将上述角度值输入至神经网络模型中，以调整神经网络模型中的权系数，并返回至将第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数的步骤。其中，第一阈值可以是固定的，也可以是根据机械臂的情况所确定的。

在一实施例中，若角度值与第一差值之间的第二差值小于第一阈值，则按照上述控制指令控制机械臂运动。

本申请的实施方式中，可以基于神经网络模型获取控制器的控制参数，神经网络可以根据机械臂的实际情况自适应调整权系数，从而使得获得的控制参数可以更为准确，控制器更加精准。减少了由于控制指令不准确而引起的机械臂震荡，提高了机械臂运动的稳定性。

参阅图2，图2是根据本申请另一实施方式的控制机械臂运动方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图2所示的流程顺序为限。如图2所示，该方法包括：

步骤S210：获取机械臂的当前角度与目标角度的第一差值。

在一实施例中，机械臂当前角度可以是初始角度，也可以是指经过运动后所处于的角度。机械臂可以包括多个关节，机械臂的当前角度可以是指需要运动的关节的角度。例如，机械臂可以包括肩关节、大臂关节、小臂关节、肘关节、手腕摆动关节以及手腕旋转关节等。

步骤S220：将第一差值输入输入层。

在一实施例中，神经网络模型为BP神经网络模型，其包括输入层、至少一层隐含层和输出层。例如，BP神经网络模型可以采用三层结构，即输入层，隐含层和输出层，其结构如图3所示。图3是根据本申请一实施方式的BP神经网络模型的结构示意图。如图3所示，BP神经网络模型可以包括m个输入节点310，q各隐含节点320，l个输出节点330。其中输入节点的数量m取决于控制器所控制的系统的复杂度。输出节点分别对应与控制器的参数。例如控制器为PID控制器时，输出节点分别对应于Kp、Ki、Kd，即l＝3。

在一实施例中，可以将当前时刻的第一差值，或基于第一差值获得的其他参数输入输入层。例如，BP神经网络的输入可以表示为如式(2)所示：

其中，e(k-j)为k-j时刻的位置误差，k代表当前时刻。

步骤S230：将输入层的各节点进行加权求和后输入隐含层，并基于隐含层的函数获得隐含层各节点的输出。

具体的，隐含层的输入可以是输入层的各节点的加权求和，如式(3)所示，隐含层的输出如式(4)所示：

其中，

为隐含层权系数，上标的(1)(2)、(3)分别代表输入层、隐含层、输出层，f(x)为隐含层函数。例如，隐含层函数可以为双曲正切函数，即如式(5)所示：

f(x)＝(e^x-e^-x)/(e^x+e^-x)(5)

步骤S240：将隐含层各节点的输出进行加权求和后输入输出层，并基于输出层的函数获得控制机械臂的控制器对应的当前控制参数。

具体的，输出层三个节点的输入、输出分别如式(6)、(7)所示：

其中，

为输出层权系数，g(x)为输出层的函数。由于控制器参数不能为负，输出层的函数可以为非负的simoid函数。例如，输出层的函数可以为，如式(8)所示：

g(x)＝e^x/(e^x+e^-x) (8)

例如，在控制器为PID控制器时，输出如下式(9)所示：

步骤S250，获得当前控制参数和第一差值下的控制器所输出的控制指令，并根据控制指令获得机器臂能够运动的角度值。

步骤S260：获取第二差值。

在一实施例中，获取所能够运动的角度值与目标角度值之间差值，以此为第二差值。

步骤S270：判断第二差值是否大于等于第一阈值。

在一实施例中，若为是，则进行步骤S270调整神经网络模型中的权系数。在一实施例中，若为否，则进行步骤S280按照控制指令控制机械臂运动。

步骤S280：调整神经网络模型中的权系数。

在一实施例中，可以利用第二差值和预设性能指标函数获得性能指标E(k)。具体的，预设性能指标函数可以如式(10)所示：

其中，r(k)为第一差值，y(k)为运动角度值。

在一实施例中，基于性能指标以及按照梯度下降法修正BP神经网络模型中的各个权系数。具体地，以E(k)对权系数的负梯度方向为搜索方向进行搜索调整，并附加一使搜索快速收敛全局极小的惯性项，如式(11)所示：

其中，η为学习率，γ为惯性系数。其中，学习率和惯性系数可以是根据情况预设定的。

在一实施例中，

可以由式(12)获得：

其中，式(12)中的

为变量，可以近似用

取代，由此带来计算不精确的影响可以通过调整学习率η来补偿。

如下式(13)所示：

从而，可以得到BP神经网络输出层权系数的计算公式为，如式(14)、(15)所示：

同理可得隐含层权系数的计算公式为，如式(16)、(17)所示：

步骤S290：按照控制指令控制机械臂运动。

在一实施例中，可以按照控制指令确定施加在机械臂上的力矩，从而控制机械臂运动。

步骤S2100：接收反馈的机械臂按照控制指令运动后的实际角度。

在一实施例中，可以接收设置在机械臂上的角度传感器所反馈的机械臂运动后的角度信息。

步骤S2110：判断实际角度与目标角度是否相同。

由于机械臂在实际的运动过程中，可以会存在所承受的压力过大，或需要抓起的物体过重等情况，导致机械臂按照控制指令运动后无法达到目标角度。

在一实施例中，若实际角度与目标角度不同，则将实际角度作为当前角度，并返回至获取机械臂的当前角度与目标角度之间的第一差值的步骤。其中，实际角度与目标角度不同可以是指实际角度与目标角度不相同，或不是近似相同；也可以是指实际角度与目标角度之间的差值大于第二阈值。

在一实施例中，若实际角度与目标角度相同，则结束。

本申请的实施方式中，神经网络模型可以基于机械臂的运动情况，根据获得的机械臂运动差值调整权系数，使得控制器在控制机械臂运动时的权系数可以更加准确。

在一实施例中，可以在控制器内部预设机械臂的动力学模型，以使得控制器能够根据动力学模型、当前控制参数和第一差值输出控制指令。具体的动力学模型的构建方法参阅图4，图4是根据本申请一实施方式的机械臂动力学模型的构建方法的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图4所示的流程顺序为限。如图4所示，该方法包括：

步骤S410：对机械臂进行建模，以获得机械臂各关节的坐标矩阵。

在一实施例中，可以按照D-H原则建立各个关节坐标系之间的相应关系。参阅图5，图5是根据本申请一实施方式的机械臂模型的示意图。其中，510为基座，520为肩关节，530为大臂关节，540为小臂关节，550为肘关节，560为手腕摆动关节，570为手腕旋转关节，580为手爪开合关节。其中，图5中各部分的坐标系均为笛卡尔坐标系。

在一实施例中，各关节的坐标变化矩阵可以如下式(18)所示：

其中，i＝0-7，分别代表基座，肩关节，大臂关节，小臂关节，肘关节，手腕摆动关节，手腕旋转关节，手爪开合关节。

步骤S420：获得各关节的角速度雅克比矩阵和线速度雅克比矩阵。

在一实施例中，可以基于式(18)的坐标变化矩阵获得角速度的雅克比矩阵，可以表示为，如式(19)所示：

Jω_i＝[Z₁,...Z_i,0,...],Jω_i∈R^3×6,i＝1...6 (19)

其中，z_i可以是指每个关节的矩阵中前三列的前三个元素的值。

在一实施例中，机械手个关节质心的线速度雅克比矩阵可以为，如式(20)所示：

步骤S430：基于各关节的角速度雅克比矩阵和线速度雅克比矩阵，获得机械臂的总动能。

在一实施例中，机械臂的总动能等于各个关节动能的总和，即运动部件的平均动能和转动动能的总和。具体的，机械臂的总动能如式(21)所示：

步骤S440：基于机械臂各关节的坐标矩阵，获得机械臂的总势能。

在一实施例中，机械臂的重力势能为所有关节的势能之和，如式(22)所示：

其中PC_i为每个关节在世界坐标系中的位置。

步骤S450：基于机械臂的总动能、总势能以及机械臂所承受的压力，构建机械臂的动力学模型。

在一实施例中，可以根据拉格朗日能量法建立机械手的动力学方程，如式(23)所示：

简化后，机械臂的动力学模型可以表示为，如式(24)所示：

其中，

请参阅图6，图6是根据本申请一实施方式的机械臂控制系统的结构示意图。该实施方式中，机械臂控制系统包括获取模块610、神经网络模块620、控制模块630。其中，获取模块610用于获取机械臂的当前角度与目标角度之间的第一差值；神经网络模块620用于将第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数；控制模块630用于获得当前控制参数和第一差值下的控制器所输出的控制指令，并根据控制指令获得机械臂能够运动的角度值，其中，若角度值与第一差值之间的第二差值大于等于阈值，则将角度值输入至神经网络模型中，以调整神经网络模型中的权系数，并返回至将第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数的步骤；否则，按照控制指令控制机械臂运动。

请参阅图7，图7是根据本申请一实施方式的机械臂控制设备的结构示意图。该实施方式中，机械臂控制设备700包括处理器710和输入单元720。处理器710还可以称为CPU(Central Processing Unit，中央处理单元)。处理器710可能是一种集成电路芯片，具有信号的处理能力。处理器710还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器710也可以是任何常规的处理器等。

机械臂控制设备700可以进一步包括存储器(图中未示出)，用于存储处理器710运行所需的指令和数据。

处理器710用于执行指令以实现上述本申请机械臂控制方法任一实施例及任意不冲突的组合所提供的方法。

输入单元720可以用于接收输入的目标角度信息。在一实施例中，输入单元可以包括主手和控制盒。其中，主手设置于控制盒的控制面板上。控制盒可以用于采集主手的角度信号，显示机械臂本体的角度或设置控制系统参数等。主手的关节数以及各关节的运功角度与机械臂本体完全相同。主手各关节处均设置有角度传感器，用于获取主手的角度位置。输入单元可以将所接收的目标角度信息传送至处理器710。

请参阅图8，图8是根据本申请一实施方式的机械臂系统的结构示意图。该实施方式中，机械臂系统800包括机械臂控制设备700和机械臂810。

机械臂810设置有角度传感器和液压系统，其中角度传感器用于获取机械臂本体的角度位置，液压系统用于为机械臂本体运动提供动力，以使得机械臂运动。

请参阅图9，图9是根据本申请一实施方式的具有存储功能的装置的结构示意图。本申请实施例的计算机可读存储介质900存储有指令/程序数据910，该指令/程序数据910被执行时实现本申请机器人地图构建方法任一实施例以及任意不冲突的组合所提供的方法。其中，该指令/程序数据910可以形成程序文件以软件产品的形式存储在上述存储介质900中，以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式方法的全部或部分步骤。而前述的存储介质900包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁碟或者光盘等各种可以存储程序代码的介质，或者是计算机、服务器、手机、平板等终端设备。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (9)

1.一种控制机械臂运动的方法，其特征在于，所述方法包括：

在控制器内部预设机械臂的动力学模型；其中所述机械臂的动力学模型是基于所述机械臂的总动能、总势能以及机械臂所承受的压力构建的；

获取机械臂的当前角度与目标角度之间的第一差值；

将所述第一差值输入至神经网络模型，以获得控制机械臂的所述控制器对应的当前控制参数；其中，所述控制器为PID控制器时，所述控制参数为Kp，Ki，Kd；所述控制器为PD控制器时，所述控制参数为Kp，Kd；

获得当前控制参数和所述第一差值下的所述控制器所输出的控制指令，并根据所述控制指令获得所述机械臂能够运动的角度值；所述控制器根据所述动力学模型、所述当前控制参数和所述第一差值输出所述控制指令；

若所述角度值与所述第一差值之间的第二差值大于等于阈值，则将所述角度值输入至所述神经网络模型中，以调整所述神经网络模型中的权系数，并返回至将所述第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数的步骤；否则，按照所述控制指令控制所述机械臂运动。

2.根据权利要求1所述的方法，其特征在于，所述神经网络模型为BP神经网络模型，所述将所述第一差值输入至神经网络模型，以获得控制机械臂的控制器对应的当前控制参数，包括：

将所述第一差值输入所述BP神经网络模型的输入层；

将所述输入层的各节点进行加权求和后输入所述BP神经网络模型的隐含层，并基于所述隐含层的函数获得所述隐含层各节点的输出；

将所述隐含层各节点的输出进行加权求和后输入所述BP神经网络模型的输出层，并基于所述输出层的函数获得所述控制机械臂的控制器对应的当前控制参数。

3.根据权利要求2所述的方法，其特征在于，所述输出层的函数为非负的sigmoid函数。

4.根据权利要求1所述的方法，其特征在于，所述调整所述神经网络模型中的权系数包括：

利用所述第二差值和预设性能指标函数获得性能指标；

基于所述性能指标以及按照梯度下降法修正所述神经网络模型中的各个权系数。

5.根据权利要求1所述的方法，其特征在于，所述在所述控制器内部预设所述机械臂的动力学模型包括：

对机械臂进行建模，以获得所述机械臂各关节的坐标矩阵；

获得各关节的角速度雅克比矩阵和线速度雅克比矩阵；

基于所述各关节的角速度雅克比矩阵和线速度雅克比矩阵，获得所述机械臂的总动能；

基于所述机械臂各关节的坐标矩阵，获得所述机械臂的总势能；

基于所述机械臂的总动能、总势能以及机械臂所承受的压力，构建所述机械臂的动力学模型。

6.根据权利要求1所述的方法，其特征在于，当所述角度值与所述第一差值之间的所述第二差值在所述阈值内，且按照所述控制指令控制所述机械臂运动后，还包括：

接收反馈的所述机械臂按照所述控制指令运动后的实际角度；

若所述实际角度与所述目标角度不同，则将所述实际角度作为所述当前角度，并返回至获取机械臂的当前角度与目标角度之间的第一差值的步骤。

7.一种机械臂控制设备，其特征在于，包括：

输入单元，用于获取机械臂的目标角度；

处理器，与所述输入单元耦接，用于接收所述目标角度，并执行指令以实现如权利要求1-6任一项所述的控制机械臂运动的方法。

8.一种机械臂系统，其特征在于，包括：

权利要求7所述的机械臂控制设备以及机械臂。

9.一种具有存储功能的装置，其特征在于，所述装置存储有程序，所述程序被执行时能够实现如权利要求1-6任一项所述的控制机械臂运动的方法。

Images