一种基于SVD分解法的机械臂末端姿态调整方法

本发明属于机器人，具体的，涉及一种基于svd分解法的机械臂末端姿态调整方法，目的是对机械臂进行调平与定位。

背景技术：

1、变电站是电力传输的枢纽，其安全可靠的运行与国民经济建设和电气化铁路交通系统的安全运行息息相关。受变电站户内设备体型小、运动空间狭窄等因素限制，站内设备操作大多依靠人工进行，操作人员不仅费时费力，而且面临较大的劳动风险。针对此种情况，为了实现变电所无人化管理，许多变电所引入移动机械臂代替操作人员操作变电所内的设备，如对电气柜上的按钮下达指令，但在实际应用过程中，机器人很难精准操作目标按钮，常常因为机器人无法正确完成操作过程而引起更大的安全问题，因此，为了解决机械臂姿态调平和定位，精准按下按钮开关完成操作过程的问题，本发明提出一种机械臂末端姿态调整的方法，利用可靠的取像装置，以达到通过机器人技术代替人工对设备进行精准操作的目的。

2、中国发明专利文献cn115042175a于2022年9月13日公开的《一种机器人机械臂末端姿态的调整方法》采用了一种机器人机械臂末端姿态的调整方法，该方法虽然做到了机械臂末端的目标姿态的调整，但受到必须张贴标定板的限制，使用范围较为局限，难以适应多个电气柜多个位置情况下机器人机械臂末端姿态的调整。

3、在机器人作业中，机械臂末端姿态的准确性，直接影响着机器人是否能精确完成操作。其中，对于机器人操作电气柜按钮技术的研究还存在以下技术问题：

4、1、移动小车到达目标电气柜操作位置，定位不精确，存在定位误差，小车上面机械臂的初始位姿及按钮拍照位姿和电气柜操作面板因此产生一定的倾斜，使得机械臂无法精确地操作目标按钮。

5、2、受电气柜的限制，电气柜操作面板可能与地面不垂直，存在一定的倾斜，机械臂到达按钮拍照位姿时，机械臂末端轴垂面和电气柜操作面板并不平行，因而在随后的相机拍照识别和按钮定位过程中，机械臂操作总是存在偏差，无法更为精确的完成按下目标按钮的过程。

技术实现思路

1、针对现有技术的不足，本发明提供了一种基于svd分解法的机械臂末端姿态调整方法，能够使机械臂根据电气柜待操作面进行调平以及待操作点进行定位，进而精准按下按钮。

2、本发明的目的是这样实现的，本发明提供了一种基于svd分解法的机械臂末端姿态调整方法，本调整方法涉及的系统包括机器人、agv小车和电气柜；所述机器人包括6自由度机械臂、末端执行器和深度相机，所述6自由度机械臂包括6个旋转关节和机械臂基座，所述机械臂基座安装在agv小车上，所述6个旋转关节由机械臂基座开始依次为旋转关节一、旋转关节二、旋转关节三、旋转关节四、旋转关节五、旋转关节六，且该六个旋转关节依次分别绕第一关节轴、第二关节轴、第三关节轴、第四关节轴、第五关节轴、第六关节轴旋转，其中，第四关节轴和第五关节轴相互垂直，第五关节轴和第六关节轴相互垂直；所述末端执行器和深度相机均安装在6自由度机械臂的末端上，深度相机的光轴与第六关节轴平行且方向一致，深度相机跟随6自由度机械臂运动；所述电气柜的操作面板上有待操作的按钮，将该按钮的几何中心记为待操作点p，在待操作点p的上方设有两个指示灯，该两个指示灯处于同一水平线上，且分别记为指示灯1和指示灯2；

3、所述调整方法包括以下步骤：

4、步骤1，定义像素坐标系、图像坐标系、相机坐标系、机械臂末端坐标系、机械臂末端执行器坐标系和机械臂基座坐标系，并使用深度相机进行相机标定；

