CN116185009A

CN116185009A - 基于复合导航的差速agv纠偏控制系统及方法

Info

Publication number: CN116185009A
Application number: CN202211707448.8A
Authority: CN
Inventors: 奚青; 陈曲燕; 周德强
Original assignee: Wuxi Hongyi Intelligent Technology Co ltd
Current assignee: Wuxi Hongyi Intelligent Technology Co ltd
Priority date: 2022-12-29
Filing date: 2022-12-29
Publication date: 2023-05-30

Abstract

本发明涉及一种基于复合导航的差速AGV纠偏控制系统及方法，方法包括：采集车轮编码器数据和微型惯性测量单元数据，利用扩展卡尔曼滤波算法对两组数据进行融合，获得位于两个二维码之间区域内的AGV行进时的实际位姿；规划位于两个二维码之间的纠偏路径：以实际位姿为输入项，在约束条件的约束下，通过控制点调整和终点位姿调整完成对B样条曲线的优化求解，获得目标曲线；采用滑膜控制器作为路径跟踪控制器，以实际位姿为路径跟踪控制器的输入项，以AGV中心质点的速度和角速度为被控目标，控制实际位姿与目标曲线的目标位姿之间差值最小，实现对目标曲线的路径跟踪控制。完善了AGV轨迹推算能力提高了AGV纠偏系统的稳定性和平滑性。

Description

基于复合导航的差速AGV纠偏控制系统及方法

技术领域

本发明涉及AGV纠偏控制技术领域，尤其是一种基于复合导航的差速AGV纠偏控制系统及方法。

背景技术

自动导引车(Automated Guided Vehicle,AGV)是一种按照规划路径自动行驶可搬运货物的智能无人驾驶车辆，目前AGV主要通过磁条、二维码、激光slam或视觉slam等导航方式实现按照预定规划好的路径运动。由于行驶过程中一些不确定因素的干扰，AGV会偏离既定轨迹，如何提高AGV路径纠偏的效率和精度是这一领域的热点问题。AGV的运动控制根据运动规划输出和实时反馈的车辆行驶状态来控制底盘执行器的动作，使车辆稳定、平滑、精确地跟踪期望路径/轨迹。运动控制作为AGV系统的核心环节之一，其性能表现直接影响AGV行驶安全和仓库运行效率。

现有技术中，纠偏控制中有一类是仅使用控制器进行纠偏，其纠偏方式采用模糊控制器、PID控制器、模糊PID等，完成控制的先决条件在于精确的横向偏差与航向角偏差，其后所设计控制器要合理分配以上两个偏差的权重，用以优先考虑何种偏差，该方式多以人为决定，加大了对轨迹的推算难度，导致横向偏差与航向角偏差的失真，该信息传入又导致控制的失真，最终无法收敛到理想路径状态。故该方案一般多用于磁导引AGV，由速度差反复切换所产生的颤动不明显固可用于低速场景中，但AGV高速行驶时，该方法将导致AGV左右横摆，极易造成车体剧烈抖动，严重时造成车体冲出轨道范围，发生碰撞事故。还有一类是将平面坐标和角度偏差信息作为模型预测控制、鲁棒控制、线性最优控制等的输入项，完成对路径的跟踪，该方案同样需要以精确的输入信息作为先决条件，该方案存在的问题主要体现在轨迹推算精度较低，采用单一传感器推算较多，采用多传感器融合往往并未融合出较高的推算精度。其次，对于纠偏轨迹的规划是多样的，集中于贝塞尔曲线和B样条曲线，其中B样条曲线的应用目前较少，且针对的应用场景不同需要不同的设计方案，目前多用于明确起始点和终点的情况下，并且对于曲线调整的策略也是一个影响纠偏精度的重要因素。路径跟踪控制器是多样的，但多数要么基于较高的算力条件，要么基于较高的经验水平，使用效果并不好。

发明内容

针对现有技术的不足，本发明提供一种基于复合导航的差速AGV纠偏控制系统及方法，目的是完善AGV轨迹推算能力并提高AGV纠偏系统的稳定性和平滑性。

本发明采用的技术方案如下：

本申请提供一种基于复合导航的差速AGV纠偏控制方法，包括：

实时位姿推算：采集车轮编码器数据和微型惯性测量单元数据，利用扩展卡尔曼滤波算法对两组数据进行融合，获得位于两个二维码之间区域内的AGV行进时的实际位姿；

