一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法及系统

摘要: 本发明属于机器人视觉伺服控制与双臂机器人协同操作,特别是涉及一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法及系统。背景技术:1、与单臂机器人相比,双臂机...
一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法及系统

本发明属于机器人视觉伺服控制与双臂机器人协同操作,特别是涉及一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法及系统。

背景技术:

1、与单臂机器人相比,双臂机器人具有更强的负载能力和操作灵巧性,但在实际任务中,被操作物体的初始位姿和环境条件存在着不确定性,仅依靠离线规划难以保证操作精度。因此,双臂机器人通常需要借助视觉传感器获取物体图像特征,并通过视觉伺服实现在线调节。然而,在双臂协同视觉伺服中,视觉误差通常定义在物体层,而控制命令需要分配至两条机械臂,且物体运动由双臂共同产生,使得视觉反馈调节与双臂协调控制之间存在明显耦合。

2、现有位置视觉伺服方法通常依赖完整位姿重建,图像特征可能偏离相机视场;图像视觉伺服方法直接调节图像误差,但可能存在图像雅可比奇异、局部极小值以及笛卡尔空间轨迹不理想等问题。基于单应性矩阵的视觉伺服能够利用平面视觉特征之间的单应关系重构相对旋转和部分平移几何信息,兼具图像误差反馈和几何解释性。然而,现有基于单应性矩阵的视觉伺服研究多集中于单机械臂系统,难以直接适用于双臂协同抓取场景。

3、此外,在高精度双臂协同操作任务中,控制系统不仅需要最终稳态误差较小,还需要对误差收敛过程和调节时间具有明确约束。预设时间性能控制的目标是在控制器设计阶段由用户直接给定时间参数,使视觉误差在指定时间内进入预设残差范围,并在该时间之后保持在允许精度区域内。与一般渐近收敛或有限时间收敛方法相比,预设时间性能约束更便于根据任务节拍、操作安全性和协同执行效率设定性能指标。然而,传统预设性能控制方法通常要求初始误差落在预先设定的性能边界内,而双臂视觉伺服任务的初始物体位姿、抓取构型和期望视觉参考往往随任务变化,初始误差可能难以满足固定性能边界的可行性条件。因此,亟需一种面向双臂机器人的视觉伺服控制方法,以在不依赖初始误差可行性条件的情况下实现预设时间视觉位姿调节。

技术实现思路

1、针对以上技术问题,本发明提供一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法及系统。

2、本发明解决其技术问题采用的技术方案是:

3、第一方面,本发明提供一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法,方法包括以下步骤:

4、s100:采集当前视觉标识平面的图像,获取当前视觉标识平面与固定期望视觉标识平面的对应特征点,基于相机内参矩阵估计图像单应性矩阵;

5、s200:基于相机内参矩阵将图像单应性矩阵转换为欧氏单应性矩阵,并对欧氏单应性矩阵进行分解,从分解得到的候选解中选取物理可行解,获得当前视觉标识平面相对于期望视觉标识平面的相对旋转参数;

6、s300:建立双臂机器人刚性抓取运动学关系,构建物体速度到双臂机器人关节速度的映射关系;

7、s400:基于相对旋转参数采用角轴表示构造旋转误差向量,并基于视觉标识中心点的图像坐标和深度信息构造扩展平移误差向量,基于旋转误差向量和扩展平移误差向量构造得到原始误差通道;

8、s500:针对各原始误差通道分别构造预设时间性能边界及其对应的障碍函数变换,将各原始误差通道转换为变换误差通道;

9、s600:根据各变换误差通道分别设计物体角速度控制律和物体平移速度控制律,并将组合得到的物体速度命令通过映射关系转换为双臂机器人关节速度命令,以驱动机器人实现预设时间视觉伺服调节。

10、优选地,s100包括:

11、s110:获取当前视觉标识平面与期望视觉标识平面上的至少四对非共线对应特征点,并将其像素齐次坐标分别记为和,其中,为特征点的序号索引,,且,为所选取的对应特征点对的总数量,为当前视觉标识平面上第个特征点的像素齐次坐标向量,分别为当前视觉标识平面上第个特征点在图像坐标系中的横坐标和纵坐标,为期望视觉标识平面上第个特征点的像素齐次坐标向量,分别为期望视觉标识平面上第个特征点在图像坐标系中的横坐标和纵坐标;

12、s120:在相机内参矩阵已知的条件下,采用最小二乘单应性矩阵估计方法求解图像单应性矩阵,并建立当前特征点与期望特征点之间的投影单应约束关系:

13、(1)

14、其中,为尺度因子;为欧氏单应性矩阵。

15、优选地,s200包括:

16、s210:对欧氏单应性矩阵进行尺度归一化,并采用基于奇异值分解的欧氏单应性矩阵分解方法,将分解为满足的候选解集合,其中为当前视觉标识平面相对于期望视觉标识平面的相对旋转矩阵;为尺度平移向量;为期望视觉标识平面的单位法向量;

