本发明涉及机器人，尤其涉及一种基于旋转搜索策略的机械臂力控螺栓对准方法。
背景技术：
1、在接触网机械臂紧固螺栓场景中，通常需要先将机械臂末端套筒与螺栓对准。传统方式下，通过机械臂的位置控制设定路点直接对准螺栓。然而，在实际操作中，螺栓的角度很容易发生变化，如果采取简单的路点控制机械臂对准螺栓，很可能因为套筒与螺栓存在的角度差在对准过程中发生刚性碰撞导致机械臂急停，从而减小了螺栓对准的安全性和螺栓对准的成功率。
技术实现思路
1、为了解决传统螺栓对准方法中由于套筒和螺栓角度误差带来的对准成功率不高、安全性低的问题，本发明提出一种基于旋转搜索策略的机械臂力控螺栓对准方法，解决上述问题。
2、本申请公开了一种基于旋转搜索策略的机械臂力控螺栓对准方法，包括以下步骤：
3、s1、机械臂预对准，控制机械臂运动到预对准位置，并使机械臂套筒与螺栓同轴，无需要求套筒直线进入对准位置；
4、s2、机械臂直线运动接触螺栓，控制机械臂沿笛卡尔空间工具坐标系z轴正方向运动，并获取实时的力信息，当z轴负方向获取的力大于设定阈值f1时，停止机械臂运动；
5、s3、机械臂旋转搜索，控制机械臂绕笛卡尔空间工具坐标系z轴顺时针旋转，并获取实时的力信息，当z轴负方向获取的力小于设定阈值f2或绕z轴逆时针方向的力矩大于设定阈值t1时，停止机械臂旋转运动；
6、s4、机械臂直线对准进入螺栓，控制机械臂沿笛卡尔空间工具坐标系z轴正方向运动，并获取实时的力信息，当z轴负方向获取的力大于设定阈值f3时，停止机械臂运动。
7、优选的，所述笛卡尔空间工具坐标系与机械臂法兰坐标系同向，所述笛卡尔空间工具坐标系的坐标中心点位于机械臂套筒底面几何中心点。
8、优选的，所述s2包括以下步骤：
9、获取当前机械臂套筒相对于机械臂基座的坐标系关系，通过齐次变换矩阵表示为：
10、
11、其中，左上角3*3子矩阵表示旋转矩阵，右上角3*1子矩阵表示平移矩阵；
12、机械臂沿笛卡尔空间工具坐标系z轴正方向平移后相对于机械臂基座坐标系的位姿为：
13、
14、其中，
15、
16、式中a为机械臂沿z轴正方向的平移距离，且a>b，b为s1中机械臂到达预对准位置时，机械臂套筒与螺栓的距离；
17、控制机械臂沿直线路径运动至在机械臂运动至过程中，通过六维力传感器实时获取力信息，当六维力传感器获取的z轴负方向力大于设定阈值f1时，停止机械臂运动。
18、优选的，所述s2中采用恒力f4控制机械臂沿笛卡尔空间工具坐标系z轴正方向运动，且f4≥f1。
19、优选的，所述s3包括以下步骤：
20、获取当前机械臂套筒相对于机械臂基座的坐标系关系，通过齐次变换矩阵表示为：
21、
22、其中，左上角3*3子矩阵表示旋转矩阵，右上角3*1子矩阵表示平移矩阵；
23、由于螺栓为正六边形，且每个边对应圆心角为60°，所以可以保证无论相差多少角度，套筒都能在旋转不超过60°的角度后对准螺栓。机械臂绕笛卡尔空间工具坐标系z轴顺时针旋转60°后相对于机械臂基座坐标系的位姿为：
24、
25、其中，
26、
27、控制机械臂套筒旋转至在机械臂套筒旋转至过程中，通过六维力传感器实时获取力信息，当六维力传感器获取的z轴正方向的力小于设定阈值f2，或者当六维力传感器获取的沿z轴逆时针方向的力矩大于设定阈值t1时，停止机械臂旋转。
28、优选的，所述s3中采用恒力f4控制机械臂沿笛卡尔空间工具坐标系z轴正方向运动。
29、优选的，所述s4包括以下步骤：
30、获取当前机械臂套筒相对于机械臂基座的坐标系关系，通过齐次变换矩阵表示为：
31、
32、其中，左上角3*3子矩阵表示旋转矩阵，右上角3*1子矩阵表示平移矩阵；
33、机械臂沿笛卡尔空间工具坐标系z轴正方向平移后相对于机械臂基座坐标系的位姿为：
34、
35、其中，
36、
37、式中c为机械臂沿z轴正方向的平移距离，且c>d，d为s3中机械臂旋转结束时，机械臂套筒与螺栓的距离；
38、控制机械臂沿直线路径运动至在机械臂运动至过程中，通过六维力传感器实时获取力信息，当六维力传感器获取的z轴负方向力大于设定阈值f3时，停止机械臂运动，此时对准完成。
39、优选的，所述s4中采用恒力f5控制机械臂沿笛卡尔空间工具坐标系z轴正方向运动，且f5≥f3。
40、本发明的有益效果：
41、本发明通过六维力传感器反馈的信息来实时判断机械臂与螺栓的接触状态，并能够在机械臂末端套筒旋转搜索过程中，通过六维力传感器反馈的力信息判断套筒与螺栓是否对准，使得系统能够有效补偿螺栓与套筒之间可能存在的角度误差，从而显著提高了对准过程的成功率和操作安全性。
1.一种基于旋转搜索策略的机械臂力控螺栓对准方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于旋转搜索策略的机械臂力控螺栓对准方法，其特征在于，所述笛卡尔空间工具坐标系与机械臂法兰坐标系同向，所述笛卡尔空间工具坐标系的坐标中心点位于机械臂套筒底面几何中心点。
3.根据权利要求2所述的基于旋转搜索策略的机械臂力控螺栓对准方法，其特征在于，所述s2包括以下步骤：
4.根据权利要求3所述的基于旋转搜索策略的机械臂力控螺栓对准方法，其特征在于，所述s2中采用恒力f4控制机械臂沿笛卡尔空间工具坐标系z轴正方向运动，且f4≥f1。
5.根据权利要求4所述的基于旋转搜索策略的机械臂力控螺栓对准方法，其特征在于，所述s3包括以下步骤：
6.根据权利要求5所述的基于旋转搜索策略的机械臂力控螺栓对准方法，其特征在于，所述s3中采用恒力f4控制机械臂沿笛卡尔空间工具坐标系z轴正方向运动。
7.根据权利要求6所述的基于旋转搜索策略的机械臂力控螺栓对准方法，其特征在于，所述s4包括以下步骤：
8.根据权利要求7所述的基于旋转搜索策略的机械臂力控螺栓对准方法，其特征在于，所述s4中采用恒力f5控制机械臂沿笛卡尔空间工具坐标系z轴正方向运动，且f5≥f3。