5、所述像素坐标系以图像左上角为原点建立的以像素为单位的直角坐标系，包括互相垂直的像素u轴和像素v轴，像素u轴上的横坐标为像素在其图像中的列数，方向为平行于图像面向右，像素v轴上的纵坐标为像素在其图像中的行数，方向为垂直于像素u轴向下；所述图像坐标系以相机光轴与图像平面的交点为原点建立的坐标系，包括互相垂直的图像x轴和图像y轴，图像x轴与像素u轴平行且方向一致，图像y轴与像素v轴平行且方向一致；所述相机坐标系是以相机光心为原点建立的坐标系，包括相机xc轴、相机yc轴和相机zc轴，其中，相机zc轴为相机光轴且取摄影方向为正方向，相机xc轴与像素u轴平行且方向一致，相机yc轴与像素v轴平行且方向一致；所述机械臂末端坐标系以旋转关节六末端端面中心为原点建立的坐标系，包括机械臂末端xe轴、机械臂末端ye轴和机械臂末端ze轴，其中，机械臂末端ze轴与相机光轴平行且方向一致，机械臂末端xe轴与相机xc轴平行且方向一致，机械臂末端ye轴与相机yc轴平行且方向一致；所述机械臂末端执行器坐标系以执行器末端端面中心为原点建立坐标系，包括末端执行器xt轴、末端执行器yt轴和末端执行器zt轴，其中，末端执行器zt轴与机械臂末端ze轴平行且方向一致，末端执行器xt轴与机械臂末端xe轴平行且方向一致，末端执行器yt轴与机械臂末端ye轴平行且方向一致；机械臂基座坐标系以机械臂基座中心点为原点建立的坐标系，包括机械臂基座xb轴、机械臂基座yb轴和机械臂基座zb轴，其中，机械臂基座zb轴和第一关节轴平行且方向一致，机械臂基座xb轴以agv小车正前方为正方向，机械臂基座yb轴由坐标系右手法则确定；

6、使用相机标定算法对深度相机进行相机标定，标定出的结果为深度相机的内参矩阵m，其表达式为：

7、

8、式中，fx为图像坐标系中图像x轴上的归一化焦距，fx＝f/dx，f为深度相机的焦距，单位为mm，dx为像素坐标系中的每个像素在图像x轴上的物理尺寸，单位为mm/pixel；fy为图像坐标系中图像y轴上的归一化焦距，fy＝f/dy，dy为像素坐标系中的每个像素在图像y轴上的物理尺寸，单位为mm/pixel；u0为图像坐标系的原点在像素坐标系中的横坐标，v0为图像坐标系的原点在像素坐标系中的纵坐标，即图像坐标系的原点在像素坐标系中的坐标为(u0，v0)；

9、步骤2，机器人移动到目标电气柜操作面板前，驱动6轴机械臂展开，通过深度相机对电气柜操作面进行拍摄，得到一张rbg三通道图像，以该rbg三通道图像中心点为中心做边长为n个像素点的正方形区域，得到n2个像素点，将其中任意一个记为像素点i，像素点i的像素坐标为(ui，vi)、深度值为di，i＝1，2，...，n2；

10、通过坐标变换得到该像素点i在相机坐标系下的相机坐标(xci，yci，zci)，并将n2个像素点i在相机坐标系下的相机坐标(xci，yci，zci)组成大小为n2×3的点集矩阵k，点集矩阵k的表达式如下：

11、

12、步骤3，将n2个像素点i拟合出一个平面q，并定义该平面q的平面单位法向量为f，该平面单位法向量f在相机坐标系下的坐标为(xfc，yfc，zfc)；对点集矩阵k进行中心化处理后进行svd分解，得到平面单位法向量f在相机坐标系下的坐标(xfc，yfc，zfc)；

13、步骤4，定义3×3的第一旋转矩阵用于描述机械臂末端坐标系相对于机械臂末端执行器坐标系的坐标旋转，定义3×1的第一平移向量用于描述机械臂末端坐标系相对于机械臂末端执行器坐标系的坐标平移，定义机械臂末端坐标系相对于机械臂末端执行器坐标系的位姿变换矩阵，并记为第一位姿变换矩阵其表达式为：

14、

15、定义3×3的第二旋转矩阵用于描述机械臂末端执行器坐标系相对于机械臂基座坐标系的坐标旋转，定义3×1的第二平移向量用于描述机械臂末端执行器坐标系相对于机械臂基座坐标系的坐标平移，定义机械臂末端执行器相对于机械臂基座坐标系的位姿变换矩阵，并记为第二位姿变换矩阵其表达式为：

16、

17、定义3×3的手眼标定旋转矩阵用于描述相机坐标系相对于机械臂末端坐标系的坐标旋转，定义3×1的手眼标定平移向量用于描述相机坐标系相对于机械臂末端坐标系的坐标平移，定义相机坐标系相对于机械臂末端坐标系的位姿变换矩阵，并记为手眼标定矩阵其表达式为：

18、

19、定义3×3的齐次变换旋转矩阵用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标旋转，定义3×1的齐次变换平移向量用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标平移，定义机械臂末端坐标系相对于机械臂基座坐标系的位姿变换矩阵，并记为齐次变换矩阵其表达式为：

20、

21、步骤5，根据步骤3求出的平面单位法向量f在相机坐标系下的坐标(xfc，yfc，zfc)、步骤4定义的齐次变换旋转矩阵手眼标定旋转矩阵得到平面单位法向量f在机械臂基座坐标系下的坐标(xfb，yfb，zfb)，其表达式为：

22、

23、步骤6，引入第一欧拉角α、第二欧拉角β和第三欧拉角γ，令第三欧拉角γ＝0，给出机械臂末端姿态矩阵rzyz(α，β，γ)，其表达式为：

