王玉　沈丹峰　王荣军　李耀杰　李靖宇

摘要： 针对自动落布车在纺织车间里的换道轨迹规划和跟踪控制问题，文章提出基于B样条的快速高效且避障的换道拐弯轨迹规划，并设计模型预测控制的落布车轨迹跟踪控制器。以自主设计的双舵轮自动落布车为研究对象，利用B样条曲线规划纺织车间里落布车换道轨迹，分析车身结构并建立其运动学模型，求得落布车在纺织车间里的定位算法及考虑车体的运动性能和环境约束，使用模型预测控制（MPC）减少复杂环境中的跟踪误差和随机干扰。最后，通过Matlab软件进行仿真，验证了MPC在给定的可行距离和角度偏差范围内能够稳定地跟踪参考轨迹，并与传统PID控制进行对比，最终证明采用MPC的轨迹跟踪算法具有稳定良好的跟踪性能。

关键词： 自动落布车;B样条;模型预测;PID控制;轨迹跟踪;Matlab仿真

中图分类号： TS103.7;TP242文献标志码： A文章编号： 10017003（2022）04006509

引用页码： 041110DOI： 10.3969/j.issn.1001-7003.2022.04.010（篇序）

目前大部分织造车间都采用了手工落布的方式，由于人工落布速度慢，消耗大量人力，影响纺织车间的作业效率，因此迫切需要在落布过程中以机器代替人，采用自动落布的方法。自动落布车主要应用于织造车间，将卷布成品自动脱下，输送至验布车间，减少了对劳动力的需求，从而降低运营成本。落布车的自主运动主要是指根据预定的目标轨迹及落布车的状态和纺织车间里的环境信息，自动控制落布车到达指定的目标点。此外，轨迹跟踪控制要求被控对象在给定的时间内到达指定的目标点，其目的是确保车辆遵循预定路径，选择最合适的速度并最大限度地减少跟踪误差。由于其独特的布局和生产方式，自动落布车在织造车间的轨迹跟踪控制方法亟待探讨。

常用的轨迹跟踪控制算法大多采用比例积分微分（Proportional-Integral-Derivative，PID）控制[1-2]，滑膜控制[3-4]，或神经网络控制[5-6]，但这些方法高度依赖于参数和环境，因此适应性不强。Chein等[7]提出了一种轨迹跟踪算法，用于汽车类机器人输出速度和转向命令，从而使跟踪误差和控制器工作量最小化;杨勇生等[8]提出的基于Lyapunov第二方法的自动导引车轨迹跟踪控制器，采用真实的样车参数模拟直线和圆弧轨迹;Geng等[9]提出了一种新的容错模型预测控制（Model Predictive Control，MPC）算法，用于自主车辆的鲁棒路径跟踪控制，通过构建目标函数，考虑前轮偏角和轮胎侧偏的动态约束，设计了车辆横向运动控制的线性时变模型预测控制算法;Liu等[10]提出將自动导向车（Automated Guided Vehicle，AGV）的角速度和纵向速度的误差校正作为耦合估计误差，将耦合估计误差和改进的纯跟踪算法相结合作为AGV小车的横向控制，同时采用PID控制作为纵向控制，进一步减小误差干扰。MPC具有明确的基本思想和强大的发展潜力，它可以与各种系统模型、控制理论和优化算法相结合，形成各种控制算法[11-13]。此外，落布车是由机械和电气部件组成的，也受到物理限制，因此，必须考虑落布车的非线性特性及其与车间环境的相互作用[14-15]。

本文针对自主设计的双舵轮自动落布车，考虑纺织车间里复杂的道路环境，对比不同阶次B样条曲线规划的轨迹及曲率，最终选择7次B样条在织机之间生成平滑的换道轨迹[16-17]，并分析了落布车的双舵轮车体结构，建立其运动模型及在车间里的定位算法，然后结合控制系统设计基于模型预测的自动落布车纠偏控制器，控制器根据落布车轨迹与目标轨迹之间的位置偏差和角度偏差，在每个采样时刻进行滚动优化，并实时调整落布车航向角和速度。最后，采用Matlab软件仿真落布车纠偏控制过程，与传统PID控制算法进行比较，验证模型预测控制的实时性和鲁棒性。

