一种锚护工作臂关节零位校准方法和系统与流程

本发明属于井下锚护的，具体公开了一种锚护工作臂关节零位校准方法和系统。

背景技术：

1、随着工业化和科技水平的提高，矿井下少人化、无人化成为一种趋势，对锚护工作臂的定位精度要求越来越高。工作臂零位是锚护工作臂定位精度的基础，让工作臂的控制系统能够准确地知道工作臂的各个轴的位置和角度，从而保证工作臂的运动精度和安全性。通过将工作臂的各个轴对准机械零点标记，然后使用电子控制仪或其他技术辅助工具测量和记录轴的零点位置，从而使轴的机械位置和电气位置保持一致。现有的锚护工作臂零位校准方法，测量设备占用空间大，操作过程繁琐，操作人员需要大量的培训时间，且矿井下工作环境恶劣，不能实现快速校准。

2、激光光靶定位技术具有定位简单、高效的优点，非常适配矿井的工作条件，因此本申请提出一种基于激光光靶定位技术的锚护工作臂关节零位校准方法和系统。

技术实现思路

1、本发明提供一种基于激光光靶定位技术的锚护工作臂关节零位校准方法和系统，占用空间小，操作简单，能够实现快速校准。

2、本发明提供的锚护工作臂关节零位校准方法，包括下述步骤：

3、s1-确定锚护工作臂中的待标定关节，包括连接机载连接件和连杆ⅰ的旋转关节ⅰ、连接连杆ⅰ和连杆ⅱ的旋转关节ⅱ、连接连杆ⅱ和连杆ⅲ的移动关节ⅲ、连接连杆ⅲ和连杆ⅳ的旋转关节ⅳ、连接连杆ⅳ和连杆ⅴ的旋转关节ⅴ、连接连杆ⅴ和负载机架的旋转关节ⅵ；

4、旋转关节ⅰ由油缸ⅰ驱动旋转，旋转关节ⅱ由油缸ⅱ驱动旋转，移动关节ⅲ由油缸ⅲ驱动移动，旋转关节ⅳ由油缸ⅳ驱动旋转，旋转关节ⅴ由油缸ⅴ驱动旋转，旋转关节ⅵ由油缸ⅵ驱动旋转；

5、油缸ⅰ、油缸ⅱ、油缸ⅲ、油缸ⅳ、油缸ⅴ为伸缩油缸，油缸ⅵ为回转油缸；

6、s2-打开锚护机器人上的激光光靶，将激光发射器安装在连杆ⅲ处，根据光斑在激光光靶上的位置关系，得到光斑中心的位置坐标x0和y0；

7、反馈信号至机载数据处理终端，机载数据处理终端对光斑中心的位置坐标进行后端优化后得到：

8、

9、

10、其中，l0为激光发射器在锚护工作臂的当前位置，d0为激光发射器安装架高度；

11、由锚护工作臂运动学模型计算得到：

12、a2(cosθ1cosθ2-sinθ1sinθ2)＝δx

13、a2(cosθ1 sinθ2+cosθ2sinθ1)＝δy   (2)

14、其中，θ1为误差补偿后旋转关节ⅰ的旋转角，θ2为误差补偿后旋转关节ⅱ的旋转角，a2为旋转关节ⅱ的运动学模型连杆长度参数；

15、根据公式(2)计算得到θ1和θ2，根据θ1和θ2计算油缸ⅰ和油缸ⅱ的移动量；

16、s3-将激光发射器安装到连杆ⅲ的末端，根据光斑在激光光靶上的位置关系，求出等截面当量臂的截面惯性矩id：

17、

18、其中，i1为连杆ⅲ即伸缩臂的惯性矩、i2为固定臂即连杆ⅱ的惯性矩，k为2；

19、测量移动关节ⅲ伸出部分长度为l1，移动关节ⅲ与连杆ⅱ重叠部分长度为l12，求出公式(3)中的参数β1、β2、β0：

20、

21、外负载对锚护工作臂产生的力为f，在力f的作用下锚护工作臂产生的挠性变形δf和锚护工作臂末位置处的转角θf为：

22、

23、

24、在均布载荷q的作用下锚护工作臂产生的挠性变形δq和锚护工作臂末位置处的转角θq为：

25、

26、

27、外负载对锚护工作臂产生的力矩为m，在力矩m的作用下锚护工作臂产生的挠性变形δm和锚护工作臂末位置处的转角θm为：

28、

29、

30、其中，l＝(ac)/2+cd，ac为油缸ⅱ与连杆ⅱ的连接点到旋转关节ⅱ的距离，cd为油缸ⅱ与连杆ⅱ的连接点到锚护工作臂末位置的距离，e为常量，i＝id；

