单个自动代客泊车机器人的轨迹跟踪控制方法

本发明涉及代客泊车机器人平台的智能控制，尤其是一种单个自动代客泊车机器人的轨迹跟踪控制方法。

背景技术：

1、近年来，我国汽车保有量持续增长，与此同时，城市可开发并应用于停车场建设的土地不断减少，停车位难找、停车位不足及停车事故频发等一系列停车问题日益涌现。为了能够尽可能充分利用城市紧张有限的停车空间，安全且高效地停放车辆，越来越多的智能停车系统(smart parking system，sps)被提出。目前，智能停车系统可分为直接式和间接式两类，一是在智能车辆配备自动停车系统(automated parking systems,aps)，让车辆本身自动完成泊车任务。这种直接式方案适应的停车场景多，对停车场基础设施要求小，但该方法的缺点是对停车场空间利用率有限，对汽车自身的智能化水平要求高；二是利用泊车自动导引车辆(automated guided vehicle，agv)来自动地间接完成泊车需求，与aps相比，基于泊车agv的avp(automated valet parking)系统使用泊车agv自动运输车辆，它可以实现向任何位置的全方位移动，并且对汽车的智能化水平没有要求；同时，由于它不需要为每个车位预留开关车门和车辆进出的空间；并且相对于传统车辆而言，全方位泊车agv机动性强，因而它能有效地提高停车场的空间利用率；同时，借助泊车agv，用户只需要将车开到停车场接驳区，这样既可以节省用户的停/取车时间，又可以提高停车的安全性。

2、目前，基于泊车agv的avp系统有其显著的优势，因此引起了相关研究人员的极大关注，开发了很多类型和形式的泊车agv，其中全方位运动夹持式泊车agv因轮胎夹紧臂可以在没有任何其他设备帮助的情况下自动夹紧车轮并提升车辆的优点而得到了广泛地应用。

3、在泊车agv平台的基础之上，研究人员对停车位分配、路径规划和定位做了大量研究，然而，泊车agv的轨迹跟踪控制受到的关注很少，尽管已经有人对全向移动泊车agv的轨迹跟踪控制进行了一些研究，其中滑模控制在全方位运动泊车agv的轨迹跟踪控制得到了广泛的运用，但是大都没有考虑外部扰动对控制系统的影响，这就导致轨迹跟踪效果不佳，鲁棒性不强。

技术实现思路

1、为解决全向移动泊车agv的轨迹跟踪效果不佳，鲁棒性不强的问题，本发明的目的在于提供一种使泊车agv能够准确跟踪期望轨迹的同时，估计并消除外部扰动对轨迹跟踪的影响，提高跟踪过程中的鲁棒性的单个自动代客泊车机器人的轨迹跟踪控制方法。

2、为实现上述目的，本发明采用了以下技术方案：一种单个自动代客泊车机器人的轨迹跟踪控制方法，该方法包括下列顺序的步骤：

3、(1)建立泊车agv的运动学模型；

4、(2)建立泊车agv的动力学模型；

5、(3)根据泊车agv的运动学模型和泊车agv的动力学模型，设计基于非线性扰动观测器的滑膜控制器，将非线性扰动观测器估计的扰动值作为滑膜控制器的输入信号，滑膜控制器输出电机扭矩转化为电机转速，再输出至泊车agv的运动学模型，泊车agv的运动学模型输出四个麦克纳姆轮所需的转速，实现轨迹跟踪控制。

6、所述步骤(1)具体是指：

7、泊车agv是一个麦克纳姆轮全向移动平台，xwowyw是泊车agv的全局坐标系；xvovyv泊车agv车体坐标系；ov是泊车agv车体坐标系的坐标原点，并且是四个麦克纳姆轮的中心点；om是泊车agv的质心；viw对应各麦克纳姆轮心的线速度，i＝1,2,3,4，viw＝rmwiw，rm为麦克纳姆轮半径，wiw对应各麦克纳姆轮的转动角速度；vir对应各麦克纳姆轮接地辊子的切向速度；wz为泊车agv的横摆角速度；θvw为泊车agv的航向角，得到泊车agv的逆运动学方程如下式；

8、

9、式中：vx为车体中心速度在xv轴的速度分量；vy为车体中心速度在yv轴的速度分量；a为横向左右两个麦克纳姆轮中心距离的一半；b为纵向前后两个麦克纳姆轮中心距离的一半；式(1)为泊车agv的运动学模型；

10、将式(1)改写成式(2)：

11、

12、式中：为麦克纳姆轮旋转角速度；其中，和分别为泊车agv车体坐标系下沿xv、yv的速度分量；为泊车agv车体坐标系下的横摆角速度；

13、定义雅克比矩阵j为：

14、

15、得到泊车agv的正向运动学方程如下式；

16、

17、式中：j+＝(jt·j)-1·j；

18、建立泊车agv车体坐标系和全局坐标系的坐标变换方程，根据运动关系，得到坐标变换公式如下式：

19、

20、式中：其中，分别为泊车agv在全局坐标系下沿xw，yw的速度分量；为泊车agv在全局坐标系下的横摆角速度；rw2v(θvv)用下式表示；θvv为泊车agv车体坐标系下的航向角；

21、

22、联立式(1)至式(6)，得到和泊车agv速度的转换关系如下式；

23、

24、式中，r(θvv)为坐标变换矩阵；