1自动落布车在织机间的换道路径规划

由于纺织车间里独特的织机布局和生产模式，导致落布车在织机间的换道轨迹需要更精确地规划，而B样条[18-19]曲线具有局部支撑性、凸包性和变差缩减性等优点，可以调整控制点灵活地配置曲线几何形状，适用于落布车在狭窄的织机间换道的局部轨迹规划。在二维空间中给定n+1个控制点P，P，…，P和一个节点向量U=[u，u，…，u]，这些控制点和节点向量用于定义样条曲线的极限范围，所以K阶B样条曲线的定义为：

B-spline曲线对规划的轨迹影响很大，因为曲线的调整点越少，计算时间越快，但选择得太少可能会偏离实际路线，也会导致落布车与织机相互碰撞的路线，因此分别用3、5、7次B样条曲线生成的落布车换道轨迹曲线如图1所示。

对B样条曲线进行不同阶次特征分析，令换道过程中两台织机之间的距离为落布车车道宽d，本文中取值为1 m，P点为车道的中点，即为0.5d，纵向位移X设定为在落布车恒定车速换道时间下的纵向行驶距离。可得基于3次、5次和7次B样条曲线规划的变道轨迹曲线及对应的曲线曲率，如图2所示。

从图2可以看出，虽然3次或5次B样条曲线可以在指定的位置之间生成一条可行的平滑路径，但在初始和终止位置处生成的轨迹的曲率值不为0，使得自动落布车在实际行驶中难以遵循规划的预期轨迹，且5次B样条曲率的最大值达到4.5×10。而7次B样条生成的轨迹更接近于车道的中点，基于7次B样条曲线在起始点和终止点的曲率值能完全满足为0的条件，虽然相同条件下7次B样条规划的路径最大曲率比3次B样条略大，但最大值2.8×10仍远小于4.5×10，即能充分满足落布车在两台织机间的换道动作。因此，采用7次B样条曲线来规划落布车在纺织车间的换道轨迹。

由于B样条曲线的形状由选取的坐标点决定，因此将基于7次B样条曲线的理想换道轨迹规划问题转化为数学优化问题求9个坐标点。通过优化求解得到落布车对应换道需求下的最优换道轨迹，这是解决更贴近纺织车间里实际换道情况问题的关键，同时，也为实现目标点的动态搜索提供了可能。由式（3）可知，自动落布车换道路径曲率的连续性要求B样条曲线是二阶连续的，且规划曲线两端的节点具有重复度K，得到B样条曲线的节点向量U=[0，0，0，0，0，0，0，0.25，05，0.75，1，1，1，1，1，1，1]。

因此，本文选择图1（c）所示的7次B样条曲线规划自动落布车换道路径，如图3所示。P=（P，P）（i=0，1，…，8）为7次B样条曲线的控制点，d为两台织机之间的宽度，L≥0，…，L≥0，这些均为决定控制点的待优化坐标参数。

图3建立地面坐标系OXY，则B样条曲线的控制点P=（P，P）（i=0，1，…，8）可表示为：

2自动落布车运动学模型分析

自动落布车的运动系统是由两个舵轮和两个万向轮组成，前后两个舵轮分别安装在落布车前进方向轴线上。落布车主要控制舵轮在车间内实现直线和转弯换道运动，使其能按照预设的路线行进，且主要是通过动态调整落布车的两个舵轮来实现运动。因此，在运动学建模分析不影响的情况下，将自动落布车简化为前后两个驱动轮的运动模型[20]。假设落布车车体为刚性，移动速度较低，水平运动时两个驱动轮受力相同，则忽略轮子宽度保证驱动轮同心转动。