24、

25、在机械臂末端ze轴方向上取一末端单位向量g，根据末端姿态矩阵rzyz(α，β，γ)得到末端单位向量g在机械臂基座坐标系下的坐标(xzyz，yzyz，zzyz)，其表达式为：

26、(xzyz，yzyz，zzyz)t＝rzyz(α，β，γ)[0，0，1]t＝[cosαsinβ sinαsinβ cosβ]t

27、步骤7，令步骤5所求平面单位法向量，在机械臂基座坐标系下的坐标(xfb，yfb，zfb)与末端单位向量g在机械臂基座坐标系下坐标(xzyz，yzyz，zzyz)相等，得到第一欧拉角α和第二欧拉角β的值，表达式如下：

28、(xfb，yfb，zfb)t＝(xzyz，yzyz，zzyz)t＝[cosαsinβ sinαsinβ cosβ]t

29、β＝arccos(zfb)

30、α＝a tan2(yfb，xfb)

31、式中，atan2()是双变量反正切函数，arccos()是反余弦函数；

32、驱动6自由度机械臂使机械臂将姿态调整到第一欧拉角α和第二欧拉角β所描述的姿态下；

33、步骤8，通过深度相机获取电气柜中的指示灯1和指示灯2的几何中心的像素坐标，并分别记为(ul，vl)和(ur，vr)，根据两点的像素坐标差求出相机xc轴相对于电气柜水平线的偏移角度angle，表达式如下：

34、angle＝arctan|(vl-vr)/(ul-ur)|

35、式中，arctan()是反正切函数；

36、再对机械臂末端调整偏移角度angle，使得两个指示灯在图像坐标系中处于同一水平线，则机械臂末端姿态调整结束；

37、步骤9，在步骤8机械臂末端姿态调整完毕后，通过深度相机获取电气柜中待操作点p的像素坐标(up，vp)和深度值dp，并通过坐标变换公式计算出待操作点p在相机坐标系下的相机坐标(xpc，ypc，zpc)；

38、步骤10，根据待操作点p在相机坐标系下的相机坐标(xpc，ypczpc)、手眼标定旋转矩阵第一旋转矩阵第二旋转矩阵计算出待操作点p相对于机械臂基座坐标系的位置坐标(xpb，ypb，zpb)，表达式如下：

39、

40、步骤11，驱动6自由度机械臂使其末端执行器到达待操作点p相对于机械臂基座坐标系的位置坐标(xpb，ypb，zpb)处，随后末端执行器按下按钮，完成操作；

41、步骤12，6自由度机械臂恢复到不操作时的位姿，等待下次操作的指令。

42、优选地，步骤2所述对像素点i进行坐标变换得到该像素点i在相机坐标系下的相机坐标(xci，yci，zci)，其坐标变换的公式为：

43、

44、优选地，步骤9所述通过坐标变换公式计算出待操作点p在相机坐标系下的相机坐标(xpc，ypc，zpc)，其坐标变换公式为：

45、

46、优选地，步骤3的实现过程如下：

47、步骤3.1，将n2个像素点i拟合出一个平面q，定义该平面q的平面单位法向量为f，且该平面单位法向量f在相机坐标系下的坐标为(xfc，yfc，zfc)；

48、步骤3.2，定义一个点集中心点j，并将点集中心点j在相机坐标系中的坐标记为其表达式如下：

49、

50、对点集矩阵k进行中心化处理，得到一个中心化数据矩阵b，其表达式为：

51、

52、步骤3.3，设平面q过点集中心点j，则点集中心j和其他n2个像素点i形成的向量与平面q的平面单位法向量f正交，其正交表达式bft如下：

53、

54、对正交表达式bft进行求解，即对中心化数据矩阵b进行svd分解，表达式如下：

55、b＝kdwt

56、其中，d是n2×3的对角矩阵，主对角线上元素为奇异值，k是n2×n2的第一酉矩阵，由方阵bbt的所有特征向量组成，w是3×3的第二酉矩阵，由方阵btb的所有特征向量组成；

57、获取第二酉矩阵w的最后一列为特征向量且特征向量的坐标表示为求解出平面单位法向量f在相机坐标系下的坐标(xfc，yfc，zfc)的具体数值，其表达式分别为：

58、

59、

60、

61、与现有技术相比，本发明的有益效果如下；

62、1、本发明采用调整方法用于机器人机械臂操作电气柜上的按钮的位置调整，解决了由于agv小车定位误差和电气柜做工限制带给机器人机械臂末端姿态的影响，从而使机器人机械臂更为精确地操作电气柜按钮。

63、2、本发明采用svd分解法的机械臂末端姿态的调整方法，校正过程简单快速，易于实施，不需要标定板，满足机器人机械臂操作电气柜按钮的需求。