基于受限优化和力反馈引导的水下电动机械臂遥操作方法

本发明涉及了一种水下电动机械臂遥操作方法，具体涉及一种基于受限优化和力反馈引导的水下电动机械臂遥操作方法。

背景技术：

1、随着海洋开发技术的发展，例如海洋渔业考察、石油探测、海底管道铺设和维修、深海采样等水下作业大规模增多。尽管现代机器人技术已经得到长足的发展，由全自主式机器人智能作业的难度仍然极大。因此，用于水下作业的遥操作系统对提升水下作业装备在复杂未知的深海环境中的作业能力具有重要意义。

2、遥操作系统是指操作者操作主端机器人，通过通信通道向从端机器人发送命令信号，从端机器人跟踪接收的信号执行作业任务，并通过通信通道将环境信息反馈给操作者的机器人系统。

3、在遥操作系统应用中，从端执行装置不可避免地会受到各种运动学以及动力学约束一旦违反此类约束，会造成整个运动系统失稳以及系统硬件装置损坏等问题。而在遥操作系统中，从端机器人的命令轨迹是由操作者操作主端机械臂在线生成的，其数学描述不可提前获悉，因此现有的规划器计算高效性和可靠性难以保证，从而恶化遥操作系统稳定性。

技术实现思路

1、为了解决背景技术中存在的问题，本发明所提供一种基于受限优化和力反馈引导的水下电动机械臂遥操作方法。方法用于处理遥操作系统中从端水下电动机械臂的多级物理约束并提供给人类操作者力反馈引导，引入自主约束规划器，以帮助人类操作员减少身体/精神工作量，提高遥操作系统的安全性和高效性。

2、本发明采用的技术方案是：

3、本发明的基于受限优化和力反馈引导的水下电动机械臂遥操作方法，包括：

4、第一步：根据从端水下电动机械臂的期望末端位置，使用逆运动学求解方法获得从端水下电动机械臂的期望关节角位移；逆运动学求解具体为几何法。

5、第二步：考虑从端水下电动机械臂的多级物理约束，建立基于受限优化的从端水下电动机械臂的从端约束规划器，将从端水下电动机械臂的期望关节角位移输入从端约束规划器中，从端约束规划器输出从端水下电动机械臂的满足多级物理约束的期望轨迹并输入从端水下电动机械臂的从端控制器中控制从端水下电动机械臂的运行，以保证从端位置跟踪性能。

6、第三步：建立从端水下电动机械臂的从端运动学模型，将从端水下电动机械臂在运行中的实际关节角位移输入从端运动学模型中，从端运动学模型输出从端水下电动机械臂的实际末端位置。

7、第四步：考虑从端水下电动机械臂末端位置规划误差，建立基于弹簧阻尼的从端水下电动机械臂的虚拟引导力反馈，将从端水下电动机械臂的期望末端位置和实际末端位置输入虚拟引导力反馈中，虚拟引导力反馈输出从端水下电动机械臂的从端虚拟反馈力。

8、第五步：建立主端机械臂的主端运动学模型，将从端虚拟反馈力输入主端运动学模型中，主端运动学模型输出主端机械臂的加速度，进而获得主端机械臂的末端位置。

9、第六步：建立主从端位置映射模型，将主端机械臂的末端位置输入主从端位置映射模型中，主从端位置映射模型输出从端水下电动机械臂经位置映射后的期望末端位置，返回第一步，将从端水下电动机械臂经位置映射后的期望末端位置使用逆运动学求解方法获得从端水下电动机械臂的下一期望关节角位移，继续进行步骤循环完成闭环，最终实现水下电动机械臂的遥操作。从端虚拟反馈力和通过从端水下电动机械臂经位置映射后的期望末端位置均通过通信通道在主从端间传递。

10、所述的第二步中，基于受限优化的从端水下电动机械臂的从端约束规划器，具体如下：

11、

12、s,t:η-≤x≤η+

