CN112462760B - 一种双舵轮agv路径跟踪方法 - Google Patents

Abstract

本发明提供了一种双舵轮AGV路径跟踪方法，涉及自动控制技术领域，能够有效提高双舵轮AGV的轨迹跟踪效果，保证AGV车体实时纠偏，从而控制车体准确沿着目标路径行驶；该方法步骤包括：S1、建立双舵轮AGV运动学模型；S2、根据AGV车体的里程计信息，结合实时车体姿态数据，推算AGV行驶轨迹；S3、根据推算出的AGV行驶轨迹，计算当前时刻AGV的姿态偏差；S4、根据计算出的AGV的姿态偏差，获得车体下一周期的目标控制量；S5、通过S4中计算出的下一周期的目标控制量，结合双舵轮AGV运动学模型计算出双舵轮的调整量，实现纠偏。本发明提供的技术方案适用于双舵轮AGV行使的过程中。

Description

一种双舵轮AGV路径跟踪方法

【技术领域】

本发明涉及自动控制技术领域，尤其涉及一种双舵轮AGV路径跟踪方法。

【背景技术】

AGV全称自动导引车(Automated Gu ided Vehicle),指装备电磁或光学等自动导引装置，能够沿规定路径行驶，具有安全保护和各种移载功能的运输车。AGV的运动控制是实现自动导引车高精度、高可靠性运行的关键技术，其中，AGV的路径跟踪算法策略的研究又是AGV运动控制的难点。目前AGV路径跟踪算法常用的算法有最优控制方法、线性模型控制方法、模糊控制方法等。

非线性控制方式如模糊控制方法不需要建立精确的运动学模型，模仿人的模糊推理和决策过程，依靠现场经验设计模糊控制规则，但是该方式控制精度低，稳定性差，不适合实现AGV高精度路径跟踪。

线性模型控制方法相对其他非线性控制方式需要建立精确的运动学模型，常用的是PID控制方法。PID控制方法简单易懂，计算效率高，对于工作环境变化不大的应用场景，一旦PID参数整定完成，AGV的路径跟踪精度高，可保证AGV跟随指定路径自动行驶。

因此，本申请在线性模型控制方法的基础上，研究一种双舵轮AGV路径跟踪方法来应对现有技术的不足，以解决或减轻上述一个或多个问题。

【发明内容】

有鉴于此，本发明提供了一种双舵轮AGV路径跟踪方法，能够有效提高双舵轮AGV的轨迹跟踪效果，保证AGV车体实时纠偏，从而控制车体准确沿着目标路径行驶。

一方面，本发明提供一种双舵轮AGV路径跟踪方法，其特征在于，所述路径跟踪方法的步骤包括：

S1、建立双舵轮AGV运动学模型；

S2、根据AGV车体的里程计信息，结合实时车体姿态数据，推算AGV行驶轨迹；

S3、根据推算出的AGV行驶轨迹，计算当前时刻AGV的姿态偏差；

S4、根据计算出的AGV的姿态偏差，获得车体的目标控制量；

S5、通过S4获得的目标控制量，结合双舵轮AGV运动学模型计算出双舵轮的调整量，实现纠偏。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，AGV行驶轨迹包括车体姿态数据和目标移动路径。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，步骤S3中AGV的姿态偏差包括角度偏差、法向偏差和切向偏差。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，步骤S4中目标控制量包括车体的目标线速度、目标角速度和目标航向角。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，步骤S1的内容包括：根据AGV车体中心的线速度、角速度和航向角解算得到两个舵轮的线速度和舵向值；其中，舵轮的线速度方向与该舵轮的舵向值一致。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，步骤S1的具体步骤包括：

S11、根据AGV车体中心的线速度和角速度计算出AGV的旋转半径；再结合航向角得到旋转半径与AGV车体中心点速度的几何关系；

S12、将AGV旋转中心点与两舵轮的回转中心点连接形成三角形，根据三角几何运算得到两个舵轮的回转半径以及两个舵轮的航向角；