纠偏轨迹规划，规划位于两个二维码之间的纠偏路径：以所述实际位姿为输入项，在约束条件的约束下，通过控制点调整和终点位姿调整完成对B样条曲线的优化求解，获得目标曲线；

路径跟踪：采用滑膜控制器作为路径跟踪控制器，以所述实际位姿为路径跟踪控制器的输入项，以AGV中心质点的速度和角速度为被控目标，控制所述实际位姿与所述目标曲线的目标位姿之间差值最小，实现对目标曲线的路径跟踪控制。

进一步技术方案为：

纠偏轨迹规划过程，所述控制点调整和终点位姿调整包括：

将连接两个二维码的直线作为参考路径，读取二维码信息并结合所述实际位姿获得B样条曲线的起点P₁的位姿状态[x₁ y₁θ₁]，选取所述参考路径上某点P_N作为到AGV距离最近的初始终点，其位姿状态[x_N y_Nθ_N]，在起点P₁的前、后侧距离其L₁位置处增加控制点P₀、P₂，在初始终点P_N的前、后侧距离其L₂位置处增加控制点P_N-1、P_N+1，

与过P₁的AGV车头方向相同、/>

与过P_N的AGV车头方向相同，设定L₁＝L₂＝l₂，获得各控制点对应的位姿状态，其中x、y、θ分别为横、纵坐标和AGV车头方向与参考路径之间的夹角；

对于给定距离l₂，以点P_N为基点按照步长l₁沿所述参考路径进行终点寻找，对终点进行约束条件验证，若满足约束条件，则寻得终点，若无法寻得终点，调整l₂继续进行终点寻找，若仍无法寻得终点，调整l₁后继续进行终点寻找，重复以上对l₂、l₁的调整过程，直至寻得最优终点。

所述约束条件包括：

B样条曲线的曲率k小于等于最大曲率k_max＝1/R_min，R_min为AGV急转弯时所需的最小转弯半径。

所述约束条件包括：

B样条曲线在其起点及终点满足：

以实际位姿[x yθ]^T与目标曲线的目标位姿[x_g y_gθ_g]^T之间的差值[x_e y_eθ_e]^T作为滑膜控制器的滑膜函数：

其中，

结合差速AGV的运动模型获得所述滑膜函数的微分方程：/>

式中，AGV中心质点角速度ω＝(v_l-v_r)/L，L为左、右车轮轮距，AGV中心质点速度v＝(v_l+v_r)/2，v_l、v_r分别为左、右车轮速度；

基于双幂次趋近律将所述微分方程转化为下式：

式中，k_1x,k_2x,k_1y,k_2y,k_1θ,k_2θ,r1_x,r1_y,r1_θ,r2_x,r2_y,r2_θ,ε_x,ε_y,ε_θ均为无具体意义的数学参数，且均大于0；

以[vω]^T为滑膜控制器的被控对象，不断求解上式，使差值[x_e y_eθ_e]^T逐渐向0收敛，将所得的v_l、v_r不断返回给AGV调整其姿态，达到路径跟踪的目的。

所述车轮编码器的输出模型如下：

x_k＝f(x_k-1,μ_k-1)+W_k-1

式中：x_k＝[x₀ y₀θ₀]^T为车轮编码器k时刻测量的位姿状态向量，f为k-1时刻与状态向量x_k-1、编码器控制系统输入量u_k-1有关的非线性函数，W_k-1为编码器控制系统在k-1时刻的高斯白噪声布。

所述微型惯性测量单元的输出模型如下：

z_k＝h(x_k,v_k)+V_k

式中：z_k＝[x_i y_iθ_i]^T为微型惯性测量单元k时刻观测的位姿状态向量，h为k时刻关于状态向量x_k、微型惯性测量单元控制器输入量v_k的非线性函数，V_k为微型惯性测量单元控制器在k时刻的观测噪声，呈高斯正态分布。

本申请还提供一种基于复合导航的差速AGV纠偏控制系统，包括：

实时位姿推算模块，用于采集车轮编码器数据和微型惯性测量单元数据，利用扩展卡尔曼滤波算法对两组数据进行融合，获得位于两个二维码之间区域内的AGV行进时的实际位姿；

纠偏轨迹规划模块，用于规划位于两个二维码之间的纠偏路径：以所述实际位姿为输入项，在约束条件的约束下，通过控制点调整和终点位姿调整完成对B样条曲线的优化求解，获得目标曲线；