13、其中，x表示角加加速度级优化变量，表示从端水下电动机械臂的角加加速度；ω表示优化等效系数；η+和η-分别表示角加加速度级优化变量x的上下界。

14、从端约束规划器输出的从端水下电动机械臂的满足多级物理约束的期望轨迹包括期望关节角位移的约束值角速度的约束值和角加速度的约束值

15、基于受限优化的从端约束规划器包括优化问题建立、优化目标转化、约束级统一和约束规划更新四部分，具体如下：

16、1)优化问题建立：

17、考虑到从端水下电动机械臂的多级物理约束(关节角位移qs、角速度角加速度和角加加速度的约束)，采用角加加速度级配置转换策略对系统状态约束进行处理。为了使从端水下电动机械臂在任何时候都能达到任意位形并消耗最少的能量，可以建立如下优化问题：

18、

19、

20、

21、

22、

23、其中，和分别为从端水下电动机械臂实际关节多级物理约束的上限和下限，qsd为从端水下电动机械臂的期望关节角位移。

24、由于优化问题中的不等式约束是多级的，因此可以使用以下两个性质进行统一约束目标转化和约束级统一：

25、性质1.对于任意时变的实数w(t)，当正参数ξ＞＞0足够大，且时间t→∞，则w(t)→0在物理和数学意义上等价于

26、性质2.对于任意时变的实数w(t)，当正参数ξ＞＞0足够大，且时间t→∞，则w(t)≤0在物理和数学意义上等价于

27、2)优化目标转化：

28、定义角位移级误差函数δp＝qs-qsd，δp→0等价于可得：

29、

30、其中，ξ≥0用于影响δp的收敛速率。

31、由此可将式(1a)中的角位移级优化目标转化为等效角速度级优化目标：

32、

33、类似地，定义角速度级误差函数则δv→0等价于可得：

34、

35、由此可将式(3)中的角速度级优化目标转化为等效角加速度级优化目标：

36、

37、进一步地，定义角加速度级误差函数则δa→0等价于可得：

38、

39、由此可将式(5)中的角加速度级优化目标转化为等效角加加速度级的二次优化目标：

40、

41、其中，表示角加加速度级优化变量，表示优化等效系数。

42、则根据式(2)-式(7)，将式(1a)中角位移级优化目标转化为式(7)所示的等效角加加速度级的二次优化目标。

43、3)约束级统一：

44、对于式(1b)中的角位移级约束，设计一个足够大的角位移正参数ξp，则式(1b)等价于：

45、

46、同样由式(8)继续导出，则式(1b)中的角位移级约束最终可以转化为：

47、

48、其中，表示由角位移级约束转化得的角加加速度级约束下界，表示由角位移级约束转化得的角加加速度级约束上界。

49、同理，式(1c)中的角速度级约束和式(1d)中的角加速度级约束均可转换为式(10)和式(11)中的角加加速度级约束：

50、

51、其中，表示由角速度级约束转化得的角加加速度级约束下界，表示由角速度级约束转化得的角加加速度级约束上界，ξv表示角速度正参数。

52、

53、其中，表示由角加速度级约束转化得的角加加速度级约束下界，表示由角加速度级约束转化得的角加加速度级约束上界，ξa表示角加速度正参数。

54、则根据式(8)-式(11)，式(1b)-(1d)中的多级物理约束转化为统一的角加加速度级约束其中，统一约束上下界η+和η-定义为：

55、

56、

57、因此，式(1)的优化问题就可以转化为标准的二次规划问题:

58、

59、s,t:η-≤x≤η+   (14b)

60、4)约束更新：

61、则在每个采样周期内，从端水下电动机械臂的期望关节角位移qsd可以通过运动学逆解实时更新。通过上述推导，将原来的具有各级物理约束的优化问题转化为标准的二次规划问题。因此，通过在线解析解求解该二次规划问题，可以得到满足各级物理约束的从端水下电动机械臂的期望关节角位移角速度和角加速度

62、所述的优化等效系数ω具体如下：

63、