S13、根据两个舵轮的回转半径、AGV的旋转半径以及AGV车体中心的线速度计算出两个舵轮的线速度；完成双舵轮AGV运动学模型的建立。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，步骤S2的里程计信息包括AGV车体中心点的坐标和航向角。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述里程计信息由两个舵轮的行走速度和转向角度推算得到。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，将法向偏差、切向偏差和角度偏差分别输入到法向PID控制器、切向PID控制器和角度PID控制器中，得到AGV车体中心点的目标线速度、角速度和航向角。

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，双舵轮的调整量包括对应舵轮的行走速度调整量和转向角度调整量。

另一方面，本发明提供一种双舵轮AGV导引系统，其特征在于，所述导引系统包括：

AGV行驶轨迹计算模块，用于根据AGV车体的里程计信息，结合实时车体姿态数据，推算出AGV行驶轨迹；

AGV姿态偏差计算模块，用于根据AGV行驶轨迹，计算当前时刻AGV的姿态偏差；

目标控制量计算模块，用于根据AGV的姿态偏差，计算出车体的目标控制量；

纠偏模块，用于根据目标控制量，结合双舵轮AGV运动学模型计算出双舵轮的调整量，并将调整量以命令形式发送给待纠偏AGV。

与现有技术相比，本发明可以获得包括以下技术效果：通过建立双舵轮AGV运动学模型，根据行驶轨迹和目标控制量，解算出双舵轮的调整量，能够有效提高双舵轮AGV的轨迹跟踪效果，保证AGV车体实时纠偏，从而控制车体准确沿着目标路径行驶。

当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有技术效果。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本发明一个实施例提供的双舵轮对角分布结构示意图；

图2是本发明一个实施例提供的双舵轮AGV运动学模型示意图；

图3是本发明一个实施例提供的双舵轮AGV姿态偏差示意图；

图4是本发明一个实施例提供的双舵轮路径跟踪流程图。

【具体实施方式】

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

在本发明实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本发明。在本发明实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。

本发明提供一种双舵轮AGV路径跟踪方法。其中，双舵轮AGV自动转运过程中已知目标移动路径，该路径跟踪算法主要包括以下内容：1.建立双舵轮AGV运动学模型；2.根据已知AGV车体的里程计信息，结合导航雷达反馈的车体姿态数据，推算AGV行驶轨迹；3.根据所获AGV行驶轨迹的车体姿态数据和目标移动路径，计算当前时刻AGV的姿态偏差(包括角度偏差、法向偏差和切向偏差)；4.根据所获偏差结合PID算法获得车体下一个周期的目标控制量(包括车体的目标线速度，目标角速度和目标航向角)；5.通过所述目标控制量，结合车体运动学模型计算得到下一个周期双舵轮的线速度和舵向值并控制双舵轮下一个周期按所得线速度和舵向值动作。该路径跟踪算法执行过程中，可根据当前时刻车体姿态偏差结合PID算法计算得下一个周期车体得目标控制量，保证AGV车体实时纠偏，从而控制车体准确沿着目标路径行驶。

双舵轮AGV路径跟踪方法包括如下具体步骤：

步骤1、建立双舵轮AGV的运动学模型；

运动学模型是一种理想化的数学模型，可根据所需AGV车体中心的线速度、角速度和航向角(V，ω，A)解算得到两个舵轮的线速度和舵向值(舵轮线速度方向一定与舵轮舵向值一致)。本发明以双舵轮对角分布的AGV为例，详细描述其运动学模型建立过程，双舵轮对角分布结构示意图如图1所示。建立车体坐标系,X轴正方向过车体中心指向车头侧，Y轴正方向过车体中心指向车体左侧，车体坐标系符合右手笛卡尔坐标系。所述双舵轮AGV包括车体、舵轮1、舵轮2以及万向轮1、万向轮2，假设双舵轮AGV车体、所有车轮及系统均为刚体，车轮在运行表面上无打滑。可将双舵轮AGV简化成两个舵轮旋转中心刚性连接，以双舵轮的回转中心来计算AGV车体中心点的线速度和角速度，双舵轮AGV在任意位置的运动状态示意图如图2所示。已知导航算法下发给AGV中心点的参数(V，ω，A)，两舵轮之间的纵向距离为L,横向距离为D，舵轮间距可以计算得到距离为H，夹角为B1，求舵轮1的参数(V1，ω1，A1),舵轮2的参数(V2，ω2，A2),其中ω＝ω1＝ω2。

