一种基于任务解耦的机器人远心运动控制方法

本发明涉及机器人运动控制领域，具体说是一种基于任务解耦的机器人远心运动控制方法。

背景技术：

1、在机器人执行任务过程中，往往需要把持器械在环境的约束下利用器械末端进行操作。为不使器械与环境约束产生碰撞干涉，需要使用特殊方法保证器械杆始终经过约束区间。具体表现为对器械杆体远端的某个点(远心点)施加远程约束，比如按照一定的规律运动或不动(点约束)、只能在一定范围的空间内无规律活动(空间约束)。远程约束限制器械的两个自由度，使器械杆体只可以沿轴向滑动，和以远心点为支点进行调向。

2、对远心点的约束方法主要通过机械方式实现(如手术机器人)和控制方法实现(如工业机器人)。但机械机构的实现方式灵活性差，不能很好地适应动态约束和活动远心点的情况。而目前控制方法实现的约束则存在着不稳定的问题。因此，需要规划出一种灵活稳定的软远心约束控制方法。

技术实现思路

1、本发明目的是提供一种通过任务解耦的方式实现稳定的远心运动控制方法。通过示教模式将器械末端移动至远程约束点或约束空间边界，或通过预先输入获得其位置；远心约束(rcm)被作为机械臂的第一任务，机械臂器械的末端位置控制作为第二任务并通过rcm雅克比的零空间实现远程点约束下的远心运动；通过引入速度阻尼，把控制问题转化为二次规划问题以求解实现远程空间约束下的远心运动；器械的运动分解为调向运动、插入运动和远心点运动以实现对于器械的控制，避免了机器人控制过程中的算法奇异。

2、本发明为实现上述目的所采用的技术方案是：一种基于任务解耦的机器人远心运动控制方法，包括以下步骤：

3、1)标定获取当前远程约束在机器人的机械臂坐标系下的位置，并输入至控制器中；

4、2)通过定义机器人的机械臂器械的插入深度参数实现远心点在器械上的定义和表示，并实现器械远心点速度雅克比矩阵的表示形式；

5、3)构建机械臂器械的远心运动控制模型：

6、根据器械远心点速度雅克比矩阵的表示形式，在远程点约束下，通过将控制任务解耦为远心约束控制和器械末端位置控制，完成建立机械臂器械的远心运动控制模型；

7、根据器械远心点速度雅克比矩阵的表示形式，在远程空间约束下，通过将控制任务化为二次规划问题，完成建立机械臂器械的远心运动控制模型；

8、4)通过将机械臂器械的运动分解为调向运动、插入运动和远心点运动实现对于器械末端的位置控制。

9、所述机械臂器械固设于机械臂末端关节上；所述机械臂器械为杆体结构。

10、所述远程约束包括远程点约束和远程空间约束；所述远程点约束为远心点按照设定规律运动或不动；所述远程空间约束为远心点只能在设定范围的空间内随机活动。

11、步骤2)中，所述通过定义插入深度变量结合机械臂正运动学，得到在器械上的远心点的位置，具体为：

12、设定机械臂的6个关节角度为qr＝[q1,q2,...,q6]t；

13、设定插入深度变量λ，则控制变量设为：qu＝[q1,q2,...,q6,λ]t；

14、设定器械杆体末端相对机械臂坐标系的位置为ps1，器械杆体安装端相对机械臂坐标系的位置为ps2，则有：

15、ps1＝p1+p，ps2＝p2+p

16、其中，p1为器械杆体末端相对机械臂末端的位置，p2为器械安装端相对机械臂末端的位置，p为机械臂末端相对机械臂坐标系的位置；

17、器械杆体上的远心点表示为：

18、prcm＝ps2+λ(ps1-ps2)

19、0≤λ≤1

20、其中，prcm为远心点的实际位置，ps1为器械的末端位置，ps2为器械的安装端位置，λ为插入深度变量。

21、所述步骤2)中，具体为：

22、远心点prcm的微分形式为：

23、

24、则远心点的雅克比矩阵jrcm为：

25、

