一种机器人自动补偿运动控制方法与流程

1.本发明涉及机械臂控制领域，更具体地，涉及一种机器人自动补偿运动控制方法。


背景技术：

2.通过手术辅助机器人辅助手术的完成是现今常见的外科手术手段，其中就包括视网膜注射。但是在机器人自动执行动作的过程中，往往存在一定的运动误差。而应用在医疗领域，这些运动误差会导致机器人造成的切口或者一些注射操作的精度降低，造成身体上的损伤。
3.现有一种基于误差补偿的工业机器人运动控制方法，包括以下步骤：s1：坐标系建立：获取机器人上一个动作后，机器人上的末端执行器停留时的所在位置，并建立坐标系；s2：参数模板建立：建立机器人的运动学参数模型；s3：建立运动轨迹：根据机器人所要运动位置的终点，选取坐标系中一点为其运动终点，并在机器人停留位置和运动终点之间建立运动轨迹：s4：运动执行：机器人根据建立的运动轨迹进行运动。该基于误差补偿的工业机器人运动控制方法，能够解决目前由于机器人本身的重力、惯性力以及所受负载的影响，使得机器人的实际轨迹将偏离预定轨迹，降低加工质量的问题。
4.但是在上述的方法中，该机器人应用在工业领域，其方法能够达到的精度依然难以满足医疗中视网膜注射的需要，其运动精度低，在使用过程中容易发生其他方向的运动。


技术实现要素：

