一种伺服机械臂运动路径的计算方法与流程

本发明涉及伺服机械臂运动控制，尤其涉及一种伺服机械臂运动路径的计算方法。

背景技术：

1、在伺服机械臂进行伸缩动作与摆动动作复合运动走直线时，需协同控制摆动轴与伸缩轴的运动轨迹。现有技术中已有相关方案，如专利cn102692930a所公开的一种运动合成方法及运动装置，其将伺服机械臂的摆动轴作为主轴、伸缩轴作为从轴，通过令主轴匀速摆动，并根据摆动轴的坐标位置实时计算出伸缩轴对应的伸缩长度和运动速度，从而实现两个伺服轴末端沿直线路径运动。

2、上述现有方案将摆动轴设定为匀速运动的主轴，伸缩轴作为从轴被动跟随，为了保证最终合成轨迹为直线，伸缩轴的速度需持续变化以拟合主轴的位置，这就导致机械臂末端在整个直线运动过程中的合速度始终处于波动状态，无法实现匀速运动。同时该方案的运动轨迹计算高度依赖于摆动轴和伸缩轴的零点位置准确性，一旦任一伺服轴在调试或检修过程中发生零点偏移，基于原有零点标定计算出的运动轨迹将偏离预定直线路径，变为弧线轨迹；而要在没有高精度测量仪器的现场条件下将零点精确恢复至原始位置，操作极为困难且耗时。

3、因此，针对上述提出的问题，本发明提出一种伺服机械臂运动路径的计算方法。

技术实现思路

1、为了克服上述因主轴-从轴固定分配模式所导致的速度均匀性与零点容错性难以兼顾的问题，本发明提出一种伺服机械臂运动路径的计算方法，以虚拟直线轴上的运动点作为虚拟主轴，将摆动轴和伸缩轴均作为从轴进行协同配合，无需依赖固定的物理零点即可完成路径规划与速度分配。

2、本发明的技术方案为：一种伺服机械臂运动路径的计算方法，包括以下步骤：

3、s1，以伺服机械臂末端在目标直线路径上运动的点作为虚拟主轴，将伺服机械臂的摆动轴和伸缩轴均作为从轴，该摆动轴和伸缩轴均配合虚拟主轴运动，该标直线路径为伺服机械臂在进行钻杆抓取或钻杆放置操作时，机械臂末端所夹持的钻杆在钻杆箱目标列位置沿上下方向运动的直线路径；

4、s2，控制伺服机械臂末端运动到目标直线路径上的第一定位点和第二定位点，分别读取并记录第一定位点对应的第一摆动轴角度值和第一伸缩轴长度值，与第二定位点对应的第二摆动轴角度值和第二伸缩轴长度值，其中第一定位点和第二定位点为目标直线路径上的任意两个不同位置点，第一摆动轴角度值和第二摆动轴角度值均为相对于伺服机械臂当前零位的角度值，第一伸缩轴长度值和第二伸缩轴长度值均为相对于伺服机械臂当前零位的长度值；

5、s3，根据第一定位点和第二定位点的坐标值，以伺服机械臂摆动轴回转中心为点o，以目标直线路径为直线l，以第一定位点为点a、第二定位点为点b，以点e为点o与直线l垂直连线的垂足，以点d为目标直线路径上的任意目标点；

6、令点a的伸缩轴长度值为a，点b的伸缩轴长度值为b，点a与点b两个定位点摆动轴角度值的差值为c，点d的摆动轴角度值为y，点d的伸缩轴长度值为d；

7、点e为点o与直线l垂直连线的垂直点。

8、根据余弦公式可得：

9、；

10、根据余弦公式由点a和点b的坐标关系求取点e对应的摆动轴角度值x：

11、因为:

12、；

13、所以:

14、；

15、化简可得：

16、；

17、两边都除以得：

18、；

19、化简得：

20、；

21、求得：

22、；

23、根据点e对应的摆动轴角度值x求取目标直线路径上任意目标点d的伸缩轴长度值d：

24、将目标点d的摆动轴角度值y和伸缩轴值d带入可得：

25、；

26、最终求得：

27、；

28、s4，设定伺服机械臂末端在目标直线路径上的直线运动线速度为v，在摆动轴方向的线速度分量为，在伸缩轴方向的线速度分量为；伺服摆动轴的摆动半径为d，故伺服摆动轴的摆动速度为。

29、作为优选，当伺服机械臂的摆动轴或伸缩轴的零位发生偏移后，重新控制伺服机械臂末端运动到目标直线路径上的两个位置，读取并记录偏移后的两个位置对应的摆动轴角度值和伸缩轴长度值作为新的定位点坐标，根据新的定位点坐标重新计算目标直线路径上任意点的摆动轴角度值和伸缩轴长度值。

