一种麦克纳姆轮移动操作平台的防倾覆控制方法

本发明属于移动机器人，涉及一种麦克纳姆轮移动操作平台的防倾覆控制方法。

背景技术：

1、移动机器人相比于基座固定式机器人，具有大范围运动性、灵活性和机动性，而以麦克纳姆轮为代表的全向移动机器人，还能在不改变移动机器人姿态的前提下，沿任意方向移动，因此更加的灵活。但是麦克纳姆轮移动机器人只能提供平面全向运动，为了扩大其使用范围，人们考虑到将机械臂集成到麦克纳姆轮移动机器人上，形成麦克纳姆轮移动操作平台，拥有多自由度的机械臂正好解决了麦克纳姆轮移动平台只能平面全向运动的缺陷，而麦克纳姆轮移动平台又增加了机械臂的灵活性，这使得麦克纳姆轮移动操作平台能够在复杂多变的环境下工作，极大程度上提升了其应用范围。但是考虑到机械臂在执行任务过程中，不可避免会与外界产生操作接触或碰撞，当接触碰撞过大例如受到较大的冲击时，会使得麦克纳姆轮移动操作平台出现倾覆的情况，这不仅会影响正常的任务执行，还存有极大安全隐患。

2、目前有不少学者对麦克纳姆轮移动操作平台的防倾覆问题进行研究，也提出了很多可行的控制方法。有的学者通过提前规划平台运动来避免碰撞或者减小碰撞的影响，以提高平台的倾覆稳定性。也有不少学者将碰撞视为扰动，在设计控制器的时候利用状态观测器对其进行补偿。以上研究方法在面对一般接触碰撞的时候能处理得较好，但是当碰撞随机并且速度很快比如碰撞冲击时，就会存在一定缺陷。首先，随机高速的碰撞冲击，难以对其进行检测并提前规划平台运动，无法避免；其次，麦克纳姆轮移动操作平台在受到碰撞冲击时，必然会发生倾覆，此时整个平台系统的数学模型会发生改变，需要重新对其进行运动学和动力学分析，但是已有的控制方法大多将碰撞视为扰动，没有考虑到这一点，由此设计出的控制器难以保证碰撞冲击下麦克纳姆轮移动操作平台的防倾覆效果。

技术实现思路

1、为克服现有技术的不足，本发明提供了一种针对碰撞冲击下麦克纳姆轮移动操作平台的防倾覆控制方法。本发明考虑到碰撞冲击无法避免，根据倾覆情况建立麦克纳姆轮移动操作平台的倾覆模型，然后引入质心虚拟力和力矩，从而建立质心—麦轮的力分配模型，为了求解此模型，还提出了基于二次规划的优化算法，在保证麦克纳姆轮移动操作平台稳态误差和收敛速度的同时还尽可能地减少输出力，最后通过麦克纳姆轮的动力学模型，将优化算法求出的解转化到电机扭矩上，以便用于实际控制。本发明的技术方案如下：

2、一种麦克纳姆轮移动操作平台的防倾覆控制方法，包括下列步骤：

3、步骤一：在世界坐标系下，建立麦克纳姆轮移动操作平台的倾覆模型；

4、步骤二：引入质心虚拟力和力矩，通过静力学分析以及结合步骤一中的倾覆模型，建立质心—麦轮的力分配模型，用于质心虚拟力和力矩到麦轮受力的转化；

5、步骤三：设计基于二次规划的优化算法，对步骤二中的力分配模型进行优化求解，得到麦轮所需的最佳地面摩擦力；

6、步骤四：通过麦克纳姆轮的动力学分析，建立麦轮的驱动电机和所受地面摩擦力之间的映射关系，得到实际驱动麦轮运动的电机扭矩，实现在碰撞冲击下麦克纳姆轮移动操作平台的稳态控制。

7、进一步的，设移动操作平台设置三个旋转关节和两段机械臂，步骤一的方法如下：对其四个麦轮进行编号分别记为1,2,3,4号轮，给出世界坐标系、基坐标系、三个旋转关节坐标系、四个角点坐标系的定义：

8、世界坐标系定义为∑owxwywzw，基坐标系定义为∑obxbybzb，三个旋转关节坐标系分别定义为∑o0x0y0z0,∑o1x1y1z1,∑o2x2y2z2，四个角点坐标系分别定义为∑p1x1y1z1,∑p2x2y2z2,∑p3x3y3z3,∑p4x4y4z4；其中p1,p2,p3,p4分别是麦克纳姆轮移动平台的四个麦轮与地面的接触点，ob是p1,p2,p3,p4构成的四边形图形的形心，角点坐标系三轴的方向始终与世界坐标系三轴方向相同；

9、在基坐标系∑obxbybzb下，求出麦克纳姆轮移动操作平台的质心坐标[xcom,ycom,zcom]t如下：

10、

11、其中，mb,m1,m2分别为麦克纳姆轮移动平台、第一段机械臂、第二段机械臂的质量；m＝mb+m1+m2为整个麦克纳姆轮移动操作平台的质量；l1,l2分别为第一段机械臂和第二段机械臂的长度；θ0,θ1,θ2分别为三个关节的转角；h为麦克纳姆轮移动平台的高度；

