CN114137825A - 一种重载双舵轮agv车体的运动控制方法、系统及装置 - Google Patents

Abstract

本发明涉及一种重载双舵轮AGV车体的运动控制方法、系统及装置，其通过修正车体的运动学模型的输入从而调整车体的运行，运动学模型的输入包括车体的转向电机输出的转向和车体的行走电机输出的速度；运动控制方法包括如下步骤：实时采集车体的实时位姿参数；在车体的路线轨迹上查找到与实时位姿参数相对应的参考位姿参数，并差值计算实时位姿参数和参考位姿参数之间的位姿偏差；根据位姿偏差修正转向和速度，修正后的转向和修正后的速度即为车体的运动学模型的修正后的输入。通过PID控制器和纯跟踪控制器共同作用得到车体两个舵轮的转向下发量；通过差速辅助控制得到两个舵轮的速度下发量，提高了车辆高速运行过程的稳定性。

Description

一种重载双舵轮AGV车体的运动控制方法、系统及装置

技术领域

本发明涉及AGV自动控制领域，特别是涉及一种重载双舵轮AGV车体的运动控制方法、系统及装置。

背景技术

重载AGV，特别是卷料搬运AGV,由于载重大，一般采用背负式结构，只能进行高位举升动作，无法完成物料与生产线的直接对接，需要专用的中间设备完成相应的抬升及对接动作，在此意义上背负式AGV只能起到物料搬运的作用。采用堆高式双舵轮双定向轮的底盘结构，承载轮受力均匀，两组舵轮采用平行设计，并由舵轮桥连接，保证舵轮与地面接触，提高行走稳定性。传统AGV运动控制系统主要由纵向运动控制与横向运动控制组成，纵向运动控制主要通过加减速规划跟踪曲线目标速度，实现前进方向的位置跟踪；横向运动控制主要通过控制舵轮转向角实现垂直AGV行进方向的位置跟踪。

现有的AGV运动控制系统中，横向运动控制基本都是通过PID控制器进行控制调节。而单一的PID横向控制由于定位偏差，地面工况以及舵轮本身响应延时等因素存在抗干扰能力差，局部精度低，运行不平稳等缺点；同时，死板的纵向运动控制由于加减速时的轮子打滑也会对运行的平稳性以及精度造成不良影响。

发明内容

基于此，有必要针对现有技术中单一的PID横向控制存在抗干扰能力差、局部精度低、运行不平稳等缺点，以及死板的纵向运动控制会对运行的平稳性以及精度造成不良影响的问题，提供一种重载双舵轮AGV车体的运动控制方法、系统及装置。

一种重载双舵轮AGV车体的运动控制方法，其通过修正所述车体的运动学模型的输入从而调整所述车体的运行，所述运动学模型的输入包括所述车体的转向电机输出的转向和所述车体的行走电机输出的速度；所述运动控制方法包括以下步骤：

实时采集所述车体的实时位姿参数；

在所述车体的路线轨迹上查找到与所述实时位姿参数相对应的参考位姿参数，并差值计算所述实时位姿参数和所述参考位姿参数之间的位姿偏差；

根据所述位姿偏差修正所述转向和所述速度，修正后的转向和修正后的速度即为所述车体的运动学模型的修正后的输入；

其中，修正后的转向为修正前的转向叠加转向修正量u_s，所述转向修正量u_S的计算方法包括以下步骤：

S1、将所述位姿偏差输入到所述车体的PID控制器中，得到所述车体的横向修正量u_l和所述车体的航向修正量u_a；

S2、计算第一转向修正量u_S1；计算公式如下：

u_S1＝Au_l+Bu_a，

S3、利用所述车体的纯跟踪控制器计算第二转向修正量u_S2；计算公式如下：

S4、通过第一转向修正量u_S1和第二转向修正量u_S2融合计算得到转向修正量u_S；计算公式如下：

u_S＝Cu_S1+Du_S2；

式中，A为横向修正量的权重系数，B为航向修正量的权重系数，C为所述PID控制的权重系数，D为所述纯跟踪控制的权重系数，L为车体的轴距，θ为当前点与一个预瞄点的连线方向和当前航向之间的航向差，l_d为当前点和一个预瞄点之间的距离；

