基于力位混合控制的冗余偏置机械臂末端柔顺控制方法

本发明属于生产制造领域，特别涉及一种基于力位混合控制的冗余偏置机械臂末端柔顺控制方法。

背景技术：

1、传统的物体表面擦拭大多是通过人工手动完成，当进行表面清洁擦拭操作时，人手可以根据实际情况施加不同的操作力，使得擦拭力具有一定清洁作用的同时又不破坏所擦拭物体的表面。

2、机械臂程序式的可重复操作性，成为替代人工解放双手的有效工具。与人工的灵活操作不同，机械臂的机械特性使其自身具有很大的刚度，程序式特性也使其无法根据环境自主灵活机变，如果不控制机械臂末端的施加力，在擦拭过程中很容易造成机械臂和擦拭物的双向破坏。

3、因此，如何控制机械臂接触式精细操作过程中的目标接触力尤为重要。

技术实现思路

1、发明目的：为了克服以上不足，本发明的目的是提供一种基于力位混合控制的冗余偏置机械臂末端柔顺控制方法，将位置控制器与力控制器结合设计了力位混合控制控制器，能够对机械臂末端的运行轨迹以及擦拭力进行控制，有效保证机械臂末端沿规定擦拭轨迹运动的同时实现机械臂末端在擦拭表面保持恒定清洁力。

2、技术方案：为了实现上述目的，本发明提供了一种基于力位混合控制的冗余偏置机械臂末端柔顺控制方法，包括：

3、s1）：根据机械臂结构，确定机械臂参数，并根据标准准则建立机械臂的标准坐标系，设置机械臂运动与力极限参数，建立冗余偏置机械臂的运动学模型；

4、按如下规则建立坐标系：

5、：绕轴由轴转向轴的关节角，按右手规则；

6、：从第坐标系的原点到轴和轴的交点沿轴的距离；

7、：从轴和轴的交点到第坐标系原点沿轴的偏置距离，即和两轴间的最小距离；

8、：绕轴由轴转向轴的偏角，按右手规则；

9、s2）：根据步骤s1）中建立的动力学模型，分别设计各方向的力控制器与位置控制器，即通过雅克比矩阵建立机械臂工作空间与机械臂关节空间的对应关系，根据规划的关节角轨迹，可得位置控制的pd控制器如下所示：

10、

11、其中，为机械臂关节基于位置的控制力矩，；为机械臂系统质量矩阵的估计值，表示机械臂关节角，即关节空间，为规划的关节角，即期望关节角；为规划关节角加速度轨迹为规划关节角与实际关节角的误差，即误差关节角；为规划关节角与实际关节角的误差角速度轨迹，和为pd控制器的系数矩阵；

12、假设机械臂末端与目标的接触力为，机械臂的动力学方程可写为：

13、

14、其中，为机械臂系统质量矩阵，为关节角加速度，为关节角速度，

15、在力控制问题中，机械臂在力跟踪方向的速度与加速度通常需要忽略，故机械臂的动力学方程可简化为：

16、

17、在实际应用中，接触力的测量通常具有较大噪声，故对力矩的测量信号进行微分会使信号失去物理意义，因此在力控制中不考虑微分控制。考虑前馈补偿和重力补偿的pi力控制器方程可以写为：

18、

19、式中，，和为正定的pi控制器参数，为雅克比矩阵，为末端期望力，为力跟踪误差信号；

20、根据简化动力学方程和pi力控制器方程可得：

21、

22、根据上式可知当和均为正定矩阵时，力跟踪误差信号可收敛至0，但是若机械臂未与目标发生预设的接触碰撞，力控制的存在会使机械臂产生巨大的速度误差；由于典型的力控制任务中速度项较小，可以直接通过一个正定的参数阵引入速度阻尼项，修改后的控制律可以写为：

23、

24、其中，为机械臂关节基于力的控制力矩，为速率阻尼项，需要注意的是雅克比矩阵与接触方向相关，碰撞参考系x轴的方向即为接触方向，其余方向由右手定则确定；

25、s3）：基于步骤s2）中建立的模型，设计力位混合控制控制器；

26、即与两个子空间相互正交，将位置控制与力控制整合为统一的控制方程：

27、

