本发明属于移动机器人研究领域，特别涉及全向移动机器人运动学模型的路径跟踪控制方法及系统。
背景技术：
1、在现代工业、服务机器人以及其他自动化系统中，全向移动机器人的应用越来越广泛。传统的移动机器人通常采用两轮或四轮驱动，虽然结构简单、控制方便，但其运动灵活性受限，无法轻松实现原地旋转或横向移动。而全向移动机器人能够突破这一限制，尤其适用于狭小空间、复杂路径的环境。
2、麦克纳姆轮全向移动机器人的运动控制涉及多个学科的交叉，如机械设计、动力学、控制理论以及计算机科学。其核心在于精确的路径跟踪控制，以确保机器人能够按照预定的路径平滑且准确地运动。这一目标的实现主要涉及控制算法以及运动学与动力学模型两方面。
3、全向移动机器人的路径跟踪控制是实现机器人自主运动的基础。由于其能够在平动过程中同时进行转动的特殊运动方式，直接在路径跟踪控制中直接计算当前位姿到目标位姿的偏差用在路径跟踪的偏差输入与机器人实际执行过程的位置偏差并不是完全相等，导致路径跟踪精度不高。
技术实现思路
1、本发明的目的在于提供全向移动机器人运动学模型的路径跟踪控制方法及系统，以解决路径跟踪精度不高的问题。
2、为实现上述目的，本发明采用以下技术方案：
3、第一方面，本发明提供全向移动机器人运动学模型的路径跟踪控制方法，包括：
4、根据给定车辆的运动路线选取当前路线上的点作为预瞄点；
5、基于预瞄点分解全向移动机器人运动过程得到精确坐标位置偏差，基于精确坐标位置偏差设计pid控制器得到控制速度输出；
6、根据全向移动平台的运动特性构建车辆输入输出之间的运动学模型；
7、基于运动学模型结合控制速度，获得全向移动机器人对应麦克纳姆轮的转速。
8、进一步的，根据给定车辆的运动路线选取当前路线上的点做为预瞄点：
9、全向移动机器人路线定义为(x,y,θ)的序列，预瞄点的选取是在参考路径上个找到一个与当前机器人位姿垂直的点，然后从该垂点开始在机器人的移动方向上向前加上在路径上的预瞄距离后得到当前控制计算的预瞄点(xr,yr,θr)。
10、进一步的，具体计算过程：
11、目标点选取根据当前车辆位姿、当前速度、目标速度，首先计算1秒后机器人预估的移动距离，根据该距离乘预瞄距离系数后得到预瞄距离d：
12、
13、d＝γs
14、其中，vs为当前机器人运动的合速度，vg为给定的目标合速度，γ为预瞄距离参数，最后得到当前速度状态下预瞄点位置距离当前位姿的距离d；速度越大对应的预瞄距离越远，机器人的跟踪将更加平滑；具体计算为在确定预瞄距离之后，计算距当前垂点位置距离为d的点做为单次控制输入的目标位姿，具体的预瞄点计算如下公式所示：
15、
16、其中m为参考路径的梯度，(x1,y1)和(x2,y2)为选取的参考路径上两个相距一段距离的点；参考路径使用直线方程来表示：
17、y＝mx+c
18、全向机器人位姿(xo,yo,θo)在参考路径上的垂点(xp,yp)由以下公式计算得到：
19、
20、yp＝mxp+c
21、全向机器人在参考路径上选取的目标位姿(xg,yg,θg)由以下公式计算：
22、θg＝tan-1(m)
23、xg＝xp+dsin(θg)
24、yg＝yp+dcos(θg)。
25、进一步的，基于预瞄点分解全向移动机器人运动过程得到精确坐标位置偏差：
26、全向移动机器人平动加转动的运动过程分解为位置控制和朝向控制两部分，在给定目标状态后，将连续的运动过程离散为三步运动得到，既先控制朝向到达目标朝向的一半，再控制机器人位置到达指定坐标，最后控制机器人旋转到指定位姿；
27、首先计算距离目标位姿的偏差，如下公式所示：
28、δx＝xg-xo
29、δy＝yg-yo
30、δθ＝θg-θo
31、全向机器人在旋转到目标朝向一半的新的位姿为：
32、x1＝xo
33、y1＝yo
34、
35、全向机器人在x1,y1,θ1的基础山平移到目标位姿为：
36、x2＝x1+δxcos(θ1)-δxsin(θ1)
37、y2＝y1+δysin(θ1)-δycos(θ1)
38、θ2＝θ1
39、最后全向机器人原地旋转到目标位姿：
40、x3＝x2
41、y3＝y2
42、
43、根据上述公式，计算在全向移动机器人从一个位姿移动到另外一个位姿全程实际位移的距离及转动量为：
44、δx′＝δxcos(θ1)+δxsin(θ1)
45、δy′＝-δysin(θ1)+δycos(θ1)
46、δθ′＝δθ。
47、进一步的，基于精确坐标位置偏差设计pid控制器得到控制速度输出：
48、得到全向移动机器人在给定局部预瞄点之后到达该预瞄点三个方向上实际的移动距离，通过该偏差以转换为车辆三个方向的速度控制量vx,vy,vω；
49、全向移动机器人运动的运动控制将其分解为三个方向的控制vx,vy,vω，具体的控制计算采用pid控制器进行，每个方向的控制量计算如下公式所示：
50、
51、控制参数设置为kp＝0.2,ki＝0.5,kd＝1；
52、在最终发送控制执行时按比例缩放使其满足机器人运动的最大约束，缩放系数计算为：
53、
54、其中，分别为机器人在三个方向上能够达到的最大速度；最终全向移动机器人的三个方向的运动速度的控制量为：
55、
56、进一步的，根据全向移动平台的运动特性构建车辆输入输出之间的运动学模型：
57、针对全向移动平台所使用的四个麦克纳姆轮进行运动学建模，输入为机器人的运动状态，输出为对应四个论在的速度；
58、将机器人底盘的运动分为三个独立变量描述分别为：x轴平动vx，y轴平动vy，z轴自转vω；定义车体坐标系为右手坐标系，既左侧为正，右侧为负；前方为正，后方为负；逆时针为正，顺时针为负；坐标系的原点o为四个轮子的几何中心，四个轮子构成矩形的对角线交点。
59、进一步的，基于运动学模型结合控制速度，获得全向移动机器人对应麦克纳姆轮的转速：
60、单个麦克纳姆轮速度合成：四个麦克纳姆轮分别编号为a、b、c、d，以车轮b为研究对象，其速度合成使用下述公式计算：
61、vbx＝vx+vωlcos(θ)
62、vby＝vy+vωlsin(θ)
63、
64、式中符号定义如下：
65、vx和vy为底盘整体的质心移动速度；vω为底盘的旋转速度；vbx和vby为右前轮质心的移动速度；l为底盘质心到右前轮质心的距离；w,h分别为全向移动机器人的宽度和长度；θ为轮子质心b位置与底盘质心o位置之间的夹角；lsin(θ)表示的是底盘旋转给车轮带来的线速度；
66、对于麦克纳姆轮的移动速度vbx是由轮子的转动线速度vb以及该轮辊子的速度vbl合成，有以下计算公式：
67、vbx＝vb+vblcos(β)
68、vby＝-vblsin(β)
69、其中β为轮轴与辊轴之间的夹角，该夹角为45°；
70、将上述多个公式联立得：
71、vx+vωlcos(θ)＝vb+vblcos(β)
72、vy+vωlsin(θ)＝-vblsin(β)
73、经过化简得到b轮的速度与底盘三个方向速度之间的计算公式为：
74、vb＝vx+vy+vωl(cos(θ)+sin(θ))
75、对其余三个轮子使用上述方法进行推导得到给定目标速度vx,vy,vω求解对应四个轮子的速度公式为：
76、va＝vx-vy-vωl(cos(θ)+sin(θ))
77、vb＝vx+vy+vωl(cos(θ)+sin(θ))
78、vc＝vx+vy-vωl(cos(θ)+sin(θ))
79、vd＝vx-vy+vωl(cos(θ)+sin(θ))。
80、第二方面，本发明提供全向移动机器人运动学模型的路径跟踪控制系统，包括：
81、预瞄点选取模块，用于根据给定车辆的运动路线选取当前路线上的点做为预瞄点；
82、控制速度输出模块，用于基于预瞄点分解全向移动机器人运动过程得到精确坐标位置偏差，基于精确坐标位置偏差设计pid控制器得到控制速度输出；
83、运动学模型构建模块，用于根据全向移动平台的运动特性构建车辆输入输出之间的运动学模型；
84、转速输出模块，用于基于运动学模型结合控制速度，获得全向移动机器人对应麦克纳姆轮的转速。
85、第三方面，本发明提供一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现全向移动机器人运动学模型的路径跟踪控制方法的步骤。
86、第四方面，本发明提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现全向移动机器人运动学模型的路径跟踪控制方法的步骤。
87、与现有技术相比，本发明有以下技术效果：
88、本发明在给定路径上选取局部预瞄点，在该预瞄点的基础上对机器人的全向移动分解为转动加平动的方式更加精确的计算路径跟踪偏差，根据该偏差使用pid控制器获得全向移动机器人平动及转动速度，同时为了满足全向移动机器人在平动及转动的最高运动速度限制，给出了一种等比例缩放的方法，最后使用运动学模型获得最终每个麦克纳姆的转动速度及方向。实现了在给定机器人移动路线情况下输出机器人四个轮子对应的转速实现了精确的路线跟踪。
89、本发明与其他全向移动机器人路径跟踪的方法相比较，该方法实时性较好，计算简便，充分考虑了麦克纳姆轮全向移动机器人的运动特点以及运动能力约束，有着更高的路径跟踪精度。
1.全向移动机器人运动学模型的路径跟踪控制方法，其特征在于，包括：
2.根据权利要求1所述的全向移动机器人运动学模型的路径跟踪控制方法，其特征在于，根据给定车辆的运动路线选取当前路线上的点做为预瞄点：
3.根据权利要求2所述的全向移动机器人运动学模型的路径跟踪控制方法，其特征在于，具体计算过程：
4.根据权利要求1所述的全向移动机器人运动学模型的路径跟踪控制方法，其特征在于，基于预瞄点分解全向移动机器人运动过程得到精确坐标位置偏差：
5.根据权利要求4所述的全向移动机器人运动学模型的路径跟踪控制方法，其特征在于，基于精确坐标位置偏差设计pid控制器得到控制速度输出：
6.根据权利要求1所述的全向移动机器人运动学模型的路径跟踪控制方法，其特征在于，根据全向移动平台的运动特性构建车辆输入输出之间的运动学模型：
7.根据权利要求6所述的全向移动机器人运动学模型的路径跟踪控制方法，其特征在于，基于运动学模型结合控制速度，获得全向移动机器人对应麦克纳姆轮的转速：
8.全向移动机器人运动学模型的路径跟踪控制系统，其特征在于，包括：
9.一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述全向移动机器人运动学模型的路径跟踪控制方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述全向移动机器人运动学模型的路径跟踪控制方法的步骤。
