协同式停车机器人的控制方法研究

高铭洋

（纽约大学 工学院，纽约 11201）

0 引言

到大自然中动物协同工作的启发，人们发现多机器人协同工作可以扩展机器人的功能，并使机器人更有序、更高效、更灵活完成更复杂的工作[1,2]。Zhao等人采用多移动机器人实现了大型构件协同加工，有效的提高了生产效率[3,4]。Krid等人设计的多机器人协同搬运系统能搬运超长、超重物体[5]。研究表明，多机器人协同搬运不仅可以增加负载量，还能使搬运操作更加灵活[6]。

停车机器人将自动导航车（AGV）技术与汽车搬运技术相结合，能够灵活、智能、高效的按照预定轨迹将汽车安全停放至停车位[7]。而双协同式停车机器人将多机器人协同技术和停车机器人技术相结合，可以利用其灵活、可重构的特点，通过独立-协同工作模式的切换，更加灵活的完成停车任务。

高精度协同控制和协同搬运的稳定性等问题一直是双协同式机器人的技术难题。常见的协同控制方法如领航-跟随法，其主要思路为令一领航机器人负责整个机器人编队的导航。队内的其他机器人根据领航机的行动方式进行相应的行动。Koshi等人采用“一轮一机”的方式使用领航-跟随法对小汽车进行协同搬运[8]。Endo等人令跟随机器人通过与领航机器人和被搬运物体间的作用力来判断领航机轨迹，使得跟随机无需与领航机进行通信，大大降低了通讯成本[9]。吴垠等人采用不依赖搬运器运动模型的模糊控制方法，分别控制领航机的轨迹和跟随机与领航机的位姿差[10]。Wu等人使用载着摄像机的牵引车进行定位，并对重型车辆的搬运机器人进行领航[11]。Sieber将人也加入机器人协同搬运的系统中，将人视为领航者，而机器人视为跟随者。这样，人可以对处于搬运系统中的机器人进行实时引导[12]。其他文献主要应用分布式控制[13]。Eberhard等采取分布式模型预测控制，利用预测误差来规划合适的轨迹，实现了平滑的运输性能[14,15]。He等在分布式控制中通过降低控制算法所需数据减小了通讯成本[16]，以及在非建模环境下采用分布式控制模式，同时结合阻抗控制等多种柔性控制器来进行动态避障，并增加计算速度[17]。还有文献采用集中式架构。Ebel等设计了三层集中控制架构，用于规划并跟踪每个机器人的运动轨迹[18]。Hichri等通过规划并赋予不同的机器人在时间和空间上互不冲突的任务来达成多机器人的协同控制[19]。还有一些协同搬运方法采用同步反馈而非与外界通信的方式。Obregon等人通过指定被搬运物体的运动，来让每个机器人根据物体预设位置进行特定运动，达到协同搬运的目的[20]。Wang等设置领航者来引导物体运动，而其他每个搬运机器人通过感知物体的运动进行协同搬运[21,22]。Loh等令每个机器人靠传感器感知其他机器人及被搬运物体的状态变化并作出反馈[23]。Gronemeyer等人采用人工力场算法避免多机器人协同时的碰撞问题[24]。从以上研究结果可以看出，协同时机器人间的通讯成本、协同运动时队形的鲁棒性以及每个机器人的控制精度是研究协同搬运最需要解决的问题。

针对以上问题，本文将提出一种基于领航跟随法和模型预测控制算法的控制策略（LFMPC，Lead follow -model predictive control）。领航机通过机器人的运动模型预测机器人在特定时间内的运动方式及所处位置，并求解出一个最优控制使得领航机沿预设路径行进。而跟随机实时通信获得领航机位置及其即将进行的运动，并用上述模型预测的方法与领航机保持特定位姿，实现跟随领航机的运动。由于本文的控制对象为双机器人协同式停车机器人，故优先采用领航-跟随的协同控制策略。且由于领航-跟随法与领航机的行动轨迹无关，因此可以使领航机引领下的搬运系统在平面内更自由的进行运动[10]。另外，模型预测控制具有鲁棒性强、通信频率要求不高，灵活利用状态与输入的约束，从而可以更好地处理有约束的控制问题[25]。同时由于模型预测控制对模型精度要求不高，因而可以弥补非线性模型线性化时造成的模型失真。Eberhard等人采用模型预测控制算法，仅对传统的“前轮转向+后轮驱动”式搬运机器人进行协同控制仿真研究，而本文控制的对象为运动更加灵活的协同式停车机器人，各使用四个全向轮驱动。而且，本文对所提出的LFMPC控制策略完成了实验验证。

