一种变质心无人搬运车轨迹跟踪控制方法与流程

本发明涉及一种变质心无人搬运车轨迹跟踪控制方法，属于控制系统技术领域。

背景技术

无人搬运车转矩响应快、全轮独立可控，对提高车辆搬运的灵活性、稳定性和轨迹跟踪能力具有重要的研究意义，已成为学术界和工业界的开发热点。

目前，关于轨迹跟踪研究未考虑无人搬运车在工位间装载、卸载货物，导致整车质心发生变化对轨迹跟踪控制产生的影响；众多理论成果有效提高了车辆行驶过程中的跟踪控制的稳定性，却忽视车辆轨迹跟踪的精准性研究。

轨迹跟踪的精准性是保证四轮转向车辆控制器应用于复杂环境、且能适应系统误差的关键。因此，轨迹跟踪的精准性研究成为了底盘智能化发展的关键问题。

技术实现要素：

本发明的目的在于克服现有技术中的不足，提供一种变质心无人搬运车轨迹跟踪控制方法，以解决现有技术中无人搬运车轨迹跟踪控制精准性差的技术问题。

为解决上述技术问题，本发明是采用下述方案实现的：

本发明提供了一种变质心无人搬运车轨迹跟踪控制方法，包括如下步骤：对无人搬运车进行动力学和运动学理论分析，建立无人搬运车动力学和运动学模型；基于Lyapunov直接法，根据无人搬运车动力学和运动学模型，建立全局渐进轨迹跟踪控制器,对参考轨迹进行全局跟踪；优化粒子群算法，自适应地调整轨迹跟踪控制器输入的控制参数。

优选的，所述建立无人搬运车动力学和运动学模型包括如下步骤：所述无人搬运车动力学模型为：

纵向运动方程：

其中，m是车辆质量；为纵向加速度；vy为车体坐标系下质心的纵向速度；γ为横摆角速度；Fxi和Fyi(i＝fl，fr，rl，rr)分别为轮胎纵向力和侧向力；δfl、δfr分别是前轮左右转向角；δrl、δrr为后轮左右转向角；

横向运动方程：

其中，为侧向的加速度；vx为车体坐标系下质心的纵向速度；

横摆运动方程：

其中，IZ为车辆绕Z轴的转动惯量；为横摆角加速度；lf和lr分别为质心到前轴和后轴的距离；d1、d2分别为左轮和右轮到等效轮的距离；

所述无人搬运车运动学模型：

设置无人搬运车质心[x，y]^T的笛卡儿坐标，θ为无人搬运车前进方向与X轴夹角，v(t)、w(t)分别为无人搬运车的平移速度和旋转速度，φ1、φ2为无人搬运车等效轮的转速，VF、VR为无人搬运车等效轮的运动速度，车轮的直径为r。考虑无人搬运车满足刚体运动规律，建立如下运动方程；

定义位置矢量q＝(x,y,θ,φ1,φ2)^T,无人搬运车运动模型为：

优选的，所述建立全局渐进轨迹跟踪控制器包括如下步骤：定义无人搬运车的单签位姿为P＝[x,y,θ]^T，当前速度为[v,w]^T，参考位置为Pr＝[xr,yr,θr]^T，参考速度为[vr,wr]^T，位姿误差为Pe＝Pr-P＝[xe,ye,θe]^T；

设置控制约束：寻找有界控制输入U＝[v,w]^T，使得无人搬运车的实际位姿能够跟踪参考位姿，并满足

结合无人搬运车运动学模型得到：

对(10)式求导得到轨迹跟踪误差的微分方程：

设置控制律，k为控制参数：

通过Lyapunov函数变换，得到：

对(13)式进行求导，并代入轨迹跟踪误差的微分方程得到：

优选的，所述优化粒子群算法包括如下步骤：定义在D维空间搜索的粒子群，用D维向量表达为：

其中，Vi＝(vi1,vi2,vi3,........,viD)为粒子群速度信息，Xi＝(xi1,xi2,xi3,........,xiD)为粒子群位置信息，Pi＝(pi1,pi2,vi3,........,piD)为粒子群粒子的个体最优值，Pg＝(pg1,pg2,vg3,........,pgD)为粒子群粒子全局最优值，k为粒子群的迭代次数，r1、r2为[0,1]的随机数，ω为惯性权重，c1、c2为学习因子。

