技术领域
[0001] 本
发明涉及自动导航领域，具体涉及一种基于双PID控制的AGV小车的路径纠偏方法。
背景技术
[0002] 自动导引小车(AGV)系统目前主要应用于仓储物流运输，工业现场存储区与生产区域的柔性工位物料连接，早期的AGV只是应用于车间的杂货运输，目前AGV被广泛应用于各行各业，包括非工业生产环境也在积极使用AGV来替代减少人工工作，包括邮件邮报运输，办公室内部包裹、信息等的递送，医院送餐送洗衣物等方面。
[0003] AGV通常是采用磁导引和视觉导引等单一的可连续型纠偏的轨道引导方式，由于单一的惯性
导航系统会随着时间累积导航误差，因此惯性导航需要结合其他诸如
射频识别和二维码等室内
定位技术。目前的纠偏主流还是使用PID进行，参见图1可知，
现有技术中的PID
控制器(比例-积分-微分控制器)，由比例单元(P)、积分单元(I)和微分单元(D)组成。透过Kp，Ki和Kd三个参数的设定。PID控制器主要适用于基本上线性，且动态特性不随时间变化的系统。PID控制器的比例单元(P)、积分单元(I)和微分单元(D)分别对应目前误差、过去累计误差及未来误差，若是不知道受控系统的特性，一般认为PID控制器是最适用的控制器，借由调整PID控制器的三个参数，可以调整控制系统，设法满足设计需求，控制器的响应可以用控制器对误差的反应快慢、控制器过冲的程度及系统震荡的程度来表示。不过，使用PID控制器不一定保证可达到系统的最佳控制，也不保证系统
稳定性。
[0004] 公开号为CN105180930A的
专利提供了一种AGV惯性导航系统，它包括
陀螺仪、磁钉、
编码器、磁
传感器、
数据处理单元和运动控制单元，陀螺仪设置在AGV小车上，磁钉铺设在地面AGV航道上，
磁传感器设置在AGV小车车头底部的中线上，陀螺仪、磁传感器、编码器和运动控制单元分别与数据处理单元连接，编码器、数据处理单元和运动控制单元设置在AGV小车上的控制盒内；所述的数据处理单元包括依次连接的陀螺仪采集模
块、固定漂移处理模块、卡尔曼滤波处理模块、
角度获取模块、磁钉校准模块、航迹推算模块和PID调节器，编码器与轨迹推算单元连接，PID调节器与运控控制单元连接。该系统角度
精度为±0.1度，航迹推算精度为±5mm，导航精度为±10mm。该发明利用惯性传感器实现AGV纠偏功能，利用固定在地面上的磁钉实现AGV的粗略定位功能。该方法增加了外围辅助磁钉的配合定位作用，成本高，而且很难及逆行更改。当前AGV的纠偏控制往往只是仅仅单纯地停留于单个，单独的某个量，
位置、角度、速度、
加速度等单独的信息。但是这些变量和反馈量一般都是相互耦合，相互影响的。
发明内容
[0005] (一)要解决的技术问题
[0006] 针对现有纠偏控制技术的不足，为了使得控制更加的精准与平滑，本文提出一种基于双PID控制的AGV小车的路径纠偏方法，该方法在于融合了惯导原件和视觉传感器的数据，在此
基础之上进行对应新型自适应PID控制，自适应PID控制可以得到非常好的结果，纠偏过程进行快速、平滑，纠偏
算法抗干扰性强，鲁棒性好。
[0007] (二)技术方案
[0008] 为了达到上述目的，本发明提供了一种基于双PID控制的AGV小车的路径纠偏方法，所述纠偏方法包括直线行驶部分和转弯行驶部分的纠偏方法；所述小车配置有惯性导航元件和辅助定位设备，并限定其转弯行驶部分的行驶轨迹的拐角均为90度的折线，惯性导航元件能提供小车行驶时的实时行驶参数，通过结合运行时间能够推算出小车的实际坐标和
姿态角；由于惯性导航随时间累积会有一定的漂移，需要辅助定位设备校正，辅助定位设备间隔一定的时间提供校正参数,校正后能得到更加准确的所述小车的实际坐标和姿态角；
[0009] 在AGV小车进行行驶时，其双PID控制器总表达式为：
[0010]
[0011] 其中，u(t)为小车的横向纠偏控制量，Kpa、Kpd分别为两个PID控制器的PID参数，其中一个PID控制器是角度PID控制器，另外一个是位置PID控制器，且两个Kpa、Kpd作为因变量Kp分别与两个ea(t)、ed(t)作为自变量e之间存在Kp＝Ae2+B的函数关系，ea(t)为惯性导航元件或辅助定位设备提供的所述小车的姿态角与目标角度的差值，ed(t)为惯性导航元件或辅助定位设备提供的所述小车的实际坐标与目标坐标的差值，TaD和TdD为小车使用前调试得到的参数，α、β为每个小车使用前调试得到的系数，其根据小车直线行驶或转弯行驶的行进方式、行进速度取不同的值,对于AGV小车来说，在不同速度或不同位置偏移量情况下，设置的PID参数均不同，同理两个PID控制器前面的系数也需要做相应调整，α、β的调整原则为：速度越快调整的幅值越小，即系数α，β值越小；当AGV小车直线行驶时，按照以下方式进行纠偏：
[0012] (1)当ed(t)小于等于0.012m时，即位移偏差很小的情况下，需要两个PID控制器具有相同的控制作用，此时设置α＝β；
[0013] (2)当ed(t)大于0.012m时候，即位置偏移量较大情况下，位置PID控制器需要占据主要控制地位，角度PID控制器作为辅助进行纠偏，此时α<β；其中，α取值范围为[0,1]，β取值范围为[1,2]。
[0014] 进一步的，ea(t)具体为辅助定位设备提供的小车角度与目标角度的差值，ed(t)具体为辅助定位设备提供的小车坐标与目标坐标的差值。
[0015] 进一步的，在直线行驶时位置偏移量较大情况下，AGV小车使用前调试得到的系数α，β设置条件如下：
[0016] (1)在低速情况下，即速度小于等于0.6m/s时，设置α＝0.4，β＝1.6；
[0017] (2)在高速情况下，即速度大于等于0.6m/s时，设置α＝0.3，β＝1.7。
[0018] 进一步的，转弯行驶时：当AGV小车转弯行驶时，无法得知实时目标角度值，无法计算角度偏差，因而不存在角度PID故α＝0、β＝1；其位置偏差ed(t)认定方法为：当AGV进行直角的弧线转弯行驶时，AGV会在进入弯道时在
主控制器内部计算一条虚拟路径作为目标路径，这条虚拟路径是连接出弯点与入弯点的一段圆弧，在AGV进行圆弧转动的过程中，通过计算AGV小车的位置与圆弧圆心点的直线距离l减去圆弧的半径r得到AGV与圆弧的偏移量ed(t)，即ed(t)＝l-r，这个偏移量ed(t)为位置偏差；当AGV小车的位置位于圆弧内部时候，证明转弯角度过大，小车继续沿当前方向行驶，当AGV小车位于圆弧外部的时候，证明此时转弯角度过小，继续加大转向角进行打角转弯行驶。
[0019] 进一步的，其辅助定位设备是视觉传感器，视觉传感器的获取的信息为：二维码的ID号、二维码中心位置与视觉传感器视域中心的x，y轴偏移量中的
像素点个数(xNum,yNum)、二维码正方向与视觉传感器正方向的夹角、二维码角点检测的三个参考点在视域中的三个坐标(xA，yA)，(xB，yB)，(xC，yC)、二维码中心点偏移量(xD，yD),根据二维码的ID号可以通过
数据库查找的方式找到二维码对应的实际绝对坐标位置为(X，Y)；根据二维码角点检测的三个参考点在视域中的位置和二维码实际边长L可以计算出二维码在视觉传感器当中的映射信息，即多长距离对应多少个像素点；那么映射关系可以计算为如下关系：
[0020]
[0021] 其中k为映射值，根据映射关系可以推算出二维码中心位置和视觉传感器中心位置的相对位移xR和yR，其计算方式表述如下：
[0022]
[0023] 根据上式得出的相对位移xR和yR，综合实际绝对坐标位置(X，Y)可以得到视觉传感器的绝对位置，因为视觉传感器与
车身呈现一种固定的偏移关系，那么在AGV扫描到二维码的同时也可以通过计算得到车身的绝对坐标以及车身的姿态角，从而将其作为经过视觉传感器校正后的所述小车的实际坐标和姿态角。
[0024] (三)有益效果
[0025] 由上述技术方案可知，本发明提出了一种基于双PID控制的AGV小车的路径纠偏方法，其新提出的双PID控制器结构和新提出的视觉传感器定位方法都能够大大提升控制精度，并能够增加AGV运行稳定性和运行灵活性，增强AGV直线运行和曲线运动精度，加强AGV复杂能
力。且使得精度可以控制在5mm以内，灵活行可以准确
跟踪任意变换的运动轨迹，稳定性方面可以实现长时间稳定运行，抑制了
硬件传感器自身
缺陷，误差小。
附图说明
[0026] 通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，在附图中：
[0027] 图1为现有技术中的PID控制结构示意图；
[0028] 图2为本发明的AGV小车运行在布满二维码点阵的工业现场坐标图；
[0029] 图3为比例系数P的大小随着偏差e的大小呈二次曲线变化的函数图；
[0030] 图4为本发明的转弯行驶示意图。
具体实施方式
[0031] 为使本发明
实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0032] 本发明实施例提供了一种基于双PID控制的AGV小车的路径纠偏方法，以解决现有的单一基于惯导纠偏的AGV控制技术存在的误差累积问题，本发明的AGV系统使用惯性元件作为主要的导航定位设备，其中视觉传感器作为辅助定位设备。惯导元件可以采集载体的加速度、载体三轴受力情况、载体三轴的受磁情况。分别用ωx、ωy、ωz，ax、ay、az，mx、my、mz进行表示，其中，x、y、z分别表示载体
坐标系的x，y，z轴。
[0033] AGV运行在布满二维码点阵的工业现场，如图2所示，在a，b，c，d等类似这种横竖线交叉的地方我们铺设方向相同的二维码。二维码内容信息为相应的
站点号(ID号)。那么因此可以根据实际工业场景建立导航坐标系，坐标原点人为制定。AGV运行过程中，惯导元件时刻采集载体的转动和受力情况。由于工业场景的限制，AGV的运行轨迹智能是横平竖直的情况，二维码铺设在AGV的行驶路径之上。所以AGV在运行当中一定会在一段时间之内扫描到二维码，扫描到二维码之后根据视觉传感器的反馈信息得到AGV当前的绝对位置。根据AGV视觉传感器采集的绝对位置和惯导元件采集
位姿信息，进行相对应的纠偏操作。具体执行情况如下：
[0034] 惯性导航元件包括高精密陀螺仪、
加速度计和磁力计，主控制器每隔2ms进行一次
数据采集，每次数据采集完毕使用加速度计和磁力计对陀螺仪进行漂移校正，使用四阶龙哥库塔法对数据进行积分，得到当前的姿态信息四元数，将四元数进行欧拉角反解得到对应的姿态角，即
俯仰角、
横滚角、
偏航角。
[0035] 视觉传感器为一个高清摄像头，可以进行高速连续拍照。视觉传感器的获取的信息为：二维码的具体内容(ID号)，二维码中心位置与视觉传感器视域中心的x，y轴偏移量(像素点个数xNum,yNum)，二维码正方向与视觉传感器正方向的夹角，二维码角点检测的三个参考点在视域种的三个坐标(xA，yA，xB，yB，xC，yC)。通过视觉传感器可以获得二维码中心点偏移量xD，yD。根据二维码的ID号可以通过数据库查找的方式找到二维码对应的实际绝对坐标位置为X，Y。根据二维码角点检测参考点在视域中的位置和二维码实际边长(L)可以计算出二维码在视觉传感器当中的映射信息，即多长距离对应多少像素点。那么映射关系可以计算为如下关系：
[0036]
[0037] 根据映射关系可以推算出二维码中心位置和视觉传感器中心位置的相对位移xR和yR，其计算方式表述如下：
[0038]
[0039] 因此可以得到视觉传感器的绝对位置，因为视觉传感器与车身呈现一种固定的偏移关系，那么我们在AGV扫描到二维码的同时也可以得到车身的绝对坐标以及车身的姿态角。当然，基于诸多现有技术可知，当其它的定位设备也一样能够作为辅助定位设备来辅助惯性导航元件进行精确定位(如GPS等)，也不一定必须是上述提到的视觉传感器，只是上述提及的新改进的视觉传感器的定位方法更加适合于本
申请而已。
[0040] 获取到通过视觉传感器精确校正后的姿态角和车身坐标之后，便可以进行对应的纠偏操作，基于此，本发明提出了一种基于双PID控制的AGV小车的路径纠偏方法。且其并不完全依赖于上面提及的视觉传感器的定位方法，其只需要小车配置有惯性导航元件和辅助定位设备即可。
[0041] 所述纠偏方法包括直线行驶部分和转弯行驶部分的纠偏方法；所述小车配置有惯性导航元件和辅助定位设备，并限定其转弯行驶部分的行驶轨迹的拐角均为90度的折线，惯性导航元件能提供小车行驶时的实时行驶参数，通过结合运行时间能够推算出小车的实际坐标和姿态角；由于惯性导航随时间累积会有一定的漂移，需要辅助定位设备校正，辅助定位设备间隔一定的时间提供校正参数,校正后能得到更加准确的所述小车的实际坐标和姿态角；
[0042] 在AGV小车进行行驶时，其双PID控制器的总表达式为：
[0043]
[0044] 其中，u(t)为小车的横向纠偏控制量，Kpa、Kpd分别为两个PID控制器的PID参数，其中一个PID控制器是角度PID控制器，另外一个是位置PID控制器，且两个Kpa、Kpd作为因变量Kp分别与两个ea(t)、ed(t)作为自变量e之间存在Kp＝Ae2+B的函数关系，ea(t)为惯性导航元件或辅助定位设备提供的所述小车的姿态角与目标角度的差值，ed(t)为惯性导航元件或辅助定位设备提供的所述小车的实际坐标与目标坐标的差值，TaD和TdD为小车使用前调试得到的参数，α、β为每个小车使用前调试得到的系数，其根据小车直线行驶或转弯行驶的行进方式、行进速度取不同的值；
[0045] 两个Kpa、Kpd作为因变量Kp分别与两个ea(t)、ed(t)作为自变量e之间存在Kp＝Ae2+B的函数关系与图3的P和e的关系一样，这样设计是考虑到在本AGV项目当中，由于行走过程中需要
舵轮不断打角来调整车身位姿，其打角目标值是在不断地变化，对于一套固定的Kp、Ki、Kd参数是无法满足这样的需求。为此，此处使用一种基于二次P的PD控制算法，即参数Kp的大小随着偏差的大小呈二次曲线变化。Kp参数有一个基础值Kp0，在基础值的基础上，其大小随着偏差e呈二次曲线变化。即当偏差e较小的时候，Kp较小；当偏差e较大的时候，Kp较大，且随着e的增大Kp的增大趋势越显著。这样，当目标值和实际值有较大差距时，较大的Kp参数能够使得角度控制更为迅速，使车身及时达到目标角度，提升了响应的实时性、迅速性；当偏差较小时，较小的Kp会使得舵轮缓慢打角目标角度，保证了车身的稳定性。
[0046] 对于双舵轮系统AGV来说，不同速度下，不同位置偏移量情况下PID调整参数均不同，同理两个PID控制器前面的系数需要做相应调整，原则为：速度越快调整的幅值越小，即系数α，β值越小(注意：此处的参数调整只限于位置偏差过大的情况，即α≠β)。
[0047] 根据试验，本发明得出了较优的参数进行纠偏行驶，当进行直线行驶时：
[0048] (1)当位置偏移量小于0.012m时，即位移偏差很小的情况下，需要两个PID控制器具有相同的控制作用。即α＝β；
[0049] (2)当位置偏移量大于0.012m时候，即位置偏移量较大情况下，位置PID控制器需要占据主题地位，角度PID作为辅助。
[0050] 本机AGV实验情况下，α取值范围为[0,1]，β取值范围为[1,2]。
[0051] 目前，通过反复调试，得出较优的AGV系数值为：
[0052] (1)低速情况下(小于等于0.6m/s时)，α＝0.4，β＝1.6。
[0053] (2)高速情况下(大于0.6m/s时)，α＝0.3，β＝1.7。
[0054] 在进行转弯行驶时，本发明也考虑到转弯的实际情况对纠偏控制进行了优化，如图4所示，当AGV进入转弯时刻，由于AGV的运行方向不再是横平竖直的样子，所以AGV的角度偏差和位移偏差无法用直线行走的方式来进行纠正。位置的偏差通过一下方法得到：当AGV进行弧线转弯行驶时候，AGV会再进入弯道时候在主控制器内部计算一条虚拟路径，这条虚拟路径是连接出弯点与入弯点的一段圆弧，在AGV进行圆弧转动的过程中，可以通过计算AGV与圆弧圆心点的距离得到AGV与圆弧的偏移量，这个偏移量为位置偏移量；当此位移偏差位于圆弧内部时候，证明转弯角度过大，AGV继续沿当前方向行驶，当此位移偏差位于圆弧外部的时候，证明此时转弯角度过小，继续加大舵机打脚。
[0055] 在直角转向过程中，AGV小车行走的预期路线是一个四分之一圆弧，为使AGV能动态跟踪转向圆弧，此处将AGV当前位置到圆心o距离l和半径r的差值作为偏差e＝L-r，采用PD控制器跟踪圆弧曲线。同样，为使小车在偏差e较大时快速跟踪目标曲线，在偏差e较小时能保持稳定跟踪，P参数(即比例系数)根据二次曲线变化。
[0056] 可见，相对于现有技术，本发明新提出的双PID控制器结构和新提出的视觉传感器定位方法都能够大大提升控制精度，其中双PID控制器不仅考虑了对位置、角度分别进行控制，其也使用了基于二次函数的P参数的PID控制，结合使用能够增加AGV运行稳定性和运行灵活性，增强AGV直线运行和曲线运动精度，加强AGV复杂能力。此外，通过使用新提出的二维码校正的视觉传感器定位方法，使本发明的纠偏控制精度可以控制在5mm以内，灵活行可以准确跟踪任意变换的运动轨迹，稳定性方面可以实现长时间稳定运行，抑制了硬件传感器自身缺陷，且误差小。
[0057] 在本发明中，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。术语“多个”指两个或两个以上，除非另有明确的限定；虽然结合附图描述了本发明的实施方式，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种
修改和变型，这样的修改和变型均落入由所附
权利要求所限定的范围之内。
[0058] 以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本
说明书记载的范围。