本文结构主要分为六个部分：第二节介绍双协同式停车机器人的工作模式和性能指标；第三节将对LFMPC控制算法进行详细论述；第四节将LFMPC算法应用在停车入库场景中，并对此进行仿真分析；第五节为实验验证；最后为本文结论。

1 停车机器人工作模式及性能指标

本文所研究的协同式停车机器人基于深圳闪移开发的Combo停车机器人，其产品图片如图1所示，搬运方式如图2所示，其主要性能指标如表1所示。协同式停车机器人由两套可独立运行的夹臂式搬运器组成。非工作状态下，两个机器人在夹臂收缩状态下合并为一体。其工作模式为：合并状态的机器人独立运行至待停车辆的底部，然后两机器人分离并分别运动至与小轿车前/后轮轴处；双机器人夹臂同步展开并夹起车轮，然后协同运动至车位处，之后双机夹臂同步松开完成自动停车；停车任务完成后，双机器人再次合并，并运行至下一待停车辆或存放点。与其他传统的停车机器人比较，这类双协同式停车机器人无需载车板、转台等辅助设施，可直接应用于现有停车场。采用双机协同工作模式，不仅减小了停车机器人的体积，从而降低了占地面积，也使其运动、转运更加灵活。

图1 双协同式停车机器人图片

图2 双协同停车机器人搬运方式

表1 停车机器人协同搬运时的性能指标

2 领航跟随-模型预测控制

2.1 单搬运器的模型预测控制方法

由于每个搬运器均由四个全向轮驱动，因此通过调节机器人的四个全向轮转速及方向可任意调节机器人整体的运动速度大小v、速度方向偏角δ以及自转角速度ω。因而可以进行任意方向的平移运动和以任意点为圆心的阿克曼转向运动。其关系为：

xr、yr为机器人阿克曼转向圆心距机器人中心处的x与y坐标差，ωr为机器人阿克曼转向角速度。

由于可以通过指定机器人的控制量来任意控制机器人的速度大小、方向和自转角速度，则不妨将机器人速度大小方向和自转速度作为系统输入量，位姿为状态量，即：

状态量与输入量的关系为：

对于某一时刻的任意控制量或输入量s，均有s=s*+Δs，s*为在该时刻的理想值，而s为实际值，Δs为实际值与理想值的偏移量。由于状态量和输入量的实际值和理想值均应满足式(6)，因此：

将机器人在一段时间内控制量保持恒定的运动状态称为定常运动，那么上述平移和阿克曼转向两种运动均为定常运动。对于任意运动轨迹均可拆分或近似拆分为若干段定常运动（平移或转向）。对于每一段定常运动，机器人的输入量的理想值保持不变。

模型预测控制的目标是通过控制Δu(k)的值能使控制间隔时间tc=n*t里偏差y(k)尽量小，同时调整量Δu(k)尽量小，故定义损失函数为：

Q和R分别为y(k)和Δu(k)的权重。将式(9)代入式(10)得：

可以使用二次规划方法求解Δu(k)的最优值，然后将u(k)=u(k)*+Δu(k)作为ρ(k)的采样时刻后tc时间内的输入，这样可以实现对任意标准运动的控制。

2.2 双搬运器协同控制方法

基于单搬运器的模型预测控制方法，本节论述跟随机如何与领航机进行协同搬运。双搬运器协同转弯方式为双搬运器同时以其中垂线上一点为圆心进行相同半径的阿克曼转向，如图3所示，因此其输入关系为：

图3 双搬运器协同阿克曼转向

运动中需要保持双搬运器的偏航角相同，同时跟随机保持在领航机的正后方特定距离。即：

上述为跟随机的理想状态量ρ2*和控制量u2*与领航机的关系。跟随机将通过与领航机通讯实时获得领航机当前的状态量ρ1*和u1*控制量，并依据式(12)～式(15)计算出跟随机当前的参考状态量ρ2*(k)和控制量u2*(k)。然后采用上小节中的模型预测控制方式，根据跟随机该时刻的实际状态量ρ2(k)计算出跟随机在接下来tc时间里的最优控制u2(k)。

2.3 双搬运器协同控制的仿真研究

本小节将利用Simulink环境对LFMPC控制方法进行仿真。仿真场景为搬运器即将进行右转弯，转弯半径为2m，标准速度为ρ*=2m/s。在初始状态下领航机状态偏离量分别为x=-1m，y=-0.05m，φ=0.12rad；跟随机状态偏离量为x=0.75m，y=0.1m，φ=-0.08rad。控制器参数为：

图4、图5分别为仿真过程中领航机和跟随机的位姿偏差，表2为在不同初始状态偏差下经过转弯运动前后的位姿偏差。由仿真结果可知，使用LFMPC控制能够在一个转弯运动中将偏差降低至原先的以内，因此LFMPC控制能够较好的完成定常运动的轨迹跟踪与队形保持任务，对外界环境及其他干扰等造成的运动偏差具有较强的鲁棒性。