25、由于泊车agv坐标原点ov和质心om理论上不重合，经过分析，用下式表示出泊车agv坐标原点ov和质心om之间的速度关系：

26、

27、式中：分别为质心om沿xv，yv轴的速度分量；dx，dy分别为质心om到坐标原点ov的横、纵向距离；

28、最后，联立式(5)至式(8)，得到下式；

29、

30、所述步骤(2)具体是指：泊车agv的动力学模型采用拉格朗日法建立，得到泊车agv的拉格朗日函数l如下式所示：

31、

32、式中：jv为泊车agv绕zv轴的转动惯量；mv为泊车agv的整车质量；jw为麦克纳姆轮的转动惯量；为泊车agv绕zw轴的横摆角速度；分别为泊车agv在全局坐标系下沿xw，yw的速度分量；为各个麦克纳姆轮旋转角速度；

33、然后联立式(9)、式(10)，并且对拉格朗日函数l求导，得到泊车agv的动力学模型，如下式：

34、

35、式中：

36、

37、

38、

39、其中，其中分别为泊车agv在全局坐标系下沿xw，yw，zw的加速度；f(θvw)＝[fx fx tz]t为广义力和力矩，fx，fy分别为泊车agv沿xv，yv轴的广义力分量；tz为泊车agv绕zv轴的广义力矩，rm为麦克纳姆轮半径，a为横向左右两个麦克纳姆轮中心距离的一半；b为纵向前后两个麦克纳姆轮中心距离的一半；dx，dy分别为质心om到坐标原点ov的横、纵向距离；θvw为泊车agv的航向角，为泊车agv在全局坐标系下的横摆角速度；为泊车agv车体坐标系下的横摆角速度。

40、所述步骤(3)具体包括以下顺序的步骤：

41、(3a)设计非线性扰动观测器：把所有承重万向轮产生的扰动力和扭矩合成为等效的扰动力和力矩，表示为：

42、fc＝[fcx，fcy，tcz]       (15)

43、式中：fcx，fcx分别为沿泊车agv的xv，yv轴的等效扰动力；tcz为绕泊车agv的zv轴的等效扰动力矩；

44、根据泊车agv的动力学模型，泊车agv的受到的广义力和力矩f(θvw)进一步表达为：

45、

46、式中：

47、

48、

49、其中，τ＝[τ1,τ2,τ3,τ4]t为麦克纳姆轮的驱动力矩；f＝[f1,f2,f3,f4]t为麦克纳姆轮受到的摩擦力，fc为承重万向轮产生的额外等效扰动力和力矩；θvw为泊车agv的航向角，a为横向左右两个麦克纳姆轮中心距离的一半；b为纵向前后两个麦克纳姆轮中心距离的一半；为各个麦克纳姆轮旋转角速度；

50、因为泊车agv的质心om非常接近泊车agv中心ov，假设每个麦克纳姆轮有相同的载荷，因此每个麦克纳姆轮所受的摩擦力表示为：

51、

52、式中：g为重力加速度；μ为麦克纳姆轮与地面之间的摩擦力系数，取μ＝0.02；mv为泊车agv的整车质量；

53、非线性扰动观测器表示为：

54、

55、式中：

56、

57、

58、

59、

60、其中，z是一个辅助变量；是扰动力和扭矩fc的估计值；c0＝[cx,cy,cz]t扰动观测器的调谐矩阵，c0＝diag[1500,1500,150]t；

61、联立式(23)(24)，得到l(qvw)为：

62、l(qvw)＝c0m(θvw)-1        (25)

63、定义观测器误差e0为：

64、

65、假设：

66、

67、最后得到：

68、

69、(3b)设计滑膜控制器：

70、首先定义跟踪误差为：

71、

72、式中：qr＝[xr,yr,θr]t为全局坐标系下参考轨迹的位置信息；qvw为全局坐标系下实际轨迹的位置信息；ex,ey,eθ分别为全局坐标系下x、y方向以及航向角的误差；

73、设计滑膜控制器的滑模面为下式：

74、

75、式中：p＞q＞0为滑膜控制器设计参数，并且满足p/q＜2，取p＝5,q＝3；χ＝diag(χx,χy,χz)为滑膜控制器设计矩阵，取χ＝diag(1.5,1.5,1.5)；

76、对式(30)求导得：

77、

78、式中：

79、令滑膜动力学方程满足指数趋近率得：

80、

81、式中：es,ks＞0为滑膜控制器切换面正定增益矩阵，且es越小，趋近速度越慢，es越大，则运动点到达切换面时将具有较大的速度，引起的抖动也较大，取es＝diag(0.5,0.5,0.5),ks＝diag(1,1,1)；

82、联立式(29)、式(30)，得到：

83、

84、最后联立式(11)、式(16)、式(31)得到：

85、

86、由上述技术方案可知，本发明的有益效果为：本发明针对泊车agv轨迹跟踪问题，提出了一种基于非线性扰动观测器的滑膜控制器，首先在分析泊车agv的运动学模型和泊车agv的动力学模型的基础上，考虑承重万向轮对泊车agv运动的干扰，并且使用非线性扰动观测器估计承重万向轮产生的扰动力和扭矩，再结合非奇异终端滑膜控制器实现泊车agv跟踪参考轨迹，结果表明，与传统的非奇异滑膜控制器相比，本发明能够很好地跟踪参考轨迹，并且跟踪稳定性提高了42％至68％。