12、以麦克纳姆轮移动操作平台系统的质心com为顶点，麦克纳姆轮移动平台四个麦轮与地面的接触点p1,p2,p3,p4为角点，将各个角点顺序依次相连接形成四条边线，由此构建四棱锥，将麦克纳姆轮移动操作平台碰撞冲击倾覆的过程看成是四棱锥绕任意一角点旋转的运动，旋转运动用倾斜、俯仰、偏转描述；规定在碰撞冲击下，四棱锥绕某一角点顺序发生绕x轴倾斜ψ角，绕y轴俯仰θ角，绕z轴偏转φ角；结合式(1)，通过坐标变换得到在角点坐标系∑pnxnynzn下(n＝1,2,3,4)，麦克纳姆轮移动操作平台倾覆后的质心位置[x′com,y′com,z′com]t，如下所示：

13、

14、其中为齐次变换矩阵，表达式如下:

15、

16、式(3)中，绕角点p1旋转时，c＝-a/2,d＝-l/2；绕角点p2旋转时，c＝a/2,d＝-l/2；绕角点p3旋转时，c＝a/2,d＝l/2；绕角点p4旋转时，c＝-a/2,d＝l/2；sψ＝sinψ,cψ＝cosψ,sθ＝sinθ,cθ＝cosθ,sφ＝sinφ,cφ＝cosφ；a和l分别表示麦克纳姆轮移动平台的长度和宽度；

17、可得在世界坐标∑owxwywzw下，麦克纳姆轮移动操作平台倾覆后的质心坐标(xcom,ycom,zcom)，如下所示：

18、

19、式(4)即是克纳姆轮移动操作平台的倾覆模型，其中分别为角点pn(n＝1,2,3,4)在世界坐标系∑owxwywzw下的位置，

20、进一步的，步骤二的方法如下：将整个麦克纳姆轮移动操作平台系统的受力——地面的反作用力f＝[fx,fy,fz]t和重力g＝mg集中于质心，形成虚拟力f和虚拟力矩t，在世界坐标系下分解后得到三个方向的虚拟力[fx,fy,fz]t以及三个方向的虚拟力矩[tx,ty,tz]t，对应质心在世界坐标下的三个方向的位移[xcom,ycom,zcom]t和绕三轴的姿态角[ψ,θ,φ]t，用pid控制器生成所需的虚拟力和力矩，控制率如下：

21、

22、其中，为期望的质心位姿；kp,ki,kd分别为比例增益，积分增益，微分增益；

23、根据麦轮离地的个数，将碰撞冲击下麦克纳姆轮移动操作平台的倾覆情况分为以下三类：一个麦轮离地、两个麦轮离地和三个麦轮离地；接下来，就分别对这三种情况建立相应的质心-麦轮力分配模型；

24、(1)一轮离地的情况，建立对应的质心—麦轮力分配模型如下：

25、

26、式中[f1x,f1y,f1z]t,[f2x,f2y,f2z]t,[f3x,f3y,f3z]t分别表示在世界坐标系下未离地的三个麦轮所受地面反作用力；[x1,y1,z1]t,[x2,y2,z2]t,[x3,y3,z3]t分别表示未离地麦轮与地面的三个接触点相对于质心的位置向量；

27、(2)两轮离地的情况，其对应的质心—麦轮力分配模型如下：

28、

29、式中[f1x,f1y,f1z]t,[f2x,f2y,f2z]t分别表示在世界坐标系下未离地的两个麦轮所受地面反作用力；[x1,y1,z1]t,[x2,y2,z2]t分别表示未离麦轮与地面的两个接触点相对于质心的位置向量；

30、(3)三轮离地的情况，其对应的质心—麦轮力分配模型如下：

31、

32、式中[f1x,f1y,f1z]t表示在世界坐标系下未离地的那个麦轮所受地面反作用力；[x1,y1,z1]t表示未离地的那个麦轮与地面的接触点相对于质心的位置向量。

33、进一步的，步骤三的方法如下：

34、将式(6)、式(7)以及式(8)所表达的质心—麦轮力分配模型，写成统一的表达式：b＝ax，每种情况的区别只是矩阵a有所不同；为了求解x，同时还保证x的值尽可能小以减少输出力，建立如下的目标函数：

35、j(x)＝(ax-b)tq(ax-b)+xtwx＝xtatqax+xtwx-2xtatqb+btqb  (9)

36、其中，q为质心虚拟力和力矩的跟随权重矩阵，是一个正定的对角矩阵；w为实现最小化满足优化所需要输出力的权重矩阵，w也是一个正定的对角矩阵，阶数和x的阶数相同；

37、忽略第四项，把力分配的问题转化成求解一个带约束的二次规划问题：

38、

39、将式(10)转化为qp优化问题，形式如下：

40、

41、其中，

42、

43、设地面摩擦系数为μ，则摩擦力的约束条件如式(13)所示：

44、

45、

46、式(14)中表示在世界坐标系下麦克纳姆轮移动操作平台质心沿z轴的加速度，fz为地面给麦克纳姆轮移动操作平台沿z方向上的总支持力fz。

47、进一步的，步骤四的方法如下：

48、1、3号麦轮的驱动电机和所受地面摩擦力之间的映射关系如下：

49、

50、2、4号麦轮的驱动电机和所受地面摩擦力之间的映射关系如下：

51、

52、其中f＝τr，r是麦轮的半径，τ为麦轮的电机输出扭矩；ax和ay是麦轮与地面接触点在世界坐标系下的加速度；

53、实现在碰撞冲击下麦克纳姆轮移动操作平台的稳态控制。