修正后的速度为修正前的速度叠加速度修正量v_d，所述速度修正量v_d的计算方法为：

v_d＝EΔv_l+FΔv_a

式中，Δv_l为横向速度修正量，Δv_a为航向速度修正量；E为横向速度修正量的权重系数，F为航向速度修正量权重系数。

上述重载双舵轮AGV车体的运动控制方法，通过PID控制器和纯跟踪控制器共同作用，得到车体运动学模型修正后的转向输入；通过基于位姿偏差及PD控制器形成的差速反馈控制，得到车体运动学模型修正后的速度输入，大大提高了车辆高速运行过程的稳定性，能够避免打滑造成的精度问题及安全隐患，且能够适应不同工况条件，满足高精度的性能要求。

在其中一个实施例中，所述车体的前端包括以所述车体的纵轴对称布置的两个舵轮，所述车体的后端包括以所述纵轴对称布置的两个定向轮。

在其中一个实施例中，所述横向修正量u_l和所述航向修正量u_a由以下计算公式获得：

其中，

是e_l的微分项，

是e_a的微分项，k_p0和k_p1为比例系数，k_i0和k_i1为积分系数，k_d0和k_d1为微分系数，e_l为横向位姿偏差，e_a为航向位姿偏差，所述位姿偏差包括横向位姿偏差e_l和航向位姿偏差e_a。

在其中一个实施例中，所述横向速度修正量Δv_l和航向速度修正量Δv_a由以下计算公式获得：

其中，k_pv0和k_pv1为差速纠偏比例系数，k_dv0和k_dv1为差速纠偏微分系数。

在其中一个实施例中，车体的运动学模型包括以下参数：

R₁＝(R₀-L_D)/sin(θ₁)，

R₂＝(R₀+L_D)/sin(θ₂)，

其中，θ₁和θ₂为两个舵轮的理论转向角，R₁和R₂为两个舵轮的理论旋转半径，R₀为车体的旋转半径，L_D为两个舵轮与车体中心轴之间的距离，L_W为车体的轴距，v₀为车体中心的速度，v₁和v₂为两个舵轮的理论速度。

在其中一个实施例中，所述权重系数A和所述权重系数B分别满足如下公式：

A＝a·v₀

B＝b·v₀

其中，a、b满足saturation函数，表达式如下：

其中，a_上限、a_下限、b_上限和b_下限为saturation函数的预设值。

在其中一个实施例中，修正后的转向包括两个舵轮的转向下发量θ_1修和θ_2修；公式如下：

θ_1修＝θ₁+u_S，

θ_2修＝θ₂+u_S。

在其中一个实施例中，修正后的速度包括两个舵轮的速度下发量v_1修和v_2修；公式如下：

本发明还公开了一种重载双舵轮AGV车体的运动控制系统，其通过修正所述车体的运动学模型的输入从而调整所述车体的运行，所述运动学模型的输入包括所述车体的转向电机输出的转向和所述车体的行走电机输出的速度；所述运动控制系统包括位姿参数获取模块、位姿偏差计算模块和输入计算模块。

位姿参数获取模块用于获取所述车体的实时位姿参数；

位姿偏差计算模块用于在所述车体的路线轨迹上查找到与所述实时位姿参数相对应的参考位姿参数，并差值计算所述实时位姿参数和所述参考位姿参数之间的位姿偏差；以及

输入计算模块用于根据所述位姿偏差修正所述转向和所述速度，修正后的转向和修正后的速度即为所述车体的运动学模型的修正后的输入；

其中，修正后的转向为修正前的转向叠加转向修正量u_s，所述转向修正量u_S的计算方法包括以下步骤：

S1、将所述位姿偏差输入到所述车体的PID控制器中，得到所述车体的横向修正量u_l和所述车体的航向修正量u_a；

S2、计算第一转向修正量u_S1；计算公式如下：

u_S1＝Au_l+Bu_a，

S3、利用所述车体的纯跟踪控制器计算第二转向修正量u_S2；计算公式如下：

S4、通过第一转向修正量u_S1和第二转向修正量u_S2融合计算得到转向修正量u_S；计算公式如下：