第(1)步：已知参数(V，ω，A)，求AGV的旋转半径。

根据A可知旋转半径与AGV车体中心点速度的几何关系。

第(2)步：由于两个舵轮旋转中心支点与AGV是刚性连接，所以两舵轮的回转中心支点必定与AGV旋转中心同心，将AGV旋转中心与两舵轮回转中心点连接，根据三角几何运算可得R1，R2，A1，A2。

第(3)步：由于R＝V/ω，ω1＝ω2＝ω，所以

V1＝V*(R1/R)

V2＝V*(R2/R)

第(4)步：将V1、V2、A1、A2分别解算出来，发送给舵轮转向驱动器和牵引驱动器。其中，转向驱动器设置为位置模式，牵引驱动器设置为速度模式。

转向驱动器和牵引驱动器均采用美国AMC公司低压直流驱动器，采用CANopen总线通讯协议实现车载控制器与转向和牵引驱动器的数据交互。本方法解算出来的V1、V2速度数值需要转换单位为转/分钟并下发转换后数值至牵引驱动器目标速度控制字，本方法解算出来的A1、A2角度数值需要转换为转向驱动器的绝对位置并下发至转向驱动器的目标位置控制字。该转换方法适用于CANopen总线通讯协议，其他驱动器通讯方式需要根据相应通讯协议进行调整。

步骤2、根据所述双舵轮运动学模型和双舵轮的编码器反馈信息，即两个舵轮的行走速度和转向角度，可推算双舵轮AGV的里程计信息，即AGV车体中心点的坐标和航向角(x,y,θ)，结合导航雷达反馈的车体姿态数据作为参考基准纠正车体里程计数据，从而得到双舵轮AGV车体中心的行驶轨迹。

步骤3、根据所获AGV行驶轨迹的车体姿态数据和目标移动路径，计算AGV的姿态偏差(包括角度偏差、法向偏差和切向偏差)，双舵轮AGV的姿态偏差如图3所示。现定义e_f为法向的位置误差；e_q为切向的位置误差；e_A为航向角的角度误差。R’为过点S与规划路径R的垂线，与R垂足于N(X_n,Y_n)，则有e_f等于线段NS的长度，e_q等于线段NM的长度，此时得到：

e_q＝|NM|

e_f＝|NS|

角度偏差e_A：e_A＝A_m-A_s。

接下来求解e_f、e_q、e_A，首先求得规划路径R的表达式：

y-y_m＝tanA_m(x-x_m)

NS的长度就是点S到规划路径R的距离，由点到直线的距离公式得：

当AGV的实际位置在规划路径R的右方时，K＝1；当AGV的实际位置在规划路径R的左方时，K＝-1；

同理NM的长度就是点M到直线SR’的距离：

步骤4、分别将法向偏差、切向偏差和角度偏差输入到法向PID控制器、切向PID控制器和角度PID控制器，得到AGV车体中心的目标线速度、角速度和航向角(V，ω，A)，根据双舵轮运动学模型分解即可得双舵轮的调整量(包括行走速度V1、V2和转向角度A1、A2)，其流程如图4所示。

本发明的有益效果包括：

1、本发明建立了双舵轮运动学模型，将AGV车体中心的目标线速度、角速度和航向角分解为两舵轮的行走速度和转向角度。

2、本发明采用PID控制器一旦参数整定完毕，该路径跟踪算法具有良好的轨迹跟踪效果，同时PID控制器设计简单，易于理解。

以上对本申请实施例所提供的一种双舵轮AGV路径跟踪方法，进行了详细介绍。以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。

