行进路径的纠偏方法与流程

1.本发明涉及自动行驶控制的技术领域，尤其涉及一种行进路径的纠偏方法。


背景技术：

2.自动行驶控制被应用在很多的行业和领域，例如，自动行驶控制技术被应用在agv(automated guided vehicle,自动导引运输车)上。
3.agv的控制系统离不开定位与路径纠偏的具体处理，现有技术中，通过在agv的目标行进路径上铺设多个二维码，通过二维码来指导agv按照目标行进路径来行驶，但是当agv行进在两个二维码之间的路径上时，若是缺乏定位和路径纠偏，可能会导致agv无法顺利到达下一个二维码所在的位置，为了能够让agv顺序按照目标行进路径来行驶，现有技术给agv装上gps模块，但是gps模块的精度不高，很难实现行驶路径上的定位。
4.现有技术中也采取了一些手段来提高agv在行驶路径上的定位精度，但是即便时定位精度提高，现有技术中缺乏具体的纠偏措施，因而也难以保证agv顺利按照目标行进路径来行驶。


技术实现要素：

5.为了解决现有技术中自动行驶控制技术难以确保行进物顺利按照目标行进路径行驶的技术问题，本发明提出了行进路径的纠偏方法。
6.本发明提出的行进路径的纠偏方法，包括：
7.根据行进物的坐标信息，以及速度曲线构建所述行进物的状态向量；
8.根据卡尔曼滤波算法得到所述行进物在行进过程中所述状态向量的值；
9.将所述状态向量的值传递给调节器进行偏差判断，若偏差需要纠正，则改变行进物左右轮的速度差，调节行进物的行进路径以纠正偏差。
10.进一步，所述坐标信息通过扫描所述行进物的目标行进路径上间隔设置的多个二维码中的至少一个二维码得到。
11.进一步，所述速度曲线为vk＝v
k-1
+e
10k-5
，k为时间，vk为k时刻行进物的速度，v
k-1
为k-1时刻行进物的速度。
12.进一步，所述卡尔曼滤波为扩展卡尔曼滤波或无迹卡尔曼滤波。
13.进一步，根据卡尔曼滤波算法得到所述行进物在行进过程中所述状态向量的值具体包括：
14.根据所述状态向量建立行进物的状态预测方程、误差协方差预测方程；
15.根据所述行进物的传感器装置构建观测方程；
16.根据状态预测方程、误差协方差预测方程以及观测方程计算卡尔曼增益；
17.根据卡尔曼增益对状态预测方程以及误差协方差预测方程进行更新，并得到的定位预测值与观测方程得到的定位观测值的权重；
18.根据定位预测值、观测预测值以及它们的权重计算得到定位过程中的状态信息。
19.进一步，当速度曲线为非线性曲线时，在计算卡尔曼增益之前，先计算所述状态预测方程以及观测方程的雅可比矩阵；
20.在得到所述卡尔曼增益之后，基于所述状态预测方程的雅可比矩阵对所述状态预测方程进行更新，以及基于所述观测方程的雅可比矩阵对所述误差协方差矩阵进行更新。
21.进一步，所述状态预测方程为所述表示k时刻预测的状态向量的值，fk是状态转移矩阵，bk为输入控制量，uk为外部控制量。
22.进一步，所述传感器装置包括加速度传感器，所述观测方程为zk＝hkxk+rk。hk为观测模型矩阵，xk为k时刻观测的状态向量的值，rk为k时刻的观测噪声协方差矩阵。
23.进一步，所述卡尔曼增益通过公式kk＝p
k-1hkt
(hkp
k-1hkt
+rk)-1
计算得到，所述kk为k时刻的卡尔曼增益，p
k-1
为k-1时刻的误差协方差矩阵，hk为k时刻的观测模型矩阵，rk为k时刻的观测噪声协方差矩阵。
24.进一步，所述调节器为pid调节器或pi调节器。
25.进一步，所述行进物为agv。
26.本发明通过使用卡尔曼滤波算法，实现行进物行驶在两个二维码之间的过程也能够进行精确定位。在此基础上，再使用经过卡尔曼滤波算法处理后的行进物的位置与速度等数据结合比例微分等控制方法实现路径纠偏的功能，以使得行进物可以顺利依照目标行进路径行驶。
附图说明
27.下面结合实施例和附图对本发明进行详细说明，其中：
28.图1是本发明的一实施例的主要流程图。
29.图2是本发明的一实施例的卡尔曼滤波算法流程图。
具体实施方式
30.为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
31.由此，本说明书中所指出的一个特征将用于说明本发明的一个实施方式的其中一个特征，而不是暗示本发明的每个实施方式必须具有所说明的特征。此外，应当注意的是本说明书描述了许多特征。尽管某些特征可以组合在一起以示出可能的系统设计，但是这些特征也可用于其他的未明确说明的组合。由此，除非另有说明，所说明的组合并非旨在限制。
32.如图1所示，本发明提出的行进路径的纠偏方法，根据行进物的坐标信息，以及速度曲线构建行进物的状态向量，接着根据卡尔曼滤波算法得到行进物在行进过程中状态向量的值，再将状态向量的值传递给调节器进行偏差判断，若偏差需要纠正，则改变行进物左右轮的速度差，调节行进物的行进路径以纠正偏差。本发明通过卡尔曼滤波算法可以对行进物在行进过程中(行驶过程中)进行精确的定位，接着通过调节器对行进物进行纠偏，从而使得行进物可以顺利按照目标行进路径行进，避免在行进过程中累积较大的偏差(误差)。
33.在一个实施例中，本发明的行进物的坐标信息具体是通过扫描目标行进路径上间隔设置的多个二维码中的至少一个二维码得到。行进物可以每到一个二维码的位置，扫描一次二维码以获得当前准确的坐标信息，在本发明的提供的算法精度足够满足要求的前提下，也可以只扫描初始启动位置的二维码，或者扫描部分二维码来获取坐标信息，在定位和纠偏均满足要求时，行进过程中的二维码也可以不扫描。
34.在一个实施例中，本发明的速度曲线为vk＝v
k-1
+e
10k-5
，k为时间，vk为k时刻行进物的速度，v
k-1
为k-1时刻行进物的速度。本发明基于该速度曲线，可以满足启动时速度足够低，避免初始阶段控制过程中行进物的过冲大而产生的行进物减速或停止的震动幅度，使得行进物在起步阶段可以平稳起步，同时该速度曲线使得行进物的后期速度较快，能够满足行进物的速度要求。本发明的速度曲线并不局限于所列举的这一个速度曲线，本领域内技术人员也可以采用其他的速度曲线来对行进物的速度进行控制。
35.在一个实施例中，本发明的卡尔曼滤波可以为扩展卡尔曼滤波，在另一个实施例中，本发明的卡尔曼滤波还可以是无迹卡尔曼滤波。无迹卡尔曼滤波的精度会略高，相当于二阶泰勒展开。由于扩展卡尔曼滤波算法以及无迹卡尔曼滤波算法均是现有的算法，因而本发明仅针对扩展卡尔曼滤波算法进行详细说明，本领域内的普通技术人员基于扩展卡尔曼滤波算法的具体应用，也能够清楚无迹卡尔曼滤波算法的具体应用。
36.如图2所示，当本发明采用的卡尔曼滤波为扩展卡尔曼滤波算法时，根据卡尔曼滤波算法得到行进物在行进过程中状态向量的值具体包括以下步骤。
37.根据状态向量建立行进物的状态预测方程、误差协方差预测方程；
38.根据行进物的传感器装置构建观测方程；
39.根据状态预测方程、误差协方差预测方程以及观测方程计算卡尔曼增益；
40.根据卡尔曼增益对状态预测方程以及误差协方差预测方程进行更新，并得到的定位预测值与观测方程得到的定位观测值的权重；
41.根据定位预测值、观测预测值以及它们的权重计算得到定位过程中的状态信息。
42.本发明通过上述步骤来实现行进物在行进过程中的精确定位。
43.在一个实施例中，由于行进物的速度通常都不是线性变化的，因而当速度曲线为非线性曲线时，在计算卡尔曼增益之前，还需要先计算状态预测方程以及观测方程的雅可比矩阵，在得到卡尔曼增益之后，基于状态预测方程的雅可比矩阵对状态预测方程进行更新，以及基于观测方程的雅可比矩阵对误差协方差矩阵进行更新，从而不断优化状态预测方程和误差协方差矩阵，实现更精准的定位。
44.在一个实施例中，本发明的调节器采用的是pid调节器或者pi调节器，扩展卡尔曼滤波所有计算结束后得到的新的状态量的值，即最新的速度与位置信息将传递给pid调节器等，通过pid调节器等进行纠偏控制，从而及时处理行进过程中产生的误差，使得行进物可以顺利沿着目标行进路径行驶。
45.下面以agv作为行进物的一个具体实施例来对本发明的技术方案进行详细说明。
46.先通过状态向量建立状态预测方程和误差协方差预测方程，通过坐标信息以及速度曲线就可以计算得到agv的状态信息,即状态向量包含两个信息，分别为位置(即坐标信息)与速度p为位置，v为速度。
47.根据agv的运动模型可以写出它的状态预测方程根据agv的运动模型可以写出它的状态预测方程表示k时刻预测的状态向量的值，fk是状态转移矩阵，它连接上一时刻与当前时刻的状态，bk为输入控制量，uk为外部控制量。
48.根据位置与速度的公式pk＝p
k-1
+v
k-1
*δt与vk＝v
k-1
+e
10k-5
写出矩阵形式那么就得到了状态转移矩阵fk，即状态转移矩阵为公式中k-1时刻的状态向量前面的那个矩阵。表示k-1时刻预测的状态向量的值，bk表示输入控制量，uk表示外部控制量。
49.将位置与速度的公式进行扩展得到就能得到bk与uk以及完整的状态预测方程，即bk为公式中a前面的矩阵，而uk作为k时刻的外部控制量，具体产生的是agv的加速度a，因而采用加速度a进行替代，而agv的加速度是可以通过传感器或者是速度等参数计算得到。
50.误差协方差预测方程表示为其中fk表示新的状态转移矩阵，f
kt
表示fk的转置矩阵，qk表示过程噪声协方差矩阵，它体现了对运动模型估计的不确定程度，是一个已知量。
51.除了上述两个方程，还需要建立观测方程。观测方程具体是通过agv的传感器装置所检测的值来建立，在一个实施例中，传感器装置具体为加速度传感器。
52.根据加速度传感器建立的观测方程为zk＝hkxk+rk。hk为观测模型矩阵，它表示观测量与状态之间的关系，xk为k时刻观测的状态向量的值，rk为k时刻的观测噪声协方差矩阵，这个矩阵和qk有相同的性质，它表示对agv参数观测的不确定程度。
53.由于agv的速度曲线为非线性曲线，即实际agv在运动过程中速度变化是非线性的，因而状态预测方程可以更改为即进一步可以抽象为
54.使用泰勒级数将非线性关系近似线性化，在一个实施例中，可以将t(x)取n项当中的第一项，本发明使用第一项就可以使得计算得到的t(x)与vk＝v
k-1
+e
10k-5
比较准确的近似。其中x为变量，即为本发明的速度，c为使得t(x)可导的值。通过将非线性化的速度函数进行求导，从而使得非线性化的关系线性化。
55.使用公式得到新的状态转移矩阵，f1为p
k-1
+δt*f(v
k-1
)，f2为f(v
k-1
)，x1为pk，x2为vk,f(v
k-1
)为之前得到的第一项泰勒级数，使用相同方法得到hk。
56.接着通过公式kk＝p
k-1hkt
(hkp
k-1hkt
+rk)-1
计算卡尔曼增益，其中kk为k时刻的卡尔
曼增益，p
k-1
为k-1时刻的误差协方差矩阵，hk为k时刻的观测模型矩阵，rk为k时刻的观测噪声协方差矩阵。
57.通过计算卡尔曼增益来决定预测数据(预测的速度和位置)与观测数据(通过传感器装置等得到的速度和位置)的权重，为之后agv数据的更新提供权重值，如agv的状态＝预测数据
×
卡尔曼增益+(1-卡尔曼增益)
×
观测数据。
58.同时，还需要进行数据更新。基于状态预测方程的雅可比矩阵对状态预测方程进行更新的具体公式为基于观测方程的雅可比矩阵对误差协方差矩阵进行更新的具体公式为pk＝p
k-1-k
khk
p
k-1
。kk为k时刻的卡尔曼增益，zk为观测方程，hk为观测模型矩阵。
59.实现扩展卡尔曼滤波后，在agv行驶过程中可以对其状态数据进行滤波得到较为准确的状态数据，对于行驶路径上agv的位置信息可以增加pi控制或者是pid控制的方法同时实现路径纠偏的功能，即采用pid调节器进行pi控制或pid控制。
60.pi控制的公式为δu(k)＝k
p
(e(k)-e(k-1))+kie(k)，δu(k)为计算出的k时刻的增量，k
p
、ki为对应的系数，具体可以通过测试确定，e(k)为当前时刻的误差量，e(k-1)为上一时刻的误差量。
61.通过得到的最新的agv的位置、速度使用pid控制方式，具体公式为：
[0062][0063]
δu(k)为计算出的k时刻的增量，k
p
、ki、kd为对应的系数，具体可以通过测试确定，e(k)为当前时刻的误差量，e(k-1)为上一时刻的误差量。
[0064]
由于间隔时间δt是预设的已知量，通过上述的卡尔曼滤波算法得到的k时刻的状态量，如x方向上的偏移量p
x
，或者是y方向上的偏移量py，或者是k时刻的速度等，均可以是pid控制的误差，即e(k),将对应时刻的误差代入到上述pid控制公式，就能够得到k时刻的增量δu(k)，而在一个较优实施例中，期望的增量为0，假设计算得到的增量不为0，那么调整时就朝着将计算得到的增量δu(k)往0的方向调。
[0065]
例如，通过惯性传感器可以得到agv车身的倾斜角度θ，当前agv车头方向(实际行驶路径)与理论行驶路径的夹角即倾斜角θ，agv的x坐标方向变化距离p
x
＝v*cosθ*δt，agv的y坐标方向变化距离py＝v*sinθ*δt。
[0066]
通过卡尔曼滤波计算得到的p
x
、py的值传递给调节器进行偏差判断(误差判断)。若偏差需要纠正，则改变agv左右驱动轮速度产生差速，就可以调节agv行驶路径，通过将最新得到位置信息、速度信息传递给调节器进行偏差判断，就可以对agv的行进路径进行优化调整。
[0067]
通过上述技术方案对卡尔曼增益系数的不断更新，将精度更高的位置、速度信息传递给调节器以实现更稳定的控制。在agv实际投入生产过程中，根据实验结果，本发明可以避免因行驶路径出现差错造成停车等现象。
[0068]
本发明使用扩展卡尔曼滤波算法后，使得通过本发明得到的行进过程中所获得的位置信息、速度信息的精度可以在agv行驶路径上进行精确定位。在此基础上，还可以结合比例微分等控制方法，在agv行驶过程中进行速度调整或纠偏等操作。本发明基于扩展卡尔曼滤波算法结合比例微分等控制方法的路径纠偏精度比模糊控制或比例微分控制方法的
纠偏精度要高。
[0069]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。