u_S＝Cu_S1+Du_S2；

式中，A为横向修正量的权重系数，B为航向修正量的权重系数，C为所述PID控制的权重系数，D为所述纯跟踪控制的权重系数，L为车体的轴距，θ为当前点与一个预瞄点的连线方向和当前航向之间的航向差，l_d为当前点和一个预瞄点之间的距离；

修正后的速度为修正前的速度叠加速度修正量v_d，所述速度修正量v_d的计算方法为：

v_d＝EΔv_l+FΔv_a

式中，Δv_l为横向速度修正量，Δv_a为航向速度修正量；E为横向速度修正量的权重系数，F为航向速度修正量权重系数。

本发明还公开了一种重载双舵轮AGV车体的运动控制装置，其包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于：所述处理器执行所述程序时实现所述的重载双舵轮AGV车体的运动控制方法的步骤。

与现有技术相比，本发明具有以下有效效果：

本实施的重载双舵轮AGV车体的运动控制方法通过根据当前位置的实时位姿参数与参考位姿参数得到位姿偏差，再将位姿偏差分解为横向位姿偏差和航向位姿偏差。分别将横向位姿偏差和航向位姿偏差输入到PID控制器和纯跟踪控制器中，得到第一转向修正量和第二转向修正量。第一转向修正量和第二转向修正量通过加权叠加的方式得到车体的转向修正量。横向位姿偏差和航向位姿偏差输入到PD控制器中，得到速度修正量。最后利用转向修正量和速度修正量叠加修正前的转向和速度，得到两个舵轮的速度下发量v_1修和v_2修，以及两个舵轮的转向下发量θ_1修和θ_2修。将速度下发量v_1修和v_2修输入到车体的行走电机中，实现对两个舵轮的速度控制；将转向下发量θ_1修和θ_2修输入到车体的运动模型中，实现对两个舵轮的转向控制。通过PID控制器和纯跟踪控制器共同作用，得到车体两个舵轮的转向下发量θ_1修和θ_2修；通过基于位姿偏差及PD控制器形成的差速反馈控制，得到两个舵轮的速度下发量v_1修和v_2修，大大提高了车辆高速运行过程的稳定性，能够避免打滑造成的精度问题及安全隐患，且能够适应不同工况条件，满足高精度的性能要求。

附图说明

图1为重载双舵轮AGV车体的底盘模型图。

图2为重载双舵轮AGV车体的运动控制方法的原理图。

图3为重载双舵轮AGV车体的运动控制方法的流程图。

图4为重载双舵轮AGV车体的运动控制方法中差速辅助转向控制的原理图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，当组件被称为“安装于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。当一个组件被认为是“固定于”另一个组件，它可以是直接固定在另一个组件上或者可能同时存在居中组件。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“或/及”包括一个或多个相关的所列项目的任意的和所有的组合。

实施例1

请参阅图1，本实施例公开了一种重载双舵轮AGV车体的运动控制方法，其通过修正所述车体的运动学模型的输入从而调整车体的运行，运动学模型的输入包括车体的转向电机输出的转向和车体的行走电机输出的速度。车体的前端包括以车体的纵轴对称布置的两个舵轮，车体的后端包括以纵轴对称布置的两个定向轮。两个舵轮可以独立转向与行走，即两个舵轮的理论转向角与速度互不依赖。AGV车体的运动学模型包括以下参数：

R₁＝(R₀-L_D)/sin(θ₁)，

R₂＝(R₀+L_D)/sin(θ₂)，

其中，θ₁和θ₂为两个舵轮的理论转向角，R₁和R₂为两个舵轮的理论旋转半径，R₀为车体的旋转半径，L_D为两个舵轮与车体中心轴之间的距离，L_W为车体的轴距，v₀为车体中心的速度，v₁和v₂为两个舵轮的理论速度。

本实施例中，车体的旋转半径R₀可以根据路线轨迹信息中的路线曲率γ计算。公式为：

请结合图2和图3，重载双舵轮AGV车体的运动控制方法包括以下步骤：

实时采集车体的实时位姿参数。实时位姿参数通过车体本身的检测器实时检测。