28、s4）：采用matlab联合adams典型轨迹仿真。

29、本发明所述的基于力位混合控制的冗余偏置机械臂末端柔顺控制方法，所述步骤s1中建立冗余偏置机械臂的运动学模型过程如下： s101）：首先确定机械臂运动与力的参数，如下：

30、①关节最大转速：5°/s；

31、②关节转角范围：-180°～+180°；

32、③最大末端力/力矩：20n，5n；

33、s102）：按dh参数选取规则得到各相邻关节和之间的标准dh参数：

34、绕轴使轴与轴平行的转角；

35、沿轴方向轴到轴的最短距离；

36、绕轴使轴与轴平行的转角；

37、沿轴方向使轴到轴最短的距离；

38、s103）：根据步骤s1中建立坐标系的规则建立机械臂的标准dh坐标系；

39、s104）：相邻连杆坐标变换关系为：

40、

41、其中，表示；表示；表示；表示；为第i个关节旋转的角度和dh参数的和；

42、最终得到从机械臂基座到末端的齐次变换矩阵，该矩阵的前三行前三列表示末端姿态，矩阵的第四列前三行表示末端位置：

43、。

44、所述步骤s2）中通过雅克比矩阵建立机械臂工作空间与机械臂关节空间的对应关系为

45、

46、其中，表示机械臂末端，即工作空间；为的导数；表示机械臂雅可比矩阵；表示关节角速度。

47、本发明所述的基于力位混合控制的冗余偏置机械臂末端柔顺控制方法，所述步骤s3中设计力位混合控制控制器的具体过程如下： 假设接触目标为刚性的，可以将多体系统的pfaffian约束记为：

48、

49、包含约束的工作空间动力学方程可写为：

50、

51、

52、其中，为质量矩阵，为包含运动信息的向量，,为的转置，

53、经过求导与代入运算可得带约束的方程，为n-k个独立的运动方程：

54、

55、秩为的矩阵为位置控制的子空间，秩为的矩阵为力控制的子空间，该矩阵将机械臂末端受到的约束力投影到力约束的子空间，该子空间可以使机械臂末端执行器沿与约束相切的方向移动；矩阵的秩为，为位置控制的子空间；

56、空间表达式为：

57、

58、与两个子空间相互正交，将位置控制与力控制整合为统一的控制方程为：

59、

60、因为位置控制器与力控制器经过与两个正交投影而相互解耦，所以力位混合控制器可以分别进行力控制和位置控制的误差响应观测和稳定性分析。

61、本发明所述的基于力位混合控制的冗余偏置机械臂末端柔顺控制方法，所述步骤s4中matlab联合adams典型轨迹仿真的具体过程如下： s401）：首先，根据冗余偏置机械臂结构参数在adams中建立简化机械臂模型；

62、s402）：在matlab中根据清洁任务确定擦拭轨迹，通过末端轨迹插值、逆运动学求解算以及力位混合控制方法得到关节空间轨迹；

63、s403）：将matlab中计算得到的关节角输入到adams的机械臂模型中驱动机械臂运动。

64、本发明所述的基于力位混合控制的冗余偏置机械臂末端柔顺控制方法，所述机械臂先从初始位置运动到擦拭轨迹起点，然后沿擦拭轨迹运动，直至运动到擦拭轨迹终点结束。

65、所述机械臂的末端设有六维力传感器。

66、上述技术方案可以看出，本发明具有如下有益效果：

67、1、本发明所述的基于力位混合控制的冗余偏置机械臂末端柔顺控制方法，将位置控制器与力控制器结合设计了力位混合控制控制器，能够对机械臂末端的运行轨迹以及擦拭力进行控制，有效保证机械臂末端沿规定擦拭轨迹运动的同时实现机械臂末端在擦拭表面保持恒定清洁力。

68、2、本发明所述的基于力位混合控制的冗余偏置机械臂末端柔顺控制方法通过位置控制器与力控制器相互解耦，可以分别进行力控制和位置控制的误差响应观测和稳定性分析。

69、3、本发明通过在机械臂的末端设置六维力传感器实现力感知，确保末端操作机构和目标的接触力在允许的范围之内，再借助相应的控制方法实现对操控力的柔顺控制，避免意外损害。