机械臂运动控制方法和系统与流程
本发明涉及机械臂技术领域，特别是涉及一种机械臂运动控制方法和系统。
背景技术：
在机械臂实际的工作场景中，可能会存在众多的障碍物。在运动规划问题中，需要在给定的起点，终点之间，规划出一条避免碰撞的路径，从而保证运行的安全。
机械臂的运动规划分为离线和在线两种方式。离线方式可以在机械臂运动前预先规划好运行路径，但是这需要事前知道工作空间中障碍物的几何结构和位置姿态；而在线方式系统一边通过传感器获取实时的环境数据，一边同步对机械臂的运动进行规划。一般来说，离线方式效率较高，但是对于一些存在动态障碍物的工作环境中，必须使用在线方式。
现有的械臂路径在线控制方式一般是根据障碍物的分布实时计算机械臂运动的多个中间点，根据中间点控制机械臂的运动路径，这种方式计算量较大，导致机械臂运动控制效率较低。
技术实现要素：
基于此，有必要针对机械臂运动控制效率较低的问题，提供一种机械臂运动控制方法和系统。
一种机械臂运动控制方法，包括以下步骤：
获取机械臂运动的起点与终点之间的障碍物的实时位置；
根据所述实时位置获取各个障碍物与机械臂关节上预先选定的控制点的最短距离，计算所述最短距离之和；
根据所述最短距离之和与所述机械臂关节的运动速度实时地计算对所述控制点的第一虚拟力以及对机械臂终端的第二虚拟力，根据所述第一虚拟力和第二虚拟力确定对所述机械臂的实时控制力矩；
根据所述实时控制力矩对机械臂进行实时路径控制。
一种机械臂运动控制系统，包括：
第一获取模块，用于获取机械臂运动的起点与终点之间的障碍物的实时位置；
第二获取模块，用于根据所述实时位置获取各个障碍物与机械臂关节上预先选定的控制点的最短距离，计算所述最短距离之和；
计算模块，用于根据所述最短距离之和与所述机械臂关节的运动速度实时地计算对所述控制点的第一虚拟力以及对机械臂终端的第二虚拟力，根据所述第一虚拟力和第二虚拟力确定对所述机械臂的实时控制力矩；
路径控制模块，用于根据所述实时控制力矩对机械臂进行实时路径控制。
上述机械臂运动控制方法和系统，能够根据障碍物的实时位置获取各个障碍物与机械臂关节上预先选定的控制点的最短距离之和，根据该最短距离之和计算第一虚拟力和第二虚拟力，根据第一虚拟力和第二虚拟力生成实时控制力矩，并根据实时控制力矩对机械臂进行实时路径控制，在机械臂运动过程中，能够根据机械臂与障碍物之间的距离生成一个作用力，将机械臂推离障碍物，避免机械臂撞上障碍物。上述方法和系统计算量小，控制效率高。
附图说明
图1为一个实施例的机械臂运动控制方法流程图；
图2为一个实施例的机械臂关节及控制点的示意图；
图3为一个实施例的机械臂运动控制方法的程序流图；
图4为一个实施例的机械臂运动控制系统的结构示意图。
具体实施方式
下面结合附图对本发明的技术方案进行说明。
如图1所示，本发明提供一种机械臂运动控制方法，可包括以下步骤：
s1，获取机械臂运动的起点与终点之间的障碍物的实时位置；
本实施例中的实时位置可以是动态的位置，即，各个障碍物的实时位置可随时间变化。起点与终点之间是指机械臂从起点ps运动到终点pe的运动路径上可能经过的位置。在一个实施例中，可以通过传感器获取障碍物的实时位置，传感器可以以一定的频率采集各个障碍物的实时位置。在一个具体实施例中，可以获取障碍物的运动速度，根据障碍物的运动速度设置传感器采集实时位置的频率。例如，当障碍物的运动速度大于预设的速度阈值时，可以以较大的频率采集实时位置；反之，可以以较小的频率采集实时位置。这里的“较大”和“较小”可以是相对于某个默认的采集频率的值。进一步地，还可以设置多个阈值档，当障碍物的运动速度大于第一速度阈值时，以第一频率采集实时位置；当障碍物的运动速度大于第二速度阈值时，以第二频率采集实时位置；……；当障碍物的运动速度大于第n速度阈值时，以第n(n为正整数)频率采集实时位置；其中，第一频率、第二频率、……、第n频率的值依次减小。通过设置不同的速度阈值与对应的采集频率，既可以保证障碍物位置获取的时效性，又可以在障碍物位置变换较慢的情况下减少数据交互量。
s2，根据所述实时位置获取各个障碍物与机械臂关节上预先选定的控制点的最短距离，计算所述最短距离之和；
控制点可以是机械臂关节上的一个点，这个点可以预先选定。机械臂的每个关节上均可以设置一个控制点。一个实施例的机械臂关节及控制点的示意图如图2所示，机械臂包括三个关节，三个关节上各有一个选定的控制点p1(q)、p2(q)和p3(q)，o为机械臂工作空间中的障碍物集合，peff(q)为机械臂的终端位置，机械臂终端是指在机械臂末端，用于与环境交互的设备，具体取决于机器人的应用场景。比说，在机械臂末端安装一个夹子，那这个夹子就是这个机械臂的终端。参数q＝[q1,q2,…,qn]表示机械臂各个关节的角度。对每个控制点，可分别计算各个障碍物到该控制点的最短距离之和。对于第i个控制点，t时刻各个障碍物到该控制点的最短距离之和可记为：
式中，di,t表示t时刻各个障碍物与第i个控制点的最短距离之和，pi表示第i个机械臂关节上的控制点，pt'表示t时刻障碍物的实时位置，oi表示第i个机械臂关节的工作空间，k表示机械臂关节的个数。
s3，根据所述最短距离之和与所述机械臂关节的运动速度实时地计算对所述控制点的第一虚拟力以及对机械臂终端的第二虚拟力，根据所述第一虚拟力和第二虚拟力确定对所述机械臂的实时控制力矩；
根据机械臂的动力学，t时刻笛卡尔空间中第i个机械臂关节的运动速度
式中，j为雅克比矩阵，t表示转置操作，
同样地，笛卡尔空间中的力f与机械臂各个关节的力矩π＝[π0,π1,…,πk]之间存在以下关系：
f＝jπ；(4)
π＝jtf；(5)
在t时刻，对于第i个控制点，可生成如下第一虚拟力：
式中，fi,t为t时刻对第i个控制点的第一虚拟力，γ，c，b为预设的控制参数，
进一步地，在t时刻，对于机械臂终端，可生成如下第二虚拟力：
其中，et＝pe-peff,t；
式中，feff,t为t时刻对机械臂终端的第二虚拟力，α，γ，c，g，b为预设的控制参数，pe为机械臂运动的终点位置，peff,t为t时刻机械臂终端的位置，
相应地，在t时刻，对第i个机械臂关节的实时控制力矩可记为：
式中，πt为t时刻对机械臂的实时控制力矩，
s4，根据所述实时控制力矩对机械臂进行实时路径控制。
根据上述实时控制力矩，可以驱动机械臂各个电机运行，实现对机械臂各个关节的控制。一个实施例的机械臂运动控制方法的程序流图如图3所示。
通过上述方式，本发明可以根据障碍物的实时位置获取各个障碍物与机械臂关节上预先选定的控制点的最短距离之和，根据该最短距离之和计算第一虚拟力和第二虚拟力，根据第一虚拟力和第二虚拟力生成实时控制力矩，并根据实时控制力矩对机械臂进行实时路径控制，在机械臂运动过程中，能够根据机械臂与障碍物之间的距离生成一个作用力，将机械臂推离障碍物，避免机械臂撞上障碍物。
本发明具有以下优点：
(1)采用在线控制方式，能够在存在动态障碍物的工作环境中有效地对机械臂进行运动控制。
(2)无需反复计算机械臂运动的中间点，减少了计算量，提高了机械臂运动控制的效率。
如图4所示，本发明还提供一种机械臂运动控制系统，可包括：
第一获取模块10，用于获取机械臂运动的起点与终点之间的障碍物的实时位置；
本实施例中的实时位置可以是动态的位置，即，各个障碍物的实时位置可随时间变化。起点与终点之间是指机械臂从起点ps运动到终点pe的运动路径上可能经过的位置。在一个实施例中，可以通过传感器获取障碍物的实时位置，传感器可以以一定的频率采集各个障碍物的实时位置。在一个具体实施例中，可以获取障碍物的运动速度，根据障碍物的运动速度设置传感器采集实时位置的频率。例如，当障碍物的运动速度大于预设的速度阈值时，可以以较大的频率采集实时位置；反之，可以以较小的频率采集实时位置。这里的“较大”和“较小”可以是相对于某个默认的采集频率的值。进一步地，还可以设置多个阈值档，当障碍物的运动速度大于第一速度阈值时，以第一频率采集实时位置；当障碍物的运动速度大于第二速度阈值时，以第二频率采集实时位置；……；当障碍物的运动速度大于第n速度阈值时，以第n(n为正整数)频率采集实时位置；其中，第一频率、第二频率、……、第n频率的值依次减小。通过设置不同的速度阈值与对应的采集频率，既可以保证障碍物位置获取的时效性，又可以在障碍物位置变换较慢的情况下减少数据交互量。
第二获取模块20，用于根据所述实时位置获取各个障碍物与机械臂关节上预先选定的控制点的最短距离，计算所述最短距离之和；
控制点可以是机械臂关节上的一个点，这个点可以预先选定。机械臂的每个关节上均可以设置一个控制点。一个实施例的机械臂关节及控制点的示意图如图2所示，机械臂包括三个关节，三个关节上各有一个选定的控制点p1(q)、p2(q)和p3(q)，o为机械臂工作空间中的障碍物集合，peff(q)为机械臂的终端位置，机械臂终端是指在机械臂末端，用于与环境交互的设备，具体取决于机器人的应用场景。比说，在机械臂末端安装一个夹子，那这个夹子就是这个机械臂的终端。参数q＝[q1,q2,…,qn]表示机械臂各个关节的角度。对每个控制点，可分别计算各个障碍物到该控制点的最短距离之和。对于第i个控制点，t时刻各个障碍物到该控制点的最短距离之和可记为：
式中，di,t表示t时刻各个障碍物与第i个控制点的最短距离之和，pi表示第i个机械臂关节上的控制点，pt'表示t时刻障碍物的实时位置，oi表示第i个机械臂关节的工作空间，k表示机械臂关节的个数。
计算模块30，用于根据所述最短距离之和与所述机械臂关节的运动速度实时地计算对所述控制点的第一虚拟力以及对机械臂终端的第二虚拟力，根据所述第一虚拟力和第二虚拟力确定对所述机械臂的实时控制力矩；
根据机械臂的动力学，t时刻笛卡尔空间中第i个机械臂关节的运动速度
式中，j为雅克比矩阵，t表示转置操作，
同样地，笛卡尔空间中的力f与机械臂各个关节的力矩π＝[π0,π1,…,πk]之间存在以下关系：
f＝jπ；(4)
π＝jtf；(5)
在t时刻，对于第i个控制点，可生成如下第一虚拟力：
式中，fi,t为t时刻对第i个控制点的第一虚拟力，γ，c，b为预设的控制参数，
进一步地，在t时刻，对于机械臂终端，可生成如下第二虚拟力：
其中，et＝pe-peff,t；
式中，feff,t为t时刻对机械臂终端的第二虚拟力，α，γ，c，g，b为预设的控制参数，pe为机械臂运动的终点位置，peff,t为t时刻机械臂终端的位置，
相应地，在t时刻，对第i个机械臂关节的实时控制力矩可记为：
式中，πt为t时刻对机械臂的实时控制力矩，
路径控制模块40，用于根据所述实时控制力矩对机械臂进行实时路径控制。
根据上述实时控制力矩，可以驱动机械臂各个电机运行，实现对机械臂各个关节的控制。一个实施例的机械臂运动控制方法的程序流图如图3所示。
通过上述方式，本发明可以根据障碍物的实时位置获取各个障碍物与机械臂关节上预先选定的控制点的最短距离之和，根据该最短距离之和计算第一虚拟力和第二虚拟力，根据第一虚拟力和第二虚拟力生成实时控制力矩，并根据实时控制力矩对机械臂进行实时路径控制，在机械臂运动过程中，能够根据机械臂与障碍物之间的距离生成一个作用力，将机械臂推离障碍物，避免机械臂撞上障碍物。
本发明具有以下优点：
(1)采用在线控制方式，能够在存在动态障碍物的工作环境中有效地对机械臂进行运动控制。
(2)无需反复计算机械臂运动的中间点，减少了计算量，提高了机械臂运动控制的效率。
本发明的机械臂运动控制系统与本发明的机械臂运动控制方法一一对应，在上述机械臂运动控制方法的实施例阐述的技术特征及其有益效果均适用于机械臂运动控制系统的实施例中，特此声明。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。
- 还没有人留言评论。精彩留言会获得点赞！