在所述车体的路线轨迹上查找到与所述实时位姿参数相对应的参考位姿参数，并差值计算所述实时位姿参数和所述参考位姿参数之间的位姿偏差。路线轨迹信息为车体的理论路线，包括了路线上每个点的参考位姿参数。根据路线轨迹信息可以获得当前位置的参考位姿参数。车体的位姿偏差包括横向位姿偏差e_l和航向位姿偏差e_θ。设定当前位置的实时位姿参数为(x_a,y_a,θ_a)，当前位置的参考位姿参数为(x_r,y_r,θ_r),那么车体的横向位姿偏差e_l和航向位姿偏差e_a为：

e_l＝cos(θ_r)·(y_r-y_a)-sin(θ_r)·(x_r-x_a)

e_a＝θ_r-θ_a

其中，x_a,y_a为当前位置在世界坐标系下X、Y轴上的实际坐标值，θ_a为当前位置在世界坐标系下与X轴之间的角度。x_r,y_r当前位置在世界坐标系下X、Y轴上的参考坐标值。θ_r为当前位置在世界坐标系下与X轴之间的参考角度，两个世界坐标系为同一坐标系。

横向位姿偏差为参考点与当前位置在车体车身坐标系下Y向的偏差，航向位姿偏差为参考点在路线轨迹的切线方向与车体当前位姿的偏差。本实施例中，车体车身坐标系的原点O为两个定向轮连线的的中心，Y轴为两个定向轮连线朝向旋转中心一侧，X轴为指向两个舵轮连线的中心。

根据所述位姿偏差修正所述转向和所述速度，修正后的转向和修正后的速度即为所述车体的运动学模型的修正后的输入。

其中，修正后的转向为修正前的转向叠加转向修正量u_s，所述转向修正量u_s的计算方法包括以下步骤：

S1、将位姿偏差输入到PID控制器中，得到横向修正量u_l和航向修正量u_a。

本实施例中，横向修正量u_l和航向修正量u_a由以下计算公式获得：

其中，

是e_l的微分项，

是e_a的微分项，k_p0和k_p1为比例系数，k_i0和k_i1为积分系数，k_d0和k_d1为微分系数。

S2、计算第一转向修正量u_S1。计算公式如下：

u_S1＝Au_l+Bu_a，

其中，A和B为横向修正量和转向修正量融合叠加的权重系数。本实施例中，权重系数A和所述权重系数B分别满足如下公式：

A＝a·v₀

B＝b·v₀

其中，a是以a_上限、a_下限为上下限的saturation函数，、b是以b_上限、b_下限为上下限的saturation函数，表达式如下：

其中，a_上限、a_下限、b_上限和b_下限为saturation函数的预设值。

S3、利用纯跟踪控制器计算第二转向修正量u_S2。计算公式如下：

其中，L为车体的轴距，θ为当前点与一个预瞄点的连线方向和当前航向之间的航向差，l_d为当前点和一个预瞄点之间的距离。其中预瞄距离l_d为与速度相关的函数，即

l_d＝f(v,t)

f(v,t)表示既可以是线性相关的函数又可以是非线性相关的。

S4、得到PID控制器与纯跟踪控制器的输出后，系统将根据当前的运行状态，即直线行驶、弯道行驶、直线弯道切换等状态，分别对两个控制器的输出建立并选择权重关系计算转向修正量u_S；计算公式为：u_S＝Cu_S1+Du_S2；其中，C和D均为C为舵轮修正量中PID控制的权重系数，D为舵轮修正量中纯跟踪控制的权重系数，C、D可以根据经验取值。具体的，比如当u_S1为某值时，根据经验C取相应的值。也可以通过查表的方式进行确定，预设的表格中，每一个u_S1对应一个C的值。

本实施例中，速度修正量v_d的获取包括以下步骤：

利用PD控制器计算横向速度修正量Δv_l和航向速度修正量Δv_a。计算公式如下：

其中，k_pv0和k_pv1为差速纠偏比例系数，k_dv0和k_dv1为差速纠偏微分系数。

修正后的速度为修正前的速度叠加速度修正量v_d，所述速度修正量v_d的计算方法为：

