机械臂控制方法、装置、控制终端及存储介质与流程

1.本发明涉及机器人控制领域，尤其涉及一种机械臂控制方法、装置、控制终端及存储介质。


背景技术：

2.在手术机器人实验过程中，通过传统的控制理论对机械臂进行控制，会因为外界的干扰产生响应慢，抖动大，调参多等问题，当机械臂按照预设的轨迹和目标姿态运动时，往往会产生抖动和误差，当机械臂运动时间越长，则误差会越来越大，使得机械臂必须实施校准，增加了计算和调整的步骤，占用了算力。


技术实现要素：

3.第一方面，本技术提供一种机械臂控制方法，包括：获取机械臂当前的状态参数；根据预先设定好的机械臂出力模型，按照设定好的运动轨迹和所述状态参数，获取当前时刻的机械臂输出力，所述机械臂出力模型根据滑膜变结构和机械臂动力模型构建；根据所述输出力，控制所述机械臂按照所述运动轨迹运动。
4.进一步的，还包括：当机械臂运动过程中发生抖动时，根据所述机械臂出力模型，调整输出力，使得所述机械臂的运动姿态恢复稳定。
5.进一步的，所述机械臂出力模型的获取方法包括：根据所述机械臂动力模型和李雅普诺夫方程，建立反馈参数模型，所述反馈参数模型用于消除所述李雅普诺夫方程中的干扰项；根据所述反馈参数模型和所述滑膜变结构的二阶参数，建立所述机械臂出力模型。
6.进一步的，所述机械臂出力模型的表达式为：式中，u为所述机械臂的输出力，f和k为预设系数，e为机械臂关节实际位置和目标位置的偏差值，为机械臂关节实际角速度和目标角速度的偏差值，f为所述反馈参数。
7.进一步的，根据所述机械臂动力模型和李雅普诺夫方程，建立反馈参数模型包括：构建李雅普诺夫方程，计算所述李雅普诺夫方程进行的微分，并根据所述李雅普诺夫方程的微分，建立对应的所述反馈参数模型。
8.其中，所述李雅普诺夫方程的表达式为：；式中v为所述李雅普诺夫方程，是机械臂的惯性矩阵。
9.进一步的，根据所述李雅普诺夫方程的微分，建立对应的所述反馈参数模型包括：
根据科里奥利力和机械臂的惯性矩阵建立所述反馈参数模型；所述反馈参数模型表达式为：式中，是机械臂的惯性矩阵，是科里奥利力和向心力耦合矩阵，是关节角度，是关节角速度，是关节角加速度，t是时间，f是所述反馈参数。
10.进一步的，所述机械臂动力模型的表达式为：式中，是机械臂的惯性矩阵，d为扰动量，是关节角度，是关节角速度，是关节角加速度，是科里奥利力和向心耦合矩阵，是重力项，为机械臂的输出力。
11.第二方面，本技术还提供一种机械臂控制装置，包括：监控模块，用于获取机械臂当前的状态参数；构建模块，用于根据预先设定好的机械臂出力模型，按照设定好的运动轨迹和所述状态参数，获取当前时刻的机械臂输出力，所述机械臂出力模型根据滑膜变结构和机械臂动力模型构建；控制模块，用于根据所述输出力，控制所述机械臂按照所述运动轨迹运动。
12.第三方面，本技术还提供一种控制终端，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行所述的机械臂控制方法。
13.第四方面，本技术还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行所述的机械臂控制方法。
14.本发明实施例公开了一种机械臂控制方法、装置、控制终端及存储介质，该方法包括：获取机械臂当前的状态参数；根据预先设定好的机械臂出力模型，按照设定好的运动轨迹和所述状态参数，获取当前时刻的机械臂输出力，所述机械臂出力模型根据滑膜变结构和机械臂动力模型构建；根据所述输出力，控制所述机械臂按照所述运动轨迹运动。使得机械臂按照模型参数控制机械臂每时每刻的动作，出现了抖动也可以很快恢复稳定，使得整个机械臂的控制过程更加安全，并减少计算，减少了对计算资源的占用，减轻了控制终端的负荷。
附图说明
15.为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
16.图1示出了本技术实施例一种机械臂控制方法流程示意图；图2示出了本技术实施例机械臂角速度仿真曲线图；图3示出了本技术实施例机械臂角速度仿真放大图；图4示出了本技术实施例又一机械臂角速度仿真放大图；
图5示出了本技术实施例一种机械臂控制装置结构示意图。
具体实施方式
17.下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
18.通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
19.在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
20.此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
21.除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。
22.本技术的技术方案应用于机械臂的控制，通过对机械臂进行目标姿态的设定，从而可以计算得到机械臂从当前姿态到目标姿态的理论轨迹，为了使得在控制过程中，面对系统的扰动有自适应能力，通过根据机器人动力学方程，建立机械臂动力模型，在建立机械臂出力模型和反馈参数模型，进而获取实际机械臂所需要输出的力，从而控制机械臂稳定的沿着预设轨迹运动。
23.实施例1如图1所示，本技术的机械臂控制方法包括以下步骤：步骤s100，获取机械臂当前的状态参数。
24.得到运动轨迹后，为了使得机械臂按照该轨迹运动，使得机械臂到达目标姿态，而驱动机械臂的运动，是依靠机械臂各个关节中电机输出的力来控制的。
25.其中，机械臂可以是二轴机械臂、三轴机械臂或者四轴机械臂等多轴机械臂，机械臂可以是用于手术的手术机械臂。
26.为此，根据机器人动力学方程，来建立机械臂的动力模型，以此通过建立机械臂动力和机械臂姿态的关系，来通过对机械臂输出力（也就是关节电机输出的力）的控制，来实现对机械臂的控制。
27.其中，该机械臂动力模型表达式为：
式中，是机械臂的惯性矩阵，d为扰动量，是关节角度，是关节角速度，是关节角加速度，是科里奥利力和向心耦合矩阵，是重力项，为机械臂的输出力。
28.根据该机械臂动力模型可知，关节角度、关节角速度和关节角加速度是机械臂当前的运动状态，是已知量，重力加速度是恒定的，因此重力项也是已知量，因此各项中除了和d外，其他的项都是已知量，扰动量d很小，可以忽略不计，因此通过对机械臂的输出力进行控制，就可以在一定程度上，控制机械臂的动作轨迹。
29.步骤s200，根据预先设定好的机械臂出力模型，按照设定好的运动轨迹和所述状态参数，获取当前时刻的机械臂输出力，所述机械臂出力模型根据滑膜变结构和机械臂动力模型构建。
30.在机械臂开始运动前，需要规划机械臂的运动轨迹，例如当前机械臂在原点，需要其控制末端前进一定距离，到达目标地点，则可以根据该目标地点，设定该机械臂的目标参数，例如在目标地点，各轴角度多少，机械臂各个连杆的姿态如何之类的目标参数。同时为了规划路径，也要获取机械臂当前的状态参数，如当前机械臂末端的位置坐标、各个轴的角度等。
31.知道上述状态参数和目标参数后，就可以根据寻路算法，来规划该机械臂的运动轨迹，获得运动轨迹的方法可以是通过两点一线的方法，也可以是最近点搜寻法等，本技术对此不做限定。
32.可以理解的是，为了让机械臂可以稳定安全的运动，需要给机械臂输出力建立模型，以计算出一个贴切的理论输出力，使得机械臂按照该力进行运动时，整个机械臂的轨迹可以贴合理论轨迹。除此之外，电机在每进行运动后，都会到新的状态，因此可以通过步进的方式来进行控制，因此按照固定的采样周期，周期性的获取当前机械臂所需要输出的力，以尽可能精确的控制机械臂的运动，该采样周期可以根据需求和机器性能设定，里如3毫秒或者4毫秒，该采样周期可以在1-5毫秒之间选取。
33.因此，根据滑膜变结构和机械臂动力模型构建机械臂出力模型。其中，滑膜变结构是一种控制策略，用于跟踪控制机械臂的角度、角速度和角加速度这三个运动参数，以实现对机械臂的三阶系统控制。
34.机械臂出力模型表达式为：式中，u为所述机械臂的输出力，f和k为对应的系数，e为机械臂关节实际位置和目标位置的偏差值，为机械臂关节实际角速度和目标角速度的偏差值，f为所述反馈参数。
35.从上式可知，除了f外，其他项也是可知的，f和k可以通过具体的机械臂型号等工装参数预先设置，e和可以通过规划的路径以及当前机械臂的姿态自动计算得到，反应了机械臂当前姿态和规划的轨迹之间的吻合度，所以e和可以视为两条滑膜线，分别反映机械臂当前的角度和速度是否和预设的理想数值吻合。
36.重力项也是可以通过当前机械臂的姿态计算得到的值，所以通过设置合适的反馈参数f，就可以输出适合的输出力。
37.为了能够输出合适的输出力，需要建立合适的反馈参数模型，通过建立反馈参数模型，以保证整个机械臂系统的精确度，因此，可以基于上述机械臂动力模型建立李雅普诺夫方程，根据所述李雅普诺夫方程，建立所述反馈参数模型。
38.因此可以设计出李雅普诺夫方程求该方程的微分有：整理上式可得：上式中，是机械臂关节的目标位置，是机械臂关节的目标速度，是机械臂关节目标加速度，为机械臂关节实际加速度和目标加速度的偏差值。
39.根据李雅普诺夫方程性质，当能够保证，则该系统是渐进稳定的。
40.通过建立适当的反馈参数模型，以减少整个系统中的不稳定因素，如摩擦力、系统误差、惯性等带来的误差，因此建立的反馈参数模型为以下表达式：式中，是机械臂的惯性矩阵是科里奥利力和向心耦合矩阵，是关节角加速度，t是时间。
41.根据该反馈参数模型和李雅普诺夫方程可以得到如下式子：由于为斜对称矩阵，结果为0矩阵，整理后得到下式：由于d为极小的扰动量，可以忽略不计。
42.则根据李雅普诺夫方程的性质，存在当，，可以确定该系统是渐进稳定的，也就是说，通过设定恰当的参数，可以使得当前的机械臂处于稳定状态，使得机械臂实际的运动轨迹和理想轨迹尽可能的吻合。
43.步骤s300，根据所述输出力，控制所述机械臂按照所述运动轨迹运动。
44.将上述步骤中的反馈参数模型和机械臂出力模型联立，可以得到表达式。该式子为最终的机械臂出力模型，在机
械臂进行实际的工作时，通过设置好目标参数，并获取当前机械臂的状态参数，得到运动轨迹后，就可以通过该表达式获得机械臂的电机所应该输出的力。
45.此外，因为该表达式根据上述的李雅普诺夫方程可知，是稳态的，因此在机械臂工作过程中，发生扰动时，也可以使用该表达式继续进行控制，由于系统的稳定性，可以迅速的消除扰动。
46.为了方便说明，以二轴机械臂为例，设该二轴机械臂两个连杆长度分别为，，两个连杆的质量分别为，，两个关节理想角度信号，，且系统初始向量为零，以此做仿真实验。
47.该仿真实验可以在matlab上进行，图2为以上述初始量进行的仿真实验图，其中，曲线100为该二轴机械臂第一个轴的实时角速度曲线和理想角速度曲线的重合曲线，曲线200为该二轴机械臂第二个轴的实时角速度曲线和理想角速度曲线的重合曲线，因为实际轨迹和目标轨迹贴合，所以两个曲线图会重叠。
48.其中，圈记起来的210部分的放大图由图3示出。
49.如图3所示，为机械臂在运动过程中产生的抖动，该抖动产生自曲线200，即在运动过程中，出现了误差，使得机械臂连杆有了偏移，所以曲线200和理想角速度曲线300之间不再重合，并且理想角速度因为机械臂位置的移动，而重新估计计算，所以曲线300也根据当前偏移的位姿重计算，和原来的轨迹并不接续，所以从图3中可以看到曲线300和曲线200之间存在短暂的空档，之后两个曲线又吻合起来，体现了整个系统的稳定性。
50.图4为图2中220部分的放大图，为机械臂刚开始运动时的曲线图，其中一开始时，曲线200和曲线300之间有明显的差别，差别小且趋势相同，并且很快就相互吻合，可见在机械臂刚开始运作时，机械臂就可以快速的进入稳定工作的状态。而且所有的调整都只需要通过上述的模型进行计算即可，无需在发生抖动时通过其他方式进行纠正。
51.本技术的机械臂控制方法，通过根据目标姿态设定机械臂的目标参数，根据机器人动力学方程，建立机械臂动力模型；根据所述机械臂动力模型，建立机械臂出力模型；根据所述机械臂动力模型，建立反馈参数模型，根据所述目标参数和所述反馈参数模型，获得所述机械臂的输出力；根据所述输出力和所述机械臂出力模型，控制所述机械臂按照所述目标参数运动。使得机械臂按照模型参数控制机械臂每时每刻的动作，出现了抖动也可以很快恢复稳定，使得整个机械臂的控制过程更加安全，并减少计算，减少了对计算资源的占用，减轻了控制终端的负荷。
52.实施例2本技术还提供一种机械臂控制装置，如图5所示，包括：监控模块10，用于获取机械臂当前的状态参数；构建模块20，用于根据预先设定好的机械臂出力模型，按照设定好的运动轨迹和所述状态参数，获取当前时刻的机械臂输出力，所述机械臂出力模型根据滑膜变结构和机械臂动力模型构建；控制模块30，用于根据所述输出力，控制所述机械臂按照所述运动轨迹运动。
53.本技术还提供一种控制终端，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行所述的机械臂控制方法。
54.本技术还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行所述的机械臂控制方法。
55.在本技术所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
56.另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
57.所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是智能手机、个人计算机、服务器、或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器（rom，read-only memory）、随机存取存储器（ram，random access memory）、磁碟或者光盘等各种可以存储程序代码的介质。
58.以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。