自动落布车的运动学模型如图4所示，每台舵轮驱动带动一个驱动轮，通过两个舵轮的舵角不断调节，落布车可以完成直线与曲线路径的轨迹跟踪任务。

图4建立平面直角坐标系XOY，两个驱动轮与地面的接触点分别为O、O;f表示前轮，r表示后轮;两轮到质心点C的距离分别为a和b;设落布车车身坐标系为xCy，前后轮的行进速度分别为V、V，行进速度与车身坐标系X轴的夹角分别为α、αr;R表示转弯半径，φ表示落布车航向角。

2.1自动落布车定位算法

如图4所示，设自动落布车的车体几何中心为前后两个舵轮中点O与O_r的连线中点，在落布车均匀负载的情况下，认为落布车的车体质心即为几何中心，表示为C=（X，Y）。α、α分別表示为两个舵轮的转角角度;V、V分别表示为两个舵轮的线速度;O点表示为落布车转弯时的瞬心;R、R分别表示两个舵轮转弯时的轨迹半径。因此，两个驱动轮的位置坐标则可表示为：

考虑落布车的刚体运动特性，落布车绕瞬心点O运动时，两个驱动轮的中心点及落布车质心点C运动时的角速度是一致的，因此结合驱动轮运动时的速度和航向角，根据正弦定理求得落布车的角速度为：

3自动落布车纠偏控制

由于自动落布车系统是非线性高耦合系统，且在纺织车间狭小的过道环境约束下及落布车车体的机动性能限制下，很难通过直接求解运动学方程获得精确解析，因此，考虑在线获取带约束优化问题的数值解，简化求解过程，满足自动落布车实时控制的系统要求。

3.1自动落布车轨迹跟踪控制误差模型

假设利用B样条规划的换道轨迹是自动落布车的目标轨迹，并且给出了落布车在任意时刻的状态量和控制量，那么就可以通过落布车实际与目标轨迹之间的偏差来进行跟踪控制。为使落布车能跟踪规划好的轨迹运行，可建立目标轨迹状态空间表达式：

模型预测控制需要在每个采样时间求解一个优化问题，因此需要预先设置性能指标函数。通过控制优化量，可以获得最大值或最小值。性能指标函数通常采用二次函数，正常的控制目标是使输出预测值尽可能接近目标值。本文涉及的轨迹跟踪控制属于输出最优控制问题，其性能函数表示为：

式（24）显示的是无约束优化，通过加权矩阵Q和R可以在一定程度上抑制预测输出和控制输入的波动。然而，控制变量不能被精确地约束。在实际过程中，施加控制量的幅度或其增量过大，会对系统造成重大影响，甚至影响系统的稳定性。所以，对于式（22）需要考虑控制变量的边界约束，即：

4仿真试验与结果分析

为了验证模型预测控制器用于自动落布车的有效性，本文采用自主设计的自动落布车进行跟踪试验，使用PLC搭建的控制系统，如图5所示。在Matlab中建立车辆的运动学模型，并在Simulink中建立控制器。模型控制器是基于当前位置模拟多个输出，优化器找到最优的。在获得最佳控制序列后，控制器将第一步应用于落布车。然后，它使用式（22）计算下一时刻的位置，式（12）将其用作新优化的起点，它在这个过程中一直重复直到跟踪控制完成。

自动落布车在纺织车间里的运动轨迹分为直线和B样条规划的两种路径，其中，落布车在车间里进行换道轨迹时的稳定跟踪能力是轨迹跟踪算法鲁棒性的重要体现。为了验证在模型预测下自动落布车的轨迹跟踪效果，本文选取的目标轨迹为直线路径和B样条规划的曲线换道路径，并将MPC控制器和传统PID控制器分别应用于自动落布车，比较轨迹跟踪效果。

4.1落布车在直线轨迹下的仿真对比试验