v_d＝EΔv_l+FΔv_a

式中，Δv_l为横向速度修正量，Δv_a为航向速度修正量；E为横向速度修正量的权重系数，F为航向速度修正量权重系数。本实施例中，E和F可以根据经验取值或者查表取值，其方法与C、D的方法类似。

本实施例中，修正后的速度包括两个舵轮的速度下发量v_1修和v_2修，修正后的转向包括两个舵轮的转向下发量θ_1修和θ_2修。请参阅图4，速度下发量v_1修和v_2修是基于差速辅助转向控制过程得到，具体包括两部分：基于路线曲率信息及运动学的差速前馈控制、基于位姿偏差信息及PD控制的差速反馈控制。公式如下：

θ_1修＝θ₁+u_S，

θ_1修＝θ₁+u_S。

通过将速度下发量v_1修和v_2修以及转向下发量θ_1修和θ_2修输入到车体的运动模型中，实现对两个舵轮的转向控制。

综上，本实施的重载双舵轮AGV车体的运动控制方法通过根据当前位置的实时位姿参数与参考位姿参数得到位姿偏差，再将位姿偏差分解为横向位姿偏差和航向位姿偏差。分别将横向位姿偏差和航向位姿偏差输入到PID控制器和纯跟踪控制器中，得到第一转向修正量和第二转向修正量。第一转向修正量和第二转向修正量通过加权叠加的方式得到车体的转向修正量。横向位姿偏差和航向位姿偏差输入到PD控制器中，得到速度修正量。最后利用转向修正量和速度修正量叠加修正前的转向和速度，得到两个舵轮的速度下发量v_1修和v_2修，以及两个舵轮的转向下发量θ_1修和θ_2修。将速度下发量v_1修和v_2修输入到车体的行走电机中，实现对两个舵轮的速度控制；将转向下发量θ_1修和θ_2修输入到车体的运动模型中，实现对两个舵轮的转向控制。通过PID控制器和纯跟踪控制器共同作用，得到车体两个舵轮的转向下发量θ_1修和θ_2修；通过基于位姿偏差及PD控制器形成的差速反馈控制，得到两个舵轮的速度下发量v_1修和v_2修，大大提高了车辆高速运行过程的稳定性，能够避免打滑造成的精度问题及安全隐患，且能够适应不同工况条件，满足高精度的性能要求。

实施例2

本实施例公开了一种重载双舵轮AGV车体的运动控制系统，其通过修正所述车体的运动学模型的输入从而调整所述车体的运行，所述运动学模型的输入包括所述车体的转向电机输出的转向和所述车体的行走电机输出的速度；所述运动控制系统包括位姿参数获取模块、位姿偏差计算模块和输入计算模块。

位姿参数获取模块用于获取所述车体的实时位姿参数。

位姿偏差计算模块用于在所述车体的路线轨迹上查找到与所述实时位姿参数相对应的参考位姿参数，并差值计算所述实时位姿参数和所述参考位姿参数之间的位姿偏差。

输入计算模块用于根据所述位姿偏差修正所述转向和所述速度，修正后的转向和修正后的速度即为所述车体的运动学模型的修正后的输入。

其中，修正后的转向为修正前的转向叠加转向修正量u_s，所述转向修正量u_S的计算方法包括以下步骤：

S1、将所述位姿偏差输入到所述车体的PID控制器中，得到所述车体的横向修正量u_l和所述车体的航向修正量u_a；

S2、计算第一转向修正量u_S1；计算公式如下：

u_S1＝Au_l+Bu_a，

S3、利用所述车体的纯跟踪控制器计算第二转向修正量u_S2；计算公式如下：

S4、通过第一转向修正量u_S1和第二转向修正量u_S2融合计算得到转向修正量u_S；计算公式如下：

u_S＝Cu_S1+Du_S2；

式中，A为横向修正量的权重系数，B为航向修正量的权重系数，C为所述PID控制的权重系数，D为所述纯跟踪控制的权重系数，L为车体的轴距，θ为当前点与一个预瞄点的连线方向和当前航向之间的航向差，l_d为当前点和一个预瞄点之间的距离；

修正后的速度为修正前的速度叠加速度修正量v_d，所述速度修正量v_d的计算方法为：