26、其中，js1和js2分别为ps1和ps2雅可比矩阵，表示机械臂的关节速度，js1,v为末端位置的线速度雅克比矩阵，js2,v为安装端位置的线速度雅克比矩阵。

27、步骤3)中，根据器械远心点速度雅克比矩阵的表示形式，在远程点约束下，通过将控制任务解耦为远心约束控制和器械末端位置控制，完成建立机械臂器械的远心运动控制模型，具体为：

28、设定远程约束的位置为pc，通过人为设定或测量标定获得，并将pc输入到控制器当中；在实时控制中，pc可以动态变化以适应任务需求；

29、对于远程点约束，定义远心点与远程约束点的位置误差ercm为：

30、ercm＝pc-prcm

31、其中，pc为远程约束点在机械臂坐标系的位置，prcm为远心点的位置；

32、将器械末端位置控制和远心约束控制在运动学层面分开进行控制，远心约束控制作为第一任务，末端位置控制作为第二任务，末端位置控制通过雅克比矩阵的零空间的映射，以避免对远心约束的干扰；

33、则机械臂器械的远心运动控制模型，即机械臂的关节速度表示为：

34、

35、其中，为雅可比矩阵jrcm的广义逆，ercm是远心点与远程约束点的位置误差，i为7×7单位矩阵，为目标机械臂关节角速度，为插入运动的速度。

36、步骤3)中，根据器械远心点速度雅克比矩阵的表示形式，在远程空间约束下，通过将控制任务化为二次规划问题，完成建立机械臂器械的远心运动控制模型，具体为：

37、对于远程空间约束，将器械末端位置控制和远心约束控制在运动学层面分开控制，当远心点靠近远程约束空间的边界时，对机械臂器械施加速度阻尼；

38、则机械臂器械的远心运动控制模型，即远心约束控制表示为：

39、

40、其中，ξ为收敛系数，d为远心点与边界的距离，为机械臂控制变量的速度；

41、同时，实现机械臂器械的控制，当最小时，利用解二次规划算法求解器械的远心运动，其中，为目标机械臂关节角速度，为插入运动的速度。

42、步骤4)中，所述机械臂器械的运动分解为调向运动、插入运动和远心点运动，具体为：

43、(1)对于调向运动，若以机械臂器械末端指向为控制目标，器械的指向误差函数定义为：

44、

45、其中，rs1,z表示器械的末端位置ps1到安装端位置ps2的刚性段的目标指向，fe(qu)为误差函数；

46、(2)若以机械臂器械末端位置为控制目标，机械臂器械末端的位置误差函数定义为：

47、fe(qu)＝||pt-ps2||

48、其中，pt为机械臂器械末端的目标位置；

49、通过求解误差函数的梯度向量，机械臂实现指向控制时的关节速度为：

50、

51、其中，ke为正系数，为指向误差的梯度；

52、(3)对于插入运动，表示为：

53、

54、其中，kλ为正系数，λd为目标插入距离，λ为插入深度变量；

55、(4)对于远心点的运动，其初始位置pc设定后，由控制器给定其目标的运动轨迹，为定点不动或者按控制器的指令移动；

56、(5)将目标器械末端速度对应的机械臂关节角速度及插入运动的速度代入至机械臂器械的远心运动控制模型，完成实现对于机械臂器械的控制。

57、本发明具有以下有益效果及优点：

58、1.本发明可以采用6自由度或冗余机械臂实现器械在远程约束下的末端控制，能够控制器械末端的4个自由度。

59、2.本发明的远程约束既可以为点约束，也可以为一定范围的空间约束；远程约束点或空间边界可以设置在任意位置，并且可以实时动态变化；可以实现高精度和稳定的器械末端的远心运动控制。

60、3.本发明的控制方式远心运动的精度高，对于器械的控制稳定性高和流畅度高。

61、4.本发明通过引入速度阻尼，把控制问题转化为二次规划问题以求解实现远程空间约束下的远心运动。

62、5.本发明的机械臂器械的运动分解为调向运动、插入运动和远心点运动以实现对于器械的控制，避免了机器人控制过程中的算法奇异。