图6和图7分别展示了落布车在直线下，用MPC控制器和传统PID控制器进行轨迹跟踪的对比效果。从图6可以看出，落布车在运行时就能快速并稳定地进行直线目标轨迹的跟踪，但PID控制器到达稳定的时间较长且运行过程中有很大的抖动。由图7可以知道，随着被控落布车速度和航向的实时调整，距离误差和角度误差迅速减小，在6 s时实现精确跟踪后误差保持在0;传统PID控制器则需要15 s才能趋于稳定，稳态误差也相比MPC更大。综合可以看出，MPC的轨迹跟踪要更快更稳定。

4.2落布车在换道轨迹下的仿真对比试验

图8和图9分别展示了在规划的B样条换道曲线下，用MPC和PID控制器进行轨迹跟踪的仿真对比效果。如图8所示，在MPC控制器下落布车能很快地开始跟踪目标轨迹，即使在换道拐弯的位置有轻微的一点偏差，也不影响后续目标的稳定轨迹跟踪，完成落布车在纺织车间里的转弯;相对于PID控制器来说，自动落布车在换道轨迹跟踪时就出现了较大幅度的超调，会使自动落布车在拐弯时与织机相撞，影响車间工作效率。如图9所示，落布车运动一段时间后位置与角度偏差在10 s左右收敛到0，基于PID控制器的落布车需要

在20 s才能达到稳定状态，且稳态误差相较于MPC较大。因此在用B样条规划的轨迹下，MPC控制器相比于传统的PID控制器能够满足落布车稳定的跟踪目标轨迹，实现在纺织车间里的拐弯换道，且具有较高的实时性和鲁棒性。

5结语

针对纺织车间里自动落布车的跟踪控制问题，本文先利用7次B样条曲线规划落布车在纺织车间里的平滑拐弯换道轨迹，再建立落布车双舵轮的转向运动模型。同时针对自动落布车设计了基于模型预测控制的轨迹跟踪算法，选取小车的线速度和角速度作为控制变量，利用预期与实际路径的误差加入边界约束，使其能快速稳定地完成轨迹跟踪任务，提高工作效率。最后通过Matlab仿真验证MPC轨迹跟踪算法的有效性与可行性，以直线和B样条规划的转弯换道路径为目标轨迹，与传统PID控制器进行对比，结果表明模型预测控制算法更具有优势，使落布车的轨迹跟踪能快速趋于稳定且跟踪精度更好。

《丝绸》官网下载中国知网下载

参考文献：

[1]刘金琨. 先进PID控制Matlab仿真[M]. 北京： 电子工业出版社， 2016.LIU Jinkun. Advanced PID Control Matlab Simulation[M]. Beijing： Electronic Industry Press， 2016.

[2]ROVIRA-MAS F， ZHANG Q. Fuzzy logic control of an electrohydraulic valve for auto-steering off-road vehicles[J]. Proceedings of the Institution of Mechanical， 2008， 222（6）： 917-934.

[3]XIA Y Q， LU K F， ZHU Z， et al. Adaptive-back-stepping sliding mode attitude control of missile systems[J]. International Journal of Robust & Nonlinear Control， 2013， 23（15）： 1699-1717.

[4]姜立标， 杨杰. 基于滑模控制的自动泊车系统路径跟踪研究[J]. 农业机械学报， 2019， 50（2）： 356-364.JIANG Libiao， YANG Jie. Path tracking of automatic parking system based on sliding mode control[J]. Transactions of the Chinese Society for Agricultural Machinery， 2019， 50（2）： 356-364.

[5]范其明， 吕书豪. 移动机器人的自适应神经网络滑模控制[J]. 控制工程， 2017， 24（7）： 1409-1414.FAN Qiming， L Shuhao. Adaptive neural network sliding mode control of mobile robots[J]. Control Engineering of China， 2017， 24（7）： 1409-1414.

[6]JAZAR R. Mathematical theory of autodriver for autonomous vehicles[J]. Journal of Vibration & Control， 2010， 16（2）： 253-279.