优选的，所述优化粒子群算法还包括如下步骤：设置自适应惯性权重ω(t)

其中，f为粒子群当前适应度值，fmin为粒子群最小适应度值，wmax和wmin为设定的粒子群惯性权重最大和最小值；fg为当前所有粒子的适应度均值,F为当前适应度值，imax表示粒子的最大迭代次数；i表示当前的迭代次数；

设置c1的数值随着迭代次数的增加而增大，c2的数值随着迭代次数的增加而减小，即：

则自适应粒子群表达式为：

优选的，所述优化粒子群算法还包括如下步骤：对自适应粒子群算法的初始参数进行设定，在设定的区域内随机初始化每个粒子的位置；

计算出自适应学习因子和自适应权重的数值，更新粒子群中每个粒子的位置；

计算出粒子群中各粒子的适应度值，将计算出的各粒子适应值与其经历过的最好位置作比较，更新粒子群粒子个体最优值；

比较当前所有个体极值和全局极值，更新粒子群粒子全局最优值；

对粒子群的各粒子适应值进行排序，用群体中部分最佳的粒子位置代替部分最差的粒子位置；

保持粒子个体最优值和全局最优值不变重复上述步骤，直至满足算法的停止条件，输出计算结果。

与现有技术相比，本发明所达到的有益效果是：

1、本发明结合Lyapunov直接法与自适应粒子群算法设计自适应全局渐进跟踪控制器，在变质心工况中控制器的自适应性能及时调整控制参数进而降低轨迹跟踪的位置误差，提高搬运车的轨迹跟踪精准性。

2、本发明在变质心工况中进行双扭线轨迹跟踪仿真和实验，验证自适应全局渐进控制器的真实性和可靠性。

附图说明

图1是本发明实施例提供的一种无人搬运车结构参数图；

图2是本发明实施例提供的一种轨迹跟踪控制流程图；

图3是本发明实施例提供的一种自适应粒子群算法流程图；

图4是本发明实施例提供的平面跟踪控制仿真轨迹；

图5为本发明实施例提供的两种轨迹跟踪控制下的X轴位置偏差；

图6为本发明实施例提供的两种轨迹跟踪控制下的Y轴位置偏差；

图7为本发明实施例提供的平面跟踪控制实验轨迹图；

图8为本发明实施例提供的轨迹跟踪控制实验中无人搬运车样机的X、Y轴位置偏差。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

如图1所示，对无人搬运车进行动力学建模，无人搬运车的稳定性主要由横向运动和横摆运动决定，因此搭建的车身模型只需考虑沿x轴的纵向运动，y轴的横向运动以及绕z轴的横摆运动。建立的动力学模型用于控制器的预测模型，在描述车辆动力学过程的基础上进行简化，以减少控制算法的计算量。在车辆动力学建模时进行以下理想化假设。假设无人搬运车在平坦路面上行驶，忽略车辆垂向运动；只考虑纯侧偏轮胎特性，忽略横向、纵向轮胎力的耦合关系、轮胎的横向载荷转移；忽略横向和纵向空气动力学对车辆横摆特性的影响。基于上述假设得到车辆动力学模型，对车辆进行受力分析，根据其受力平衡和力矩平衡得到三自由度运动方程如下：

纵向运动方程：

式中，m是车辆质量；为纵向加速度；vy为车体坐标系下质心的纵向速度；γ为横摆角速度；Fxi和Fyi(i＝fl，fr，rl，rr)分别为轮胎纵向力和侧向力；δfl、δfr

分别是前轮左右转向角；δrl、δrr为后轮左右转向角。

横向运动方程：

式中，为侧向的加速度；vx为车体坐标系下质心的纵向速度。

横摆运动方程：

式中，IZ为车辆绕Z轴的转动惯量；为横摆角加速度；lf和lr分别为质心到前轴和后轴的距离；d1、d2分别为左轮和右轮到等效轮的距离。

此外，还对无人搬运车进行运动学建模，无人搬运车运动学基本内容是根据车轮的运动状态得到车辆的状态，根据车移动距离来推算无人搬运车在大地坐标系中的位置和借助惯性导航系统完成。由运动学问题可以获得无人搬运车的移动速度、自旋角速度和控制参数。