v_d＝EΔv_l+FΔv_a

式中，Δv_l为横向速度修正量，Δv_a为航向速度修正量；E为横向速度修正量的权重系数，F为航向速度修正量权重系数。

本实施例具有与实施例1相同的有益效果，在此不再赘述。

实施例3

本实施例还公开了一种重载双舵轮AGV车体的运动控制装置，其包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于：所述处理器执行所述程序时实现如实施例1中的重载双舵轮AGV车体的运动控制方法的步骤。

本实施例中，存储器(即可读存储介质)包括闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等)、随机访问存储器(RAM)、静态随机访问存储器(SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘等。在一些实施例中，存储器可以是计算机设备的内部存储单元，例如该计算机设备的硬盘或内存。在另一些实施例中，存储器也可以是计算机设备的外部存储设备，例如该计算机设备上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)等。当然，存储器还可以既包括计算机设备的内部存储单元也包括其外部存储设备。本实施例中，存储器通常用于存储安装于计算机设备的操作系统和各类应用软件等。此外，存储器还可以用于暂时地存储已经输出或者将要输出的各类数据。

处理器在一些实施例中可以是中央处理器(Central Processing Unit，CPU)、控制器、微控制器、微处理器、或其他数据处理芯片。该处理器通常用于控制计算机设备的总体操作。本实施例中，处理器用于运行存储器中存储的程序代码或者处理数据，以实现前述实施例1中重载双舵轮AGV车体的运动控制的处理过程，从而提高车体控制准确性。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明申请范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明申请的保护范围应以所附权利要求为准。

Claims (10)

1.一种重载双舵轮AGV车体的运动控制方法，其特征在于，其通过修正所述车体的运动学模型的输入从而调整所述车体的运行，所述运动学模型的输入包括所述车体的转向电机输出的转向和所述车体的行走电机输出的速度；所述运动控制方法包括以下步骤：

实时采集所述车体的实时位姿参数；

在所述车体的路线轨迹上查找到与所述实时位姿参数相对应的参考位姿参数，并差值计算所述实时位姿参数和所述参考位姿参数之间的位姿偏差；

根据所述位姿偏差修正所述转向和所述速度，修正后的转向和修正后的速度即为所述车体的运动学模型的修正后的输入；

其中，修正后的转向为修正前的转向叠加转向修正量u_s，所述转向修正量u_S的计算方法包括以下步骤：

S1、将所述位姿偏差输入到所述车体的PID控制器中，得到所述车体的横向修正量u_l和所述车体的航向修正量u_a；

S2、计算第一转向修正量u_S1；计算公式如下：

u_S1＝Au_l+Bu_a，

S3、利用所述车体的纯跟踪控制器计算第二转向修正量u_S2；计算公式如下：

S4、通过第一转向修正量u_S1和第二转向修正量u_S2融合计算得到转向修正量u_S；计算公式如下：

u_S＝Cu_S1+Du_S2；

式中，A为横向修正量的权重系数，B为航向修正量的权重系数，C为所述PID控制的权重系数，D为所述纯跟踪控制的权重系数，L为车体的轴距，θ为当前点与一个预瞄点的连线方向和当前航向之间的航向差，l_d为当前点和一个预瞄点之间的距离；

修正后的速度为修正前的速度叠加速度修正量v_d，所述速度修正量v_d的计算方法为：

v_d＝EΔv_l+FΔv_a

式中，Δv_l为横向速度修正量，Δv_a为航向速度修正量；E为横向速度修正量的权重系数，F为航向速度修正量权重系数。

2.根据权利要求1所述的重载双舵轮AGV车体的运动控制方法，其特征在于，所述车体的前端包括以所述车体的纵轴对称布置的两个舵轮，所述车体的后端包括以所述纵轴对称布置的两个定向轮。

3.根据权利要求2所述的重载双舵轮AGV车体的运动控制方法，其特征在于，所述横向修正量u_l和所述航向修正量u_a由以下计算公式获得：

其中，

是e_l的微分项，