31、对公式(4)、(6)、(8)求和，得到：

32、δ＝δf+δq+δm   (10)

33、对公式(5)、(7)、(9)求和，得到：

34、θδ＝θf+θq+θm   (11)

35、锚护工作臂的挠性变形δ取最小均方差时，移动关节ⅲ的伸长量为d3，记录为移动关节ⅲ的零点l3，为油缸ⅲ的移动量；

36、s4-将激光发射器安装到连杆ⅴ的末端，根据光斑在激光光靶上的位置关系，配合各旋转关节传感器测量的角度值，解算得到欧拉角θ、ψ，计算旋转矩阵和移动矩阵，旋转矩阵和移动矩阵的误差通过最小二乘法补偿到旋转关节ⅳ的旋转角补偿量δθ4和旋转关节ⅴ的旋转角补偿量δθ5上，得到

37、

38、

39、其中，θ4为误差补偿后旋转关节ⅳ的旋转角，θ5为误差补偿后旋转关节ⅴ的旋转角，根据θ4和θ5计算油缸ⅳ和油缸ⅴ的移动量；

40、s5-将激光发射器安装到负载机架上，根据光斑在激光光靶上的位置关系，得到旋转关节ⅵ所在坐标系中x轴方向和y轴方向上的补偿量：

41、

42、

43、其中，a5、a6为旋转关节ⅴ、旋转关节ⅵ的运动学模型连杆长度参数；

44、旋转关节ⅵ的旋转角补偿量δθ6：

45、

46、根据逆运动学，由旋转矩阵

47、

48、得到旋转关节ⅵ未补偿前的旋转角：

49、

50、其中，c1＝cosθ1，s1＝sinθ1；

51、误差补偿后旋转关节ⅵ的旋转角θ6：

52、θ6＝θ6'+δθ6   (17)

53、得到油缸ⅵ的旋转量；

54、s6-根据油缸ⅰ、油缸ⅱ、油缸ⅲ、油缸ⅳ、油缸ⅴ的移动量，油缸ⅵ的旋转量校准锚护工作臂。

55、本发明提供的锚护工作臂关节零位校准系统，包括基准卡、激光发射器、激光光靶和机载数据处理终端；基准卡用于插入锚护工作臂中旋转关节连接的两个连杆之间；激光发射器用于安装在锚护工作臂上，发射激光；激光光靶用于安装在锚护机器人上，用于接收激光发射器发出的激光；机载数据处理终端用于处理激光光靶采集到的位置信息进行后端优化，得到锚护工作臂的位姿误差量，然后进行误差补偿和油缸移动量和旋转量的计算，根据计算结果校准锚护工作臂。

56、与现有技术相比，本发明具有以下有益效果。

57、1、本发明通过激光定位技术获取锚护工作臂关节零点的误差信息，相比于传统方法，不需要额外的测量设备，占用空间小，操作简单，能够快速的使用在矿井作业环境中。

58、2、本发明通过旋转和平移变换矩阵计算补偿量，校准后的工作臂绝对定位精度更高，提高锚护机器人的智能化程度。

技术特征：

1.一种锚护工作臂关节零位校准方法，其特征在于，包括下述步骤：

2.根据权利要求1所述的锚护工作臂关节零位校准方法，其特征在于，步骤s2具体包括：

3.根据权利要求2所述的锚护工作臂关节零位校准方法，其特征在于，步骤s3具体包括：

4.根据权利要求3所述的锚护工作臂关节零位校准方法，其特征在于，步骤s4具体包括：

5.根据权利要求4所述的锚护工作臂关节零位校准方法，其特征在于，步骤s5具体包括：

6.一种锚护工作臂关节零位校准系统，其特征在于，包括基准卡、激光发射器、激光光靶和机载数据处理终端；

技术总结本发明提供了一种锚护工作臂关节零位校准方法和系统，属于井下锚护的技术领域，锚护工作臂关节零位校准系统，包括基准卡、激光发射器、激光光靶和机载数据处理终端；基准卡用于插入锚护工作臂中旋转关节连接的两个连杆之间；激光发射器用于安装在锚护工作臂上，发射激光；激光光靶用于安装在锚护机器人上，用于接收激光发射器发出的激光；机载数据处理终端用于处理激光光靶采集到的位置信息进行后端优化，得到锚护工作臂的位姿误差量，然后进行误差补偿和油缸移动量和旋转量的计算，根据计算结果校准锚护工作臂。本发明占用空间小，操作简单，能够实现快速校准。技术研发人员：李世杰,郭治富,包振兴,张胜达,马亚丽,梁玉芳,邢晨,张磊,曹兴受保护的技术使用者：中国煤炭科工集团太原研究院有限公司技术研发日：技术公布日：2024/6/2