5.本发明为克服上述现有技术中机械人执行运动精度不够低问题，提供一种机器人自动补偿运动控制方法，令机器人能够按照设定的路径自动执行相关的运动，提高运动精度。
6.为解决上述技术问题，本发明采用的技术方案是：提供一种机器人自动补偿运动控制方法，包括机械臂和安装于所述机械臂上的执行元件，所述机械臂包括夹持所述执行元件的夹持装置和驱动所述夹持装置摆动的驱动臂；所述驱动臂包括第一关节和与所述第一关节连接的第二关节，所述第一关节驱动所述夹持装置摆动，所述第二关节驱动所述第一关节摆动；所述第一关节包括第一线性电机和第二线性电机；所述第二关节包括第三线性电机和第四线性电机；所述夹持装置设置有驱动所述执行元件直线运动的第五线性电机和驱动所述执行元件的活塞运动的第六线性电机；包括如下步骤：
7.步骤一：控制全部关节线性电机进行位置标定，完成运动初始化；
8.步骤二：选定执行元件末端初始位置，并控制机械臂运动至指定末端初始位置；
9.步骤三：设定机器人各个关节的运动路径，即设定各线性电机的位移量序列；
10.步骤四：控制机械臂驱动执行元件沿着设定的运动路径运动，当机械臂末端执行元件进行旋转运动时，进入自动补偿运动控制模式，综合设定位移量与自动补偿位移量，获得最终执行的实际线性位移量。
11.步骤五：根据步骤四中计算获得的实际线性位移量，驱动机械臂执行运动。
12.在上述的技术方案中，执行元件末端的初始位置和机械臂的运动路径是根据对操作对象的实际情况观察进行设定。本方案的重点在于如何控制机械臂驱动执行元件沿着设定好的运动路径自动行进，使得执行元件能够在机械臂的驱动下自动执行补偿运动。机械臂为现有机械臂装置，具有两个转动关节，每个转动关节的两个线性电机均为平行设置，通过两个线性电机的位置差实现关节的转动，而第五线性电机即可驱动夹持装置做直线运动，令执行元件前进。机械臂在带动执行元件前进的时候，由于所设定的运动路径需要转弯，因为令机械臂带动执行元件的末端执行自动补偿运动，令执行元件的末端在转折点进行旋转，顺利完成整个运动路径。在整个过程中，执行元件的运动是根据机械臂的沿着设定的运动轨迹进行运算完成的，无需人工操作。
13.优选的，在所述步骤四中，执行元件末端的自动补偿运动中的实际线性位移量计算如下：
14.s4.1：获取机械臂内部硬件参数dm1,dm2,h1,h2,ltool；其中，dm1为第三线性电机到第四线性电机的水平轴的垂直距离；dm2为第一线性电机到第二线性电机的水平轴的垂直距离；h1为第三线性电机和第四线性电机的水平中点到执行元件的垂直距离；h2为第一线性电机和第二线性电机的水平中点到执行元件的垂直距离；ltool为执行元件的长度。
15.s4.2：计算执行元件末端的角度，具体为：
[0016][0017][0018]
式中，θ1为第一关节与垂直面形成的角度；dl1为第一线性电机的当前位置；dl2为第二线性电机的当前位置；θ2为第二关节与垂直面形成的角度；dl3为第三线性电机的当前位置；dl4为第四线性电机的当前位置。
[0019]
s4.3：计算执行元件相对于机器人坐标系的当前位置，以执行元件末端位置为自动补偿点坐标，具体如下：
[0020][0021][0022]
z＝((ltool dl5)*cos(θ1) h1*sin(θ1))*cos(θ2) h2*sin(θ2)
[0023]
式中，x为执行元件在机器人空间坐标系下的x轴坐标位置；y为执行元件在机器人空间坐标系下的y轴坐标位置；z为执行元件在机器人空间坐标系下的z轴坐标位置；dl5为第五线性电机的当前位置。
[0024]
s4.4：计算执行元件末端的自动补偿线性位移量，具体如下：
[0025]
[0026]
式中，δl1、δl2、δl3、δl4、δl5分别为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的目标补偿位移量；m为补偿矩阵。
[0027]
式中补偿矩阵具体为：
[0028]
m＝[m
1 m
2 m
3 m
4 m5]
[0029][0030][0031][0032][0033]
式中，θ2为第二关节与垂直面形成的角度；l1为第一线性电机的设定线性位移量；l2为第二线性电机的设定线性位移量；dm1为第三线性电机到第一线性电机或第二线性电机的水平轴的垂直距离；l3为第三线性电机的设定线性位移量；l4为第四线性电机的设定线性位移量；l5为第五线性电机的设定线性位移量；dm2为第五线性电机到第三线性电机或第四线性电机的水平轴的垂直距离。
[0034]
s4.5：由设定位移量与自动补偿位移量，计算获得最终执行的实际线性位移量。具体如下：
[0035]
l1'＝l1 δl1
[0036]
l2'＝l2 δl2
[0037]
l3'＝l3 δl3
[0038]
l4'＝l4 δl4
[0039]
l5'＝l5 δl5
[0040]
式中，l1、l2、l3、l4、l5为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机原本的设定线性位移量，l1’、l2’、l3’、l4’、l5’为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机补偿后的实际线性位移量。
[0041]
设定的运动路径有每一步位置的坐标点，通过计算执行元件末端位置和执行元件末端的角度，然后按照两者的运算结果来反馈执行元件末端位置，从而令机械臂按照每个位置之间的差距来驱动执行元件末端逐步前进。
[0042]
通过补偿矩阵以及运动轨迹公式可以将自动补偿运动时执行元件末端下一步到达的位置映射到每个线性电机需要移动的位移。通过这种反向推导控制每个线性电机移动多少，才能进而精准控制执行元件末端自动补偿运动的角度。
[0043]
优选的，执行元件末端在进行自动补偿运动时，每前进一步就根据第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的设定线性位移量，同时计算并加上s4.4中得到的执行元件末端位置的补偿矩阵m获取第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的自动补偿线性位移量来控制执行元件末端的运动。通过及时获取五个线性电机的移动位移和移动轨迹来补偿执行元件末端的运动，从而令机械臂能够根据设定的运动轨迹来及时控制和调整执行元件末端的运动。
[0044]
优选的，在执行元件末端进行自动补偿运动时，执行元件末端每前进一步，就重新对移动后的执行元件末端位置进行计算更新，同时根据s4.4进行自动补偿线性位移量的计算，并综合第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的基于当前步骤的原先设定线性位移量计算新的补偿运动值。每次移动后执行元件末端的位置可能出现微小变化，补偿运动位置坐标采用移动后的执行元件末端位置坐标，即补偿运动位置坐标始终保持与执行元件末端位置坐标一致，以提高自动补偿运动的精度。
[0045]
优选的，在执行元件末端进行自动补偿运动时，执行元件末端每一步的目标位置通过计算执行元件末端与垂直面的夹角的夹角变化率以及读取机器人每一关节的电机当前位置获得。夹角变化率表示每一步移动前后执行元件末端与垂直面的夹角的变化微量。采用此方法计算每一步的目标位置，能够在自动补偿位置不断变换的情况下保证自动补偿运动的精度。
[0046]
优选的，在所述步骤一中，通过对操作对象的实际情况观察选定执行元件末端初始位置，使初始位置的确定更具灵活性与针对性。
[0047]
优选的，在执行元件末端进行自动补偿运动时，第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的设定线性位移量的设定，可通过操作杆、键盘等输入设备完成设定，或使用人为预设的固定控制信号序列。
[0048]
优选的，执行元件末端只沿着步骤二中设定的运动路径前进，不进行其他方向的位移，保证执行元件末端运动路径的效率，避免由于意外的运动造成偏离轨迹。
[0049]
一种机械臂控制系统，包括机械臂、电子设备，所述电子设备与所述机械臂电连接；所述电子设备包括存储器和处理器，所述存储器用于保存计算机程序，所述处理器用于执行所述计算机程序，以实现上述的机器人自动补偿运动控制方法。
[0050]
一种计算机可读存储介质，用于存储计算机程序；其中计算机程序被处理器执行时实现上述的机器人自动补偿运动控制方法。
[0051]
与现有技术相比，有益效果是：该方法提供一种在存在阻力的高精度环境下，既能
保证机械臂运动自由度，又能够精准完成闭环运动的控制方式，通过对机械臂的控制，实现执行元件沿着设定的运动轨迹运动，并采用了自动补偿运动控制方式，通过设定路径并计算机械臂线性电机运动的公式，引导机械臂带动执行元件末端在旋转过程中进行补偿运动，完成规定的转角，克服了人手操作机械臂完成操作的精度限制，提高了操作精度，降低了人工操作的难度。
附图说明
[0052]
图1是本发明的机械臂的结构示意图；
[0053]
图2是本发明一种机器人自动补偿控制方法的流程图。
具体实施方式
[0054]
附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。
[0055]
本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”“长”“短”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
[0056]
下面通过具体实施例，并结合附图，对本发明的技术方案作进一步的具体描述：
[0057]
实施例1
[0058]
如图1-2所示为一种机器人自动补偿运动控制方法的实施例，包括机械臂1和安装于机械臂1上的执行元件2，机械臂1包括夹持执行元件2的夹持装置3和驱动夹持装置3摆动的驱动臂；驱动臂包括第一关节4和与第一关节4连接的第二关节5，第一关节4驱动夹持装置3摆动，第二关节5驱动第一关节4摆动；第一关节4包括第一线性电机6和第二线性电机7；第二关节5包括第三线性电机8和第四线性电机9；夹持装置3设置有驱动执行元件2直线运动的第五线性电机10和驱动执行元件2活塞运动的第六线性电机11；本实施例以将该方法应用于注射为例子，执行元件2具体为注射器，包括如下步骤：
[0059]
步骤一：控制全部关节线性电机进行位置标定，完成运动初始化；
[0060]
步骤二：选定执行元件末端初始位置，并控制机械臂运动至指定末端初始位置；
[0061]
步骤三：设定机器人各个关节的运动路径，即设定各线性电机的位移量序列；
[0062]
步骤四：控制机械臂驱动执行元件沿着设定的运动路径运动，当机械臂末端执行元件进行旋转运动时，进入自动补偿运动控制模式，综合设定位移量与自动补偿位移量，获得最终执行的实际线性位移量。
[0063]
执行元件末端的自动补偿运动中的实际线性位移量计算如下：
[0064]
s4.1：获取机械臂内部硬件参数dm1,dm2,h1,h2,ltool；其中，dm1为第三线性电机到第四线性电机的水平轴的垂直距离；dm2为第一线性电机到第二线性电机的水平轴的垂
直距离；h1为第三线性电机和第四线性电机的水平中点到执行元件的垂直距离；h2为第一线性电机和第二线性电机的水平中点到执行元件的垂直距离；ltool为执行元件的长度。
[0065]
s4.2：计算执行元件末端的角度，具体为：
[0066][0067][0068]
式中，θ1为第一关节与垂直面形成的角度；dl1为第一线性电机的当前位置；dl2为第二线性电机的当前位置；θ2为第二关节与垂直面形成的角度；dl3为第三线性电机的当前位置；dl4为第四线性电机的当前位置。
[0069]
s4.3：计算执行元件相对于机器人坐标系的当前位置，以执行元件末端位置为自动补偿点坐标，具体如下：
[0070][0071][0072]
z＝((ltool dl5)*cos(θ1) h1*sin(θ1))*cos(θ2) h2*sin(θ2)
[0073]
式中，x为执行元件在机器人空间坐标系下的x轴坐标位置；y为执行元件在机器人空间坐标系下的y轴坐标位置；z为执行元件在机器人空间坐标系下的z轴坐标位置；dl5为第五线性电机的当前位置。
[0074]
s4.4：计算执行元件末端的自动补偿线性位移量，具体如下：
[0075][0076]
式中，δl1、δl2、δl3、δl4、δl5分别为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的目标补偿位移量；m为补偿矩阵。
[0077]
式中补偿矩阵具体为：
[0078]
m＝[m
1 m
2 m
3 m
4 m5]
[0079]
[0080][0081][0082][0083]
式中，θ2为第二关节与垂直面形成的角度；l1为第一线性电机的设定线性位移量；l2为第二线性电机的设定线性位移量；dm1为第三线性电机到第一线性电机或第二线性电机的水平轴的垂直距离；l3为第三线性电机的设定线性位移量；l4为第四线性电机的设定线性位移量；l5为第五线性电机的设定线性位移量；dm2为第五线性电机到第三线性电机或第四线性电机的水平轴的垂直距离。
[0084]
s4.5：由设定位移量与自动补偿位移量，计算获得最终执行的实际线性位移量。具体如下：
[0085]
l1'＝l1 δl1
[0086]
l2'＝l2 δl2
[0087]
l3'＝l3 δl3
[0088]
l4'＝l4 δl4
[0089]
l5'＝l5 δl5
[0090]
式中，l1、l2、l3、l4、l5为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机原本的设定线性位移量，l1’、l2’、l3’、l4’、l5’为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机补偿后的实际线性位移量。
[0091]
步骤五：根据步骤四中计算获得的实际线性位移量，驱动机械臂执行运动。
[0092]
具体的，执行元件末端在进行自动补偿运动时，每前进一步就根据第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的设定线性位移量，同时计算并加上s4.4中得到的执行元件末端位置的补偿矩阵m获取第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的自动补偿线性位移量来控制执行元件末端的运动。通过及时获取五个线性电机的补偿移动位移和移动轨迹来反映执行元件末端的运动，从而令机械臂1能够根据设定的运动轨迹来及时控制和调整执行元件末端的运动。
[0093]
其中，在执行元件末端进行自动补偿运动时，执行元件末端每前进一步，就重新对移动后的执行元件末端位置进行计算更新，同时根据s4.4进行自动补偿线性位移量的计算，并综合第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的基于当前步骤的原先设定线性位移量计算新的补偿运动值。每次移动后执行元件末端的位置可能出现微小变化，补偿运动位置坐标采用移动后的执行元件末端位置坐标，即补偿运动位置坐标始终保持与执行元件末端位置坐标一致，以提高自动补偿运动的精度。
[0094]
其中，在执行元件末端进行自动补偿运动时，执行元件末端每一步的目标位置通过计算执行元件末端与垂直面的夹角的夹角变化率以及读取机器人每一关节的电机当前位置获得。
[0095]
具体的，在执行元件末端进行自动补偿运动时，第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的设定线性位移量的设定，可通过操作杆、键盘等输入设备完成设定，或使用人为预设的固定控制信号序列。
[0096]
在本实施例中，执行元件末端只沿着步骤二中设定的运动路径前进，不进行其他方向的位移，保证执行元件末端运动路径的效率，避免由于意外的运动造成偏离轨迹。
[0097]
本实施例的工作原理：机械臂1为现有机械臂1装置，具有两个转动关节，每个转动关节的两个线性电机均为平行设置，通过两个线性电机的位置差实现关节的转动，而第五线性电机10即可驱动夹持装置3做直线运动，令执行元件2前进。机械臂1在带动执行元件2前进的时候，由于所设定的运动路径需要转弯，因为令机械臂1带动执行元件2的针尖末端执行自动补偿运动，令执行元件2的末端在转折点进行旋转运动，顺利完成整个运动路径。通过在整个过程中，执行元件2的运动是根据机械臂1的沿着设定的运动轨迹进行运算完成的，无需人工操作。
[0098]
在本实施例的有益效果：该方法提供一种在存在阻力的高精度环境下，既能保证机械臂1运动自由度，又能够精准完成闭环运动的控制方式，通过对机械臂1的控制，实现执行元件2沿着设定的运动轨迹运动，并采用了自动补偿运动方式，通过设定路径并计算机械臂1线性电机运动的公式，引导机械臂1带动执行元件末端在转折点进行旋转运动，完成规定的转角，克服了人手操作机械臂1完成操作的精度限制，提高了精度，降低了人工操作的难度。
[0099]
实施例2
[0100]
一种机械臂控制系统的实施例，包括机械臂和电子设备，所述电子设备与所述机械臂电连接；所述电子设备包括存储器和处理器，所述存储器用于保存计算机程序，所述处理器用于执行所述计算机程序，以实现上述实施例1的机器人自动补偿运动控制方法。
[0101]
实施例3
[0102]
一种计算机可读存储介质，用于存储计算机程序；其中计算机程序被处理器执行时实现上述实施例1的机器人自动补偿运动控制方法
[0103]
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。