是e_a的微分项，k_p0和k_p1为比例系数，k_i0和k_i1为积分系数，k_d0和k_d1为微分系数，e_l为横向位姿偏差，e_a为航向位姿偏差，所述位姿偏差包括横向位姿偏差e_l和航向位姿偏差e_a。

4.根据权利要求1所述的重载双舵轮AGV车体的运动控制方法，其特征在于，所述横向速度修正量Δv_l和航向速度修正量Δv_a由以下计算公式获得：

其中，k_pv和k_pv1为差速纠偏比例系数，k_dv0和k_dv1为差速纠偏微分系数。

5.根据权利要求1所述的重载双舵轮AGV车体的运动控制方法，其特征在于，所述车体的运动学模型包括以下参数：

R₁＝(R₀-L_D)/sin(θ₁)，

R₂＝(R₀+L_D)/sin(θ₂)，

其中，θ₁和θ₂为两个舵轮的理论转向角，R₁和R₂为两个舵轮的理论旋转半径，R₀为车体的旋转半径，L_D为两个舵轮与车体中心轴之间的距离，L_W为车体的轴距，v₀为车体中心的速度，v₁和v₂为两个舵轮的理论速度。

6.根据权利要求5所述的重载双舵轮AGV车体的运动控制方法，其特征在于，修正后的速度包括两个舵轮的速度下发量v_1修和v_2修；公式如下：

7.根据权利要求6所述的重载双舵轮AGV车体的运动控制方法，其特征在于，修正后的转向包括两个舵轮的转向下发量θ_1修和θ_2修；公式如下：

θ_1修＝θ₁+u_S，

θ_2修＝θ₂+u_S。

8.根据权利要求1所述的重载双舵轮AGV车体的运动控制方法，其特征在于，所述权重系数A和所述权重系数B分别满足如下公式：

A＝a·v₀

B＝b·v₀

其中，a、b满足saturation函数，表达式如下：

其中，a_上限、a_下限、b_上限和b_下限为saturation函数的预设值。

9.一种重载双舵轮AGV车体的运动控制系统，其特征在于，其通过修正所述车体的运动学模型的输入从而调整所述车体的运行，所述运动学模型的输入包括所述车体的转向电机输出的转向和所述车体的行走电机输出的速度；所述运动控制系统包括：

位姿参数获取模块，其用于获取所述车体的实时位姿参数；

位姿偏差计算模块，其用于在所述车体的路线轨迹上查找到与所述实时位姿参数相对应的参考位姿参数，并差值计算所述实时位姿参数和所述参考位姿参数之间的位姿偏差；以及

输入计算模块，其用于根据所述位姿偏差修正所述转向和所述速度，修正后的转向和修正后的速度即为所述车体的运动学模型的修正后的输入；

其中，修正后的转向为修正前的转向叠加转向修正量u_s，所述转向修正量u_S的计算方法包括以下步骤：

S1、将所述位姿偏差输入到所述车体的PID控制器中，得到所述车体的横向修正量u_l和所述车体的航向修正量u_a；

S2、计算第一转向修正量u_S1；计算公式如下：

u_S1＝Au_l+Bu_a，

S3、利用所述车体的纯跟踪控制器计算第二转向修正量u_S2；计算公式如下：

S4、通过第一转向修正量u_S1和第二转向修正量u_S2融合计算得到转向修正量u_S；计算公式如下：

u_S＝Cu_S1+Du_S2；

式中，A为横向修正量的权重系数，B为航向修正量的权重系数，C为所述PID控制的权重系数，D为所述纯跟踪控制的权重系数，L为车体的轴距，θ为当前点与一个预瞄点的连线方向和当前航向之间的航向差，l_d为当前点和一个预瞄点之间的距离；

修正后的速度为修正前的速度叠加速度修正量v_d，所述速度修正量v_d的计算方法为：

v_d＝EΔv_l+FΔv_a

式中，Δv_l为横向速度修正量，Δv_a为航向速度修正量；E为横向速度修正量的权重系数，F为航向速度修正量权重系数。

10.一种重载双舵轮AGV车体的运动控制装置，其特征在于，其包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于：所述处理器执行所述程序时实现如权利要求1至8中任意一项所述的重载双舵轮AGV车体的运动控制方法的步骤。

Images