本发明采用质心为[x，y]^T的笛卡儿坐标，θ为无人搬运车前进方向与X轴夹角，v(t)、w(t)分别为无人搬运车的平移速度和旋转速度，φ1、φ2为无人搬运车等效轮的转速，VF、VR为无人搬运车等效轮的运动速度，车轮的直径为r。考虑无人搬运车满足刚体运动规律，建立如下运动方程：

定义位置矢量q＝(x,y,θ,φ1,φ2)^T,则无人搬运车运动模型可以表示为：

如图2所示，根据动力学和运动学模型设计复合跟踪误差，利用Lyapunov直接法，构造出具有全局渐进稳定的全局渐进跟踪控制器，实现对参考轨迹的全局渐进跟踪。假设无人搬运车的单签位姿为P＝[x,y,θ]^T，当前速度为[v,w]^T，参考位置为Pr＝[xr,yr,θr]^T，参考速度为[vr,wr]^T，位姿误差为Pe＝Pr-P＝[xe,ye,θe]^T，此时无人搬运车的轨迹跟踪问题是对任意初始位姿及速度误差，寻找有界控制输入U＝[v,w]^T，使得无人搬运车的实际位姿能够跟踪参考位姿，并满足：

结合无人搬运车运动学模型得到：

对上式求导可得轨迹跟踪误差的微分方程：

设计控制律，k为控制参数：

通过Lyapunov函数变换，可以得到

对上式进行求导，并代入轨迹跟踪误差的微分方程：

当存在位置误差且控制参数k1、k2、k3为正常数，无人搬运车速度大于零，加速度小于零。所以控制律使得系统渐进收敛于平衡点Pe＝0，即无人搬运车可以跟踪上参考轨迹。

为确认控制控制器参数k1、k2、k3数值大小，本文采用优化粒子群算法对控制参数进行整定。假设在D维空间搜索的粒子群，用D维向量表达，表达式为：

其中Vi＝(vi1,vi2,vi3,........,viD)表示为粒子群速度信息，Xi＝(xi1,xi2,xi3,........,xiD)表示为粒子群位置信息，Pi＝(pi1,pi2,vi3,........,piD)表示为粒子群粒子的个体最优值，Pg＝(pg1,pg2,vg3,........,pgD)表示为粒子群粒子全局最优值，k为粒子群的迭代次数，r1、r2为[0,1]的随机数，ω为惯性权重，c1、c2为学习因子。

在粒子群优化算法中存在局部寻优导致准确性降低的缺点，因此本发明对粒子的惯性权重和学习因子进行自适应调整，优化粒子群算法。并通过自适应粒子群算法对跟踪误差参数进行整定。

传统粒子群算法中，惯性权重设定为一个常数。惯性权重的值越大，粒子群算法中粒子的全局搜索能力越好；惯性权重的值越小，粒子局部搜索的能力越好。若设定的惯性权重值不变，则粒子的搜索能力不变且会出现局部寻优的情况。因此，设自适应惯性权重ω(t)

其中f是粒子群当前适应度值，fmin是粒子群最小适应度值，wmax和wmin是设定的粒子群惯性权重最大和最小值。fg为当前所有粒子的适应度均值,F为当前适应度值，imax表示粒子的最大迭代次数；i表示当前的迭代次数。

粒子群算法中学习因子c1的数值决定了粒子局部搜索能力的高低，学习因子c2的数值决定了粒子最优值收敛速度的快慢，传统粒子群算法设定c1、c2为常数。本文令c1的数值随着迭代次数的增加而增大，c2的数值随着迭代次数的增加而减小，即：

因此结合上述公式自适应粒子群表达式为；

利用自适应粒子群算法优化控制参数k1、k2、k3，则自适应粒子群为三维空间，速度和位置信息分别为Vi＝(vi1,vi2,vi3)、Xi＝(xi1,xi2,xi3)，且界定值分别为(vmin,vmax)、(xmin,xmax)。自适应粒子群算法采用适应值函数判断算法的优劣，并选用跟踪控制器求得的轨迹跟踪误差为适应值，轨迹跟踪误差数值越小则算法参数求解效果越佳，具体优化步骤如图3所示：