[7]CHEIN F A， SCAGLIA G. Trajectory tracking controller design for unmanned vehicles： A new methodology[J]. Journal of Field Robotics， 2014， 31（6）： 861-887.

[8]杨勇生， 赵宏， 姚海庆. 基于Lyapunov第二方法的自动导引车轨迹跟踪控制器设计与仿真[J]. 上海海事大学学报， 2019， 40（2）： 73-77.YANG Yongsheng， ZHAO Hong， YAO Haiqing. Design and simulation of trajectory tracking controller for automated guided vehicles based on Lyapunov’s second method[J]. Journal of Shanghai Maritime University， 2019， 40（2）： 73-77.

[9]GENG K K， CHULIN N A， WANG Z W. Fault-tolerant model predictive control algorithm for path tracking of autonomous vehicle[J]. Sensors， 2020， 20（15）： 4245.

[10]LIU Y Q， JIN G H， LIU X， etal. An improved hybrid error control path tracking intelligent algorithm for omnidirectional AGV on ROS[J]. International Journal of Computer Applications in Technology， 2020， 64（2）： 115-125.

[11]张家旭， 杨雄， 施正堂， 等. 汽车紧急换道避障的路径规划与跟踪控制[J]. 华南理工大学学报（自然科学版）， 2020， 48（9）： 86-93.ZHANG Jiaxu， YANG Xiong， SHI Zhengtang， et al. Path planning and tracking control for emergency lane change and obstacle avoidance of vehicles[J]. Journal of South China University of Technology （Natural Science Edition）， 2020， 48（9）： 86-93.

[12]丁森， 瞿文燕. 双驱动AGV转弯分析与优化[J]. 计算机光盘软件与应用， 2013， 16（4）： 203-204.DING Sen， QU Wenyan. Analysis and optimization of dual-drive AGV turning[J]. Computer CD Software and Applications， 2013， 16（4）： 203-204.

[13]朱曉祥， 陈乃军， 殷邦革. AGV路径纠偏控制器设计与分析[J]. 科技创新与应用， 2017（12）： 62-63.ZHU Xiaoxiang， CHEN Naijun， YIN Bangge. Design and analysis of AGV path correction controller[J]. Technology Innovation and Application， 2017（12）： 62-63.

[14]王殿君， 关似玉， 陈亚， 等. 双驱双向AGV机器人运动学分析及仿真[J]. 制造业自动化， 2016， 38（3）： 42-46.WANG Dianjun， GUAN Siyu， CHEN Ya， et al. Kinematic analysis and simulation of double-drive and double-direction AGV robot[J]. Manufacturing Automation， 2016， 38（3）： 42-46.

[15]刘金珠， 李颖新， 罗丹， 等. 基于智能技术打造无人化数字棉纺工厂[J]. 纺织导报， 2014（1）： 47-51.LIU Jinzhu， LI Yingxin， LUO Dan， et al. Building unmanned digital cotton spinning mill based on intelligent technology[J]. China Textile Leader， 2014（1）： 47-51.

[16]施法中. 计算机辅助几何设计与非均匀有理B样条[M]. 北京： 高等教育出版社， 2001.SHI Fazhong. Computer-Aided Geometric Design and Non-Uniform Rational B-Spline[M]. Beijing： Higher Education Press， 2001.

[17]张永华， 杜煜， 潘峰， 等. 基于三次B样条曲线拟合的智能车轨迹跟踪算法[J]. 计算机应用， 2018， 38（6）： 1562-1567.ZHANG Yonghua， DU Yu， PAN Feng， et al. Intelligent vehicle path tracking algorithm based on cubic B-spline curve fitting[J]. Journal of Computer Applications， 2018， 38（6）： 1562-1567.

[18]陈成， 何玉庆， 卜春光， 等. 基于四阶贝塞尔曲线的无人车可行轨迹规划[J]. 自动化学报， 2015， 41（3）： 486-496.CHEN Cheng， HE Yuqing， BU Chunguang， et al. Feasible trajectory generation for autonomous vehicles based on quartic Bezier curve[J]. Acta Automatica Sinica， 2015， 41（3）： 486-496.