路径跟踪模块，用于采用滑膜控制器作为路径跟踪控制器，以所述实际位姿为路径跟踪控制器的输入项，以AGV中心质点的速度和角速度为被控目标，控制所述实际位姿与所述目标曲线的目标位姿之间差值最小，实现对目标曲线的路径跟踪控制。

本发明的有益效果如下：

本发明的纠偏控制方法通过卡尔曼滤波数据融合为路径跟踪提供精确的输入信息，通过B样条曲线规划的结果为跟踪控制降低了难度，通过滑膜跟踪控制的平滑度，同时又对数据融合产生影响，跟踪越平滑，则数据融合的结果越优。

本发明的纠偏控制系统通过三个模块的相互作用，提高了纠偏的精度和应用场景范围，在无需对AGV运行速度造成过大影响的前提下，保证了系统的高效性，相较于其他纠偏方式，在实现程度和精度上都具备较高的优势，整体纠偏精度达到5mm以内，满足市面在售AGV的精度要求。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。

附图说明

图1为本发明实施例的方法流程示意图。

图2为本发明实施例的小车两轮差速瞬时运动模型。

图3为本发明实施例的纠偏轨迹示意图。

图4为本发明实施例的滑膜控制器的结构示意图。

具体实施方式

以下结合附图说明本发明的具体实施方式。

参见图1，本申请提供一种基于复合导航的差速AGV纠偏控制方法，包括：

本申请采用扩展卡尔曼滤波完成对轮式编码器与微型惯性测量单元(MIMU)数据融合，由于惯性测量单元MIMU估算的AGV的运动轨迹相对于编码器的估算来说在短时间内准确度更高，但由于MIMU中加速度计和陀螺仪的双重误差累积，在长时间的运作下，对车辆的定位准确度明显下降，此时应更相信由车轮编码器估算的轨迹。通过融合可极大提高轨迹推算精度，在直线及弯道共存情况下可达到2mm推算精度，相较于采用单一传感器的推算精度取得了较大的提升。

本申请采用控制点调整和终点位姿调整方式完成对B样条曲线的优化，可快速高效求解可行目标曲线，所规划的目标曲线提高了规划效率，降低了曲线跟踪难度，提升了纠偏顺滑度。

本申请采用基于双幂次趋近律的轨迹跟踪滑模控制器，相较于基于指数趋近律的AGV轨迹跟踪滑模控制器具有更高的稳定性和响应速度，同时有效的降低了抖振的影响，具有快速精确的跟踪效果。

以下以具体实施例进一步说明本申请的基于复合导航的差速AGV纠偏控制方法，包括：

S1、实时位姿推算：

二维码导航AGV获取自身位姿[x yθ]的方式为读取二维码，二维码张贴是有一定距离的，位于两个二维码之间时，AGV无法获取自身的位姿，其中x、y、θ分别为横、纵坐标和AGV车头方向与参考路径之间的夹角。

本实施例AGV携带MIMU和编码器传感器，二者均可以进行轨迹推算，但MIMU的推算会随着时间的积累产生较大的误差，编码器的推算准确度较低。因此融合两个传感器数据从而得到精确的位姿是十分必要的。，以MIMU数据作为观测模型，以车轮编码器数据作为预测模型。分别建立MIMU航迹推算模型与车轮编码器航迹推算模型。之后采用扩展卡尔曼滤波(EKF)作为融合算法对二者数据进行融合，得到可靠的位姿信息，作为后续路径跟踪控制器的信息来源。

所述车轮编码器的输出模型如下：

x_k＝f(x_k-1,μ_k-1)+W_k-1

上述车轮编码器的输出模型的构建过程如下：

记车轮转动一周编码器产生的脉冲数为N，记测得的脉冲数差值为M，有

式中：

为左、右轮间隔时间内转过角度，以弧度为单位；M_L,M_R为左、右轮间隔时间内脉冲数增量。

建立瞬时小车位姿变化图如图2所示，δ_l为AGV走过的弧度，δ_x,δ_y分别为δ_l沿x轴、y轴的分量，δ_θ为AGV航向角变化值，R为AGV瞬时转弯半径。

AGV的速度、位置和偏航角都可以通过左右轮的运动进行推算，AGV行驶距离δ_l可以表示为：

上式中，R_wheel为驱动轮半径；

θ变化值δ_θ可用两轮运动距离和两轮间距d表示：

进而可求得AGV瞬时转动半径R：R＝δ_lδ_θ，由余弦定理，AGV的位置变化量可以表示为：