17、s220:对欧氏单应性矩阵分解得到的候选解进行物理可行性筛选:首先剔除不满足特征点正深度约束和平面法向方向约束的候选解;当筛选后仍存在多个候选解时,根据相邻时刻分解结果的连续性,从剩余候选解中选取物理可行解,得到当前视觉标识平面相对于期望视觉标识平面的相对旋转矩阵、尺度平移向量以及期望视觉标识平面的单位法向量;其中,正深度约束是指分解结果对应的视觉特征点在相机坐标系下具有正深度;平面法向方向约束是指分解得到的单位法向量方向与期望视觉标识平面的法向方向一致;连续性是指当前时刻分解结果与上一时刻分解结果之间不存在突变。

18、优选地,s300包括:

19、s310:设双臂机器人关节向量为,其中和分别为左、右机械臂关节向量;对应的关节速度向量为,其中和分别为左、右机械臂关节速度向量;左、右机械臂末端速度分别为和,其合成速度为;

20、s320:设物体位姿为,物体速度为,其中为物体平移速度,为物体角速度,在双臂机器人基坐标系下,双臂机器人末端速度与关节速度满足:

21、(2)

22、其中,为双臂机器人末端雅可比矩阵,和分别为左、右机械臂末端雅可比矩阵,表示块对角矩阵;

23、s330:在两机械臂对物体保持刚性抓取且抓取点无相对滑移的情况下,机械臂末端速度与物体速度满足:

24、(3)

25、其中,为双臂抓取矩阵,和分别为左、右机械臂对应的抓取子矩阵,用于描述物体速度与各机械臂末端速度之间的刚性运动约束关系;

26、s340:由式(2)和式(3)可得到物体速度到双臂机器人关节速度的映射关系式:

27、(4)

28、其中,为的moore-penrose伪逆,符号表示moore-penrose伪逆运算,式(4)用于将物体层视觉伺服控制器输出的物体速度命令转换为双臂机器人关节速度命令,以驱动左、右机械臂协同完成期望的物体运动。

29、优选地,s400包括:

30、s410:基于相对旋转矩阵,采用角轴表示定义旋转误差向量:

31、(5)

32、其中,为旋转误差向量,为单位旋转轴,为当前视觉标识平面与期望视觉标识平面之间的相对旋转角,且满足,相对旋转角作为旋转角误差;

33、和由下式计算得到:

34、(6)

35、其中,表示矩阵的迹,表示由向量构造的反对称矩阵,当时,表示当前视觉标识平面与期望视觉标识平面不存在相对旋转误差;

36、s420:选取视觉标识中心点作为平移误差参考点,设当前视觉标识中心点的扩展平移向量为,期望视觉标识中心点的扩展平移向量为,其中,和分别表示当前与期望视觉标识中心点的扩展平移向量;和为当前视觉标识中心点的图像坐标,为当前视觉标识中心点相对于相机坐标系的深度;、和分别为期望视觉标识中心点的图像坐标和期望深度;定义扩展平移误差为:

37、(7)

38、其中,为扩展平移误差向量,、和分别定义为:

39、(8)

40、其中,为期望深度与当前深度之间的深度比,和分别为水平像素误差、垂直像素误差,为深度比例相关误差。

41、优选地,s500包括:

42、s510:对每一误差通道,设分别表示、、和,其中表示旋转角误差,、和分别表示水平像素误差、垂直像素误差和深度比例相关误差;

43、s520:给定设计参数、预设时间、目标残差精度上界以及缩放参数,其中用于调节误差归一化尺度,用于指定误差进入目标残差区域的时间,用于指定对应误差通道在时的目标残差精度,用于调节预设时间缩放函数的变化速率;定义性能边界生成函数为:

44、(9)

45、其中,为辅助变量,用于生成误差通道的性能边界;

46、s530:根据目标残差精度上界,计算预设时间缩放函数的终值:

47、(10)

48、其中,表示误差通道对应的缩放函数在预设时间之后保持的终值,使得;

49、s540:构造预设时间缩放函数:

50、(11)

51、其中,为误差通道对应的预设时间缩放函数,且满足,并在时保持为;

52、进一步定义障碍函数变换:

53、(12)

54、其中,为归一化误差,为中间变量,为障碍函数变换误差;

55、s550:通过式(12),将原始误差通道转换为变换误差通道;由于,初始时刻对应的性能边界在极限意义下趋于无穷大,因此该性能变换不要求初始误差预先落入有限性能边界内。

56、优选地,s600中根据各变换误差通道分别设计物体角速度控制律和物体平移速度控制律,包括:

57、s610:对于旋转通道中的旋转角误差,根据式(12)获得、和,定义辅助变量:

58、(13)

59、其中,为旋转误差归一化项,为旋转通道障碍函数变换项;

60、s620:定义类雅可比矩阵为:

61、(14)

62、其中,为单位矩阵,为辛格函数,且;

63、s630:选取正设计参数,设计物体角速度控制律为:

64、(15)

65、其中,为物体角速度命令,为旋转通道预设时间缩放函数,为双臂机器人基坐标系到相机坐标系的旋转矩阵,为的一阶导数;

66、s640:对平移通道,定义:

67、(16)

68、其中,为扩展平移误差对应的障碍函数变换误差向量,分别为水平像素误差通道、垂直像素误差通道和深度比例相关误差通道对应的障碍函数变换误差,为归一化误差向量,分别为各平移误差通道对应的归一化误差,为平移通道预设时间缩放函数导数组成的向量,分别为各平移误差通道对应的预设时间缩放函数的一阶导数;

69、s650:定义矩阵:

70、(17)

71、其中,为平移通道障碍函数变换项的对角矩阵,分别对应水平像素误差通道、垂直像素误差通道和深度比例相关误差通道的障碍函数变换项,表示对角矩阵,为平移通道辅助增益矩阵,为平移通道辅助增益矩阵的对角元素,分别对应各平移误差通道的预设时间缩放函数与误差归一化项的乘积,且对有:

72、(18)

73、其中,为各平移误差通道的障碍函数变换项,为各平移误差通道在障碍函数变换中定义的中间变量,为各平移误差通道的误差归一化项;

74、s660:设,其中和为当前视觉标识中心点的图像坐标;设为由相机外参确定的矩阵,为的第三行,定义扩展交互矩阵:

75、(19)

76、其中,为相机内参矩阵,取的前三列构成子矩阵,并设,其中、、为正设计参数;

77、设计物体平移速度控制律为:

78、(20)

79、其中,为物体平移速度命令,为当前视觉标识中心点相对于相机坐标系的深度,为的moore-penrose伪逆,为的逆矩阵,为的逆矩阵,表示hadamard逐元素乘积。

80、优选地,s600中将组合得到的物体速度命令通过映射关系转换为双臂机器人关节速度命令,包括:

81、将式(15)得到的物体角速度命令和式(20)得到的物体平移速度命令组合为物体速度命令:

82、(21)

83、将式(21)代入式(4),得到双臂机器人关节速度命令:

84、(22)

85、控制器将分解为左臂关节速度命令和右臂关节速度命令,并分别发送至左、右机械臂伺服执行单元,使两条机械臂共同产生期望物体速度,实现被操作物体相对于期望视觉参考的预设时间视觉伺服位姿调节。

86、第二方面,本发明提供了一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制系统,用于执行一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法的步骤,包括:

87、图像单应性矩阵估计模块,用于采集当前视觉标识平面的图像,获取当前视觉标识平面与固定期望视觉标识平面的对应特征点,基于相机内参矩阵估计图像单应性矩阵;

88、相对旋转参数确定模块,用于基于相机内参矩阵将图像单应性矩阵转换为欧氏单应性矩阵,并对欧氏单应性矩阵进行分解,从分解得到的候选解中选取物理可行解,获得当前视觉标识平面相对于期望视觉标识平面的相对旋转参数;

89、运动学映射模块,建立双臂机器人刚性抓取运动学关系,构建物体速度到双臂机器人关节速度的映射关系;

90、误差构造模块,用于基于相对旋转参数采用角轴表示构造旋转误差向量,并基于视觉标识中心点的图像坐标和深度信息构造扩展平移误差向量,基于旋转误差向量和扩展平移误差向量构造得到原始误差通道;

91、变换模块,用于针对各原始误差通道分别构造预设时间性能边界及其对应的障碍函数变换,将各原始误差通道转换为变换误差通道;

92、控制模块,用于根据各变换误差通道分别设计物体角速度控制律和物体平移速度控制律,并将组合得到的物体速度命令通过映射关系转换为双臂机器人关节速度命令,以驱动机器人实现预设时间视觉伺服调节。

93、第三方面,本发明提供了一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时实现一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法的步骤。

94、上述一种基于单应性矩阵的双臂机器人预设时间视觉伺服控制方法及系统,通过单应性矩阵分解获取视觉标识平面间的相对旋转参数,结合视觉标识中心点的图像坐标与深度信息构造旋转误差和扩展平移误差,无需对被操作物体进行完整的三维位姿重建,具有良好的环境适应性和通用性;针对原始误差通道分别构造预设时间性能边界及障碍函数变换,将受约束的原始误差转换为无约束的变换误差,使误差收敛时间和稳态精度可预先设定,且不要求初始误差落入有限边界内,显著提升了视觉伺服控制的瞬态性能和稳态精度;通过双臂机器人刚性抓取运动学映射将物体速度命令转换为关节速度命令,实现了物体层控制律到关节空间的完整映射,使两机械臂协同完成被操作物体的预设时间视觉伺服位姿调节,适用于多种双臂协同作业场景。