30、作为优选，所述伺服机械臂为安装在智能钻车上的多轴伺服机械臂，用于执行钻杆的自动加装和自动拆卸操作，在伺服机械臂从钻杆箱中抓取钻杆或向钻杆箱中放入钻杆时，采用所述计算方法控制伺服机械臂的摆动轴和伸缩轴复合运动，使机械臂末端所夹持的钻杆沿直线路径运动。

31、本发明的有益效果：

32、1.本发明以虚拟直线轴上的运动点作为虚拟主轴，将摆动轴和伸缩轴均设为从轴，基于两个定位点的几何约束直接计算任意目标点的轴坐标，舍弃了传统主轴与从轴固定分配模式对主轴匀速假设的依赖，有效简化了复合运动走直线的路径计算，并确保实际轨迹与理论直线之间的偏差极小。

33、2.本发明不规定摆动轴和伸缩轴的固定零点，只需要采集直线路径上任意两个当前位置点的坐标即可重建整条路径的轨迹参数，在伺服轴零点发生偏移后无需借助任何高精度测量工具即可自动完成零点补偿，大幅缩短了调试和维修过程中的调零时间。

34、3.本发明通过将末端直线速度沿摆动方向和伸缩方向进行几何投影分解，分别计算出摆动轴角速度和伸缩轴线速度，使两个从轴协同配合虚拟主轴运动，避免了因主轴匀速而导致的从轴速度非线性变化，从而保证了机械臂末端在整个直线运动过程中的速度恒定且均匀。

1.一种伺服机械臂运动路径的计算方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种伺服机械臂运动路径的计算方法，其特征在于：所述步骤s3中，以伺服机械臂摆动轴回转中心为点o，以目标直线路径为直线l，以第一定位点为点a、第二定位点为点b，以点e为点o与直线l垂直连线的垂足，以点d为目标直线路径上的任意目标点；

3.根据权利要求2所述的一种伺服机械臂运动路径的计算方法，其特征在于，所述步骤s3中，根据余弦公式由点a和点b的坐标关系求取点e对应的摆动轴角度值x：

4.根据权利要求3所述的一种伺服机械臂运动路径的计算方法，其特征在于，所述步骤s3中，根据点e对应的摆动轴角度值x求取目标直线路径上任意目标点d的伸缩轴长度值d：

5.根据权利要求1所述的一种伺服机械臂运动路径的计算方法，其特征在于：所述步骤s4中，设定伺服机械臂末端在目标直线路径上的直线运动线速度为v，在摆动轴方向的线速度分量为，在伸缩轴方向的线速度分量为；伺服摆动轴的摆动半径为d，故伺服摆动轴的摆动速度为。

6.根据权利要求1所述的一种伺服机械臂运动路径的计算方法，其特征在于：所述步骤s2中，第一定位点和第二定位点为目标直线路径上的任意两个不同位置点，第一摆动轴角度值和第二摆动轴角度值均为相对于伺服机械臂当前零位的角度值，第一伸缩轴长度值和第二伸缩轴长度值均为相对于伺服机械臂当前零位的长度值。

7.根据权利要求1所述的一种伺服机械臂运动路径的计算方法，其特征在于：当伺服机械臂的摆动轴或伸缩轴的零位发生偏移后，重新控制伺服机械臂末端运动到目标直线路径上的两个位置，读取并记录偏移后的两个位置对应的摆动轴角度值和伸缩轴长度值作为新的定位点坐标，根据新的定位点坐标重新计算目标直线路径上任意点的摆动轴角度值和伸缩轴长度值。

8.根据权利要求1所述的一种伺服机械臂运动路径的计算方法，其特征在于：所述目标直线路径为伺服机械臂在进行钻杆抓取或钻杆放置操作时，机械臂末端所夹持的钻杆在钻杆箱目标列位置沿上下方向运动的直线路径。

9.根据权利要求1-8中任一项所述的一种伺服机械臂运动路径的计算方法，其特征在于：所述伺服机械臂为安装在智能钻车上的多轴伺服机械臂，用于执行钻杆的加装和拆卸操作。

10.根据权利要求9所述的一种伺服机械臂运动路径的计算方法，其特征在于：在伺服机械臂从钻杆箱中抓取钻杆或向钻杆箱中放入钻杆时，采用所述计算方法控制伺服机械臂的摆动轴和伸缩轴复合运动，使机械臂末端所夹持的钻杆沿直线路径运动。