64、其中，ξ表示优化参数，ξ≥0，用于影响δp的收敛速率；qs、和分别表示从端水下电动机械臂的关节角位移、角速度和角加速度，qsd表示从端水下电动机械臂的期望关节角位移。

65、所述的角加加速度级优化变量x的上界η+和下界η-具体如下：

66、

67、

68、

69、

70、

71、

72、

73、

74、其中，p+和p-分别表示基于角位移级约束的角加加速度级约束上界和下界；v+和v-分别表示基于角速度级约束的角加加速度级约束上界和下界，ξv表示角速度正参数；a+和a-分别表示基于角加速度级约束的角加加速度级约束上界和下界，ξa表示角加速度正参数；和分别表示从端水下电动机械臂的实际角加加速度的上限和下限。

75、所述的第三步中，从端水下电动机械臂的从端运动学模型具体如下：

76、xs＝[xs,ys,zs]t

77、

78、其中，xs表示从端水下电动机械臂的实际末端位置，xs,ys,zs分别表示从端水下电动机械臂的从端末端位置的x轴、y轴和z轴坐标；qs1,qs2,qs3,……,qsn分别表示从端水下电动机械臂的第一、二、三、…n个关节角位移；ls1,ls2,ls3,……,lsn分别表示从端水下电动机械臂的第一、二、三、…n个连杆的长度。

79、水下电动机械臂关节角用qs＝[qs1,qs2,qs3,……,qsn]t表示，连杆长度用ls＝[ls1,ls2,ls3,……,lsn]t表示，n表示水下电动机械臂的关节数量。以水下电动机械臂第一个关节中心为坐标原点os0，竖直向上为z轴zs0，第一个关节角为0时第二个关节角的转动轴为y轴ys0，垂直于y轴的水平方向为x轴xs0建立从端三维坐标系。

80、从端水下电动机械臂在运行中的实际关节角位移qs包括从端水下电动机械臂的第一、二、三、…n个关节角位移。

81、所述的第四步中，为了实现操作者对从端水下电动机械臂运动状态的感知，并对操作者的操纵状态提供约束指导，设计基于从端水下电动机械臂工作空间和位置误差的弹簧阻尼力反馈生成算法；从端水下电动机械臂的虚拟引导力反馈具体如下：

82、

83、fe＝ke[arctan(kp-1(xsd-xs))]

84、

85、

86、

87、其中，fs表示从端水下电动机械臂的从端虚拟反馈力；fe和fb分别表示误差引导力和虚拟边界弹簧阻尼力；xs和xsd分别表示从端水下电动机械臂的实际末端位置和期望末端位置，xu表示从端预设可达工作空间边界；ke表示误差引导力幅值参数；kp表示常比例映射系数矩阵；kb表示虚拟边界弹簧阻尼力fb的弹簧模量；xb表示从端工作空间边界；f0表示虚拟边界弹簧阻尼力fb的阻尼系数；表示从端水下电动机械臂的实际末端位置xs的单位向量。

88、所述的第五步中，主端机械臂的主端运动学模型具体如下：

89、

90、

91、其中，fh表示主端机械臂操作者的施加控制力；fs表示从端水下电动机械臂的从端虚拟反馈力；mm表示主端机械臂的质量；表示主端机械臂的加速度；zh表示主端机械臂操作者的阻抗系数。

92、根据主端机械臂的加速度经过两次积分器1/s积分，获得主端机械臂的末端位置xm。

93、所述的第六步中，主从端位置映射模型具体如下：

94、

95、其中，表示从端水下电动机械臂经位置映射后的期望末端位置；kp表示常比例映射系数矩阵；主端机械臂的末端位置xm；tp表示位置平移向量。

96、本发明的有益效果是：

97、1、本发明设计的基于受限优化的从端约束规划器可以保证从端系统满足水下电动机械臂的多级物理约束，提高水下电动机械臂遥操作系统的安全性和高效性。

98、2、本发明设计的虚拟引导力反馈，能够为人类操作员提供指导，帮助人类操作员减少身体/精神工作量。