如在说明书及权利要求书当中使用了某些词汇来指称特定组件。本领域技术人员应可理解，硬件制造商可能会用不同名词来称呼同一个组件。本说明书及权利要求书并不以名称的差异来作为区分组件的方式，而是以组件在功能上的差异来作为区分的准则。如在通篇说明书及权利要求书当中所提及的“包含”、“包括”为一开放式用语，故应解释成“包含/包括但不限定于”。“大致”是指在可接收的误差范围内，本领域技术人员能够在一定误差范围内解决所述技术问题，基本达到所述技术效果。说明书后续描述为实施本申请的较佳实施方式，然所述描述乃以说明本申请的一般原则为目的，并非用以限定本申请的范围。本申请的保护范围当视所附权利要求书所界定者为准。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的商品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种商品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的商品或者系统中还存在另外的相同要素。

应当理解，本文中使用的术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。

上述说明示出并描述了本申请的若干优选实施例，但如前所述，应当理解本申请并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述申请构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本申请的精神和范围，则都应在本申请所附权利要求书的保护范围内。

Claims (7)

1.一种双舵轮AGV路径跟踪方法，其特征在于，所述路径跟踪方法的步骤包括：

S1、建立双舵轮AGV运动学模型；

S2、根据所述双舵轮AGV运动学模型和双舵轮的编码器反馈信息推算双舵轮AGV的里程计信息，结合导航雷达反馈的车体姿态数据纠正车体里程计数据，从而得到双舵轮AGV车体中心的行驶轨迹；

S3、根据行驶轨迹的车体姿态数据和目标移动路径，计算当前时刻AGV的姿态偏差，包括角度偏差、法向偏差和切向偏差；

S4、分别将法向偏差、切向偏差和角度偏差输入到法向PID控制器、切向PID控制器和角度PID控制器，得到AGV车体中心的目标线速度、角速度和航向角，结合双舵轮AGV运动学模型计算出双舵轮的调整量，实现纠偏；

步骤S1的内容包括：根据AGV车体中心的线速度、角速度和航向角解算得到两个舵轮的线速度和舵向值；其中，舵轮的线速度方向与舵向值一致。

2.根据权利要求1所述的双舵轮AGV路径跟踪方法，其特征在于，步骤S1的具体步骤包括：

S11、根据AGV车体中心的线速度和角速度计算出AGV的旋转半径；再结合航向角得到旋转半径与AGV车体中心点速度的几何关系；

S12、将AGV旋转中心点与两舵轮的回转中心点连接形成三角形，根据三角几何运算得到两个舵轮的回转半径以及两个舵轮的航向角；

S13、根据两个舵轮的回转半径、AGV的旋转半径以及AGV车体中心的线速度计算出两个舵轮的线速度；完成双舵轮AGV运动学模型的建立。

3.根据权利要求1所述的双舵轮AGV路径跟踪方法，其特征在于，AGV行驶轨迹包括车体姿态数据和目标移动路径；

步骤S3中AGV的姿态偏差包括角度偏差、法向偏差和切向偏差。

4.根据权利要求1所述的双舵轮AGV路径跟踪方法，其特征在于，步骤S2的里程计信息包括AGV车体中心点的坐标和航向角。

5.根据权利要求4所述的双舵轮AGV路径跟踪方法，其特征在于，所述里程计信息由两个舵轮的行走速度和转向角度推算得到。

6.根据权利要求1所述的双舵轮AGV路径跟踪方法，其特征在于，双舵轮的调整量包括对应舵轮的行走速度调整量和转向角度调整量。

7.一种双舵轮AGV导引系统，其特征在于，用于实现权利要求1-6任一所述的方法；所述导引系统包括：

AGV行驶轨迹计算模块，用于根据AGV车体的里程计信息，结合实时车体姿态数据，推算出AGV行驶轨迹；

AGV姿态偏差计算模块，用于根据AGV行驶轨迹，计算当前时刻AGV的姿态偏差；

目标控制量计算模块，用于根据AGV的姿态偏差，计算出车体的目标控制量；

纠偏模块，用于根据目标控制量，结合双舵轮AGV运动学模型计算出双舵轮的调整量，并将调整量以命令形式发送给待纠偏AGV。

Images