[19]AHMED F， DEB K. Multi-objective path planning using spline representation[C]//IEEE International Conference on Robotics and Biomimetics. New York： Institute of Electricle and Electronics Engineers， 2011.

[20]齐嘉晖， 吴耀华， 汪威. 双舵轮自动导引车轨迹追踪控制算法[J]. 科学技术与工程， 2020， 20（20）： 8252-8260.QI Jiahui， WU Yaohua， WANG Wei. Trajectory tracking control algorithm for double-steering automated guided vehicle[J]. Science Technology and Engineering， 2020， 20（20）： 8252-8260.

Automatic plaiting vehicle trajectory tracking control algorithmWANG Yu SHEN Danfeng WANG Rongjun LI Yaojie LI Jingyu（1.School of Mechanical and Electrical Engineering， Xi’an Polytechnic University， Xi’an 710048， China;

2.Qingdao Haijia Machinery Co.， Ltd.， Qingdao 266000， China）

Abstract： China’s textile industry already accounts for more than 50% of the world， and the textile and clothing market is growing at an accelerated pace， playing an important role in market forecasting， expanding exports， increasing employment and farmers’ income and promoting urbanisation， but in China， the textile industry is a highly labour-intensive and externally dependent industry. Intelligent manufacturing is one of the important means for the transformation and modernization of China’s textile industry and for the long-term dominance of China’s textile industry in the world. After 2018， the textile machinery industry has been further promoting industrial upgrading， achieving certain development in many aspects such as intelligence by means of new efficiency， low energy consumption， flexibility， automation and digitalization. The majority of existing workshop logistics systems， consisting of conveyor belts， human vehicles， etc.， are not seamlessly integrated between upstream and downstream production processes， resulting in low logistics and transport efficiency， which directly affects the efficiency of the entire production system.

At present most of weaving workshops use manual plaiting， which requires a lot of physical labour. Due to the low efficiency of manual plaiting， it is easy to miss some plaiting tasks， and there is an urgent need to replace people with machines in the process of plaiting and adopt an automated method of plaiting. An automatic plaiting vehicle applicable for the water jet loom production workshop is designed combined with the practical requirements， the hardware and software systems of the vehicle are completed， and the trajectory tracking of the vehicle in the process of movement is studied in depth to achieve precise navigation and movement control of the vehicle during its work， conveying its full cloth rolls to the storage position. To address the problem of lane changing trajectory planning and tracking control of plaiting vehicles in textile workshops， a fast and efficient B-sample-based trajectory planning with obstacle avoidance is proposed， and a model predictive control trajectory tracking controller for plaiting vehicles is designed. The B-sample curve is used to plan the lane changing trajectory of the plaiting vehicle in the textile workshop. The structure of the body is analyzed and its kinematic model is established to find out the positioning algorithm of the plaiting vehicle in the textile workshop and to reduce the tracking error and random disturbance in the complex environment by using MPC considering the motion performance of the body and environmental constraints. Finally， the simulation is carried out by Matlab software to verify that MPC is able to track the reference trajectory stably within the given feasible distance and angle deviation， and the trajectory tracking algorithm is compared with the traditional PID control. It proves that the trajectory tracking algorithm using model predictive control has stable and good tracking performance.

The automatic plaiting vehicle has been designed and developed for the transport of full fabric rolls on water jet weaving machines， but there are still many shortcomings as far as we can see. The investigated trajectory tracking of the plaiting vehicle is done in a known environment， but special situations cannot be excluded in the textile workshop， and further verification and improvement of the algorithms on the automatic vehicle are needed to optimize and improve the control system functions of the vehicle.

Key words： automatic plaiting vehicle; B-spline; model prediction; PID control; trajectory tracking; Matlab simulation