首先，对自适应粒子群算法的初始参数进行设定，在设定的区域内随机初始化每个粒子的位置。计算出自适应学习因子和自适应权重的数值，更新粒子群中每个粒子的位置。其次，计算出粒子群中各粒子的适应度值，将计算出的各粒子适应值与其经历过的最好位置作比较，更新粒子群粒子个体最优值。比较当前所有个体极值和全局极值，更新粒子群粒子全局最优值。最后，对粒子群的各粒子适应值进行排序，用群体中部分最佳的粒子位置代替部分最差的粒子位置，同时保持粒子个体最优值和全局最优值不变重复上诉步骤，直至满足算法的停止条件。

除此之外，本发明还提供通过使用MATLAB软件进行双纽线轨迹跟踪控制仿真，验证本发明针对无人搬运车提出的轨迹跟踪控制策略的有效性。首先模拟无人搬运车的实际工况设计了一条双扭线轨迹，其次使用本文设计的轨迹跟踪控制器与MPC控制器对期望轨迹进行跟踪控制，并对仿真结果进行对比验证。其中无人搬运车的模型参数为：整车质量108kg，整车整备质量148kg，质心到前轴距离LF为0.69m，质心到后轴距离Lr为0.57m，轮距d为0.73m，轮胎滚动半径r为0.10m，横摆转动惯量Iz为70kg·m²，轮胎转动惯量I为0.42kg·m²。本发明测试使用的双扭线期望轨迹表示如下：

无人搬运车的初始位置为P0＝(0,10,0)^T，为模拟无人搬运车在现实工厂环境中质心变化的复杂工况，如图4所示，在期望轨迹中设计了上料点A、C以及卸料点B三个工位，从起点到A点为空载区，在A点上料后直到B点为载货区。同样，B点到C点为空载区，C点到B点为载货区，B点之后为空载区。仿真结果如图5和6所示：

为能准确分析本文提出的自适应全局渐进跟踪控制器的性能。取实际轨迹与期望轨迹的x轴、y轴位置偏差均值与绝对值差值，为评价两种轨迹跟踪控制器在无人搬运车变质心工况下轨迹跟踪控制效果的标准，数值越小控制效果越佳。自适应全局渐进跟踪控制器的x轴位置偏差的均值为MPC控制器的23.97％，自适应全局渐进跟踪控制器的y轴位置偏差的均值为MPC控制器的5.03％。因此，可以说明本文提出的自适应全局渐进跟踪控制器在无人搬运车变质心工况中对轨迹跟踪控制更加精准和平稳。

除此之外，本发明还提供了对自适应全局渐进轨迹跟踪控制器在变质心工况下的实际有效性的实验验证。具体设计了包含控制系统与硬件设备的无人搬运样机，搭载STM32F407单片机为下位机，通过无人搬运车驱动、转向、制动系统的电机驱动器控制电机的运转，实现智能运动控制；搭载嵌入式迷你工控机为上位机，运行本文设计的自适应全局渐进跟踪控制器，并通过串口通信实现对下位机的控制与信号反馈；搭载GPS/MINS组合导航系统，支持千寻差分、GPS双天线定向、内置双余度微惯导，能及时将实验所需的无人搬运车三维高精度位置、三轴姿态、速度、加速度、角速度等数据输入上位机中。上位机与固定基站搭载ROS智能系统实现对无人搬运车的决策与控制，两者通过WIFI进行通讯与数据传递。

实际实验中，在实验室内按期望轨迹在对应点设置上料区与下料区，模拟无人搬运车变质心工况，目标跟踪函数采用仿真所用的双扭线期望轨迹：

无人搬运车初始位置为P0＝(0,10,0)^T，通过车辆内搭载的组合惯性导航得出高精度位置坐标，并绘制出无人搬运车在二维平面位置中的双扭线跟踪路径图7。如图8所示，由组合惯性导航实时输出无人搬运车三维高精度位置坐标，并与仿真数据进行对比，结果显示本发明的自适应全局渐进跟踪控制器在实验中得出的X轴偏差均值为仿真数据得9.03％，y轴偏差值为仿真数据的7.69％。综上所述，实验数据误差处于合理误差范围之内，验证了本发明的自适应全局渐进跟踪控制器的真实性与可靠性。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。