图4 领航机位姿与预设轨迹的偏差

图5 跟随机位姿与预设轨迹的偏差

表2 停车机器人协同搬运时的性能指标

3 基于LFMPC 的停车方案和仿真

协同式停车机器人能够准确地将小汽车停入特定车位。本节将具体讨论两种不同停车入库场景下的停车方案，然后采用Simulink环境对此场景进行仿真，并对比在使用LFMPC控制策略与直接使用开环控制下的停车结果偏差。

3.1 仿真情景设定

本节将讨论的车库为较常见的T字型车位，对于这种车位主要有两种停车方案。预设情景如图6所示。设定车的预设初始位置的中轴线距离车位所在路边1.2m，汽车中心距离车库中垂线2m，车库中垂线与道路中垂线交点距离预设泊车地点2.5m。停车方案一的具体步骤为：先前进3.5m，然后以1.5m为半径向侧后方进行90o阿克曼转向运动，最后后退1m，第一种停车方案的具体步骤如图7所示。停车方案二的具体步骤为先以4.83m为半径向车库侧前方进行45o的阿克曼转向运动，再以同样半径向侧后方进行45o阿克曼转向的倒车运动，最后倒车0.5m，第二种停车方案的具体步骤如图8所示。接下来将对这两种情景分别进行仿真，设定领航机初始偏差为0.5m，初始y偏差为0.5m，姿态角偏差为0.2rad，跟随机初始x偏差为-0.2m，y偏差为0.2m，姿态角偏差为-0.15rad。

图6 情景预设

图7 停车方案一

图8 停车方案二

3.2 仿真实验结果

图9、图10分别为停车方案一使用LFMPC控制和完全开环控制的仿真结果对比；图11、图12为停车方案二在使用LFMPC控制和完全开环控制的仿真结果对比。采用停车方案一，用时约8s；采用停车方案二，则用时约12s。仿真结果表明无论采用哪种方案，使用LFMPC控制均可在整个过程的前2s便将状态偏差缩减至原先偏差的5%以内，而在整个过程结束时的状态偏差基本归零；而全程采用开环控制时状态偏差或维持原先水平，或有所增加。因此LFMPC控制在外在干扰所造成的偏差较大的情况下依然能使得停车机器人较好的保持队形，并将待停车辆准确停在指定位置。

图9 停车方案一开环偏差

图10 停车方案一闭环偏差

图11 停车方案二开环偏差

图12 停车方案二闭环偏差

LFMPC控制在外在干扰所造成的偏差较大的情况下依然能使得停车机器人较好的保持队形，并将待停车辆准确停在指定位置。

4 实验

本节将对自主研发的协同停车机器人使用LFMPC控制策略开展实验验证，并选择深圳市某地下车库作为实验场地，选用普通小轿车作为搬运对象。该车库路面大体平整，但局部凸凹不平，非常贴近于常见的小区地下车库的地面。协同停车机器人停车路线如图13所示，并采用上节中所述的停车方案一进行停车入库。实验结果如表3所示。

图13 协同机器人停车路线

表3 实验结果

在入库实验过程中，汽车无明显晃动，且基本沿规划轨迹运动。实验结果表明：LFMPC控制能够使机器人较好的适应崎岖路面与初始位置偏差等干扰，其位置偏差≤150mm，姿态偏差小于5o。

从实验结果可知：真实实验与仿真实验结果还存在一定误差。误差主要来源于两个方面：第一、来源于搬运机通过导航得到的状态量和实际状态量之间的偏差。LFMPC方法采用领航机和跟随机的位姿作为状态量进行控制和反馈，因而导航的精准程度将直接影响LFMPC方法反馈的精度，从而降低系统的鲁棒性。第二、来自于在仿真实验时未考虑实际负载时领航机与跟随机同时夹抱小轿车轮时互相之间的约束力，这一约束力会在一定程度上阻碍领航机与跟随机自由调整位姿。实验结果表明，导航误差和夹抱车轮时约束力所造成的偏差不会过多的影响LFMPC方法的鲁棒性。

5 结语

1）提出和验证了领航跟随-模型预测控制方法，适用于协同式停车机器人。

2）LFMPC控制方法对外在干扰具有较强的鲁棒性，能让双协同式停车机器人在保持搬运队形。

3）双协同式机器人进行入库时采用LFMPC方法能大大降低由初始位姿偏差、路面崎岖等因素干扰所造成的偏差，实验所得的位置偏差≤50mm，姿态偏差小于5o。