当AGV作直线运动时，δ_θ为0，此时计算得到的车身转动半径无穷大，从而导致位置变化量为0的情况，显然是不符合实际状况的。故需要通过一定变换以适应此类情形，使用泰勒级数将上述展开，进而可以推算出AGV位姿。

所述微型惯性测量单元的输出模型如下：

z_k＝h(x_k,v_k)+V_k

所述微型惯性测量单元的输出模型的构建过程包括：

MIMU内部包括一个三轴陀螺仪和一个三轴加速度计，陀螺仪测得的数据是角速度，对角速度进行积分即可得到物体运动的转角。加速度计测得的数据是物体运动的加速度，对测得的三轴加速度的合加速度进行一次积分即可得到物体运动的速度，进行二次积分即可得到位移。

扩展卡尔曼滤波适用于非线性系统，是在上一时刻的后验估计状态量下进行泰勒展开得到这附近的线性系统，然后再对系统状态进行估计。扩展卡尔曼滤波包括预测和更新2个部分。

S2、纠偏轨迹规划：

本实施例的纠偏轨迹规划并非全局规划，而是局部规划，仅规划位于两个二维码之间的纠偏路径。通过控制点调整和终点位姿调整，不断调整局部曲线，涉及局部调整和全局调整，完成对B样条曲线的优化求解，获得满足约束的样条曲线，该曲线不以牺牲AGV运行速度为代价，提高了纠偏的平滑度与速度，降低了后续路径跟踪控制器的跟踪难度。

具体的，B样条曲线为3次B样条曲线，第i段B样条曲线段的表达式为：

C_i+j为第i段曲线段中第j个控制点，B样条基函数

s为0或1，/>

基于上述表达式通过一阶、二阶求导，可得B样条曲线上的点及B样条曲线的曲率。

由于B样条曲线本身不通过首末控制点，为了使B样条曲线通过规划路径的起点和终点并满足起点和终点的航向角约束，以起点和终点为中心点，在其两端各增加一个控制点，具体包括，如图3所示：

控制点调整：将连接两个二维码的直线作为参考路径(图中长度为1070mm的虚线所示)，读取二维码信息并结合所述实际位姿获得B样条曲线的起点P₁的位姿状态[x₁ y₁θ₁]，选取所述参考路径上某点P₄作为到AGV距离最近的初始终点，其位姿状态[x₄ y₄θ₄]，在起点P₁的前、后侧距离其L₁位置处增加控制点P₀、P₂，在初始终点P_N的前、后侧距离其L₂位置处增加控制点P₃、P₅，

与过P₁、P₄的AGV车头方向相同、/>

与过P₄的AGV车头方向相同，设定L₁＝L₂＝l₂，获得各控制点对应的位姿状态，其中x、y、θ分别为横、纵坐标和AGV车头方向与参考路径之间的夹角；

P₀P₂＝2L₁，P₃P₅＝2L₂，获得各控制点对应的位姿状态：

对于给定距离l₂，以点P₄为基点按照步长l₁沿所述参考路径进行终点寻找，对终点进行约束条件验证，若满足约束条件，则寻得终点，若无法寻得终点，调整l₂继续进行终点寻找，若仍无法寻得终点，调整l₁后继续进行终点寻找，重复以上对l₂、l₁的调整过程，直至寻得最优终点。

上述优化求解本质上由两个一维搜索构成，其中行进距离l₁的一维搜索内嵌在构造距离l₂一维搜索的内部，即l₂每移动一个步长，都将通过一维搜索求得最优的行进距离l₁。循环往复，最终寻找到符合多约束条件的最优目标路径，从而完成路径规划目标。

所述约束条件包括：

B样条曲线在其起点及终点满足：

S3、路径跟踪：采用如图4所示的滑膜控制器作为路径跟踪控制器，以所述实际位姿为路径跟踪控制器的输入项，以AGV中心质点的速度和角速度为被控目标，控制所述实际位姿与所述目标曲线的目标位姿之间差值最小，实现对目标曲线的路径跟踪控制，具体包括：

其中，

结合差速AGV的运动模型获得所述滑膜函数的微分方程：/>

将AGV差速运动模型代入上式，基于双幂次趋近律将所述微分方程转化为下式：

AGV差速运动模型表达式如下：

其中：

本申请的实施例还提供一种基于复合导航的差速AGV纠偏控制系统，包括：

本领域普通技术人员可以理解：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。