一种基于改进无迹卡尔曼滤波的AGV定位方法与流程

一种基于改进无迹卡尔曼滤波的agv定位方法
技术领域
1.本发明属于agv定位领域，涉及一种定位方法，具体是关于automated guided vehicle(agv)定位的方法，该方法用激光雷达将固定区域内反光板的坐标信息传递给系统，根据无迹卡尔曼滤波算法，利用反光板的坐标信息来定位车辆所在的位置。


背景技术：

2.现有大部分的agv是用惯性导航去定位，然后通过卡尔曼滤波或扩展卡尔曼滤波进行滤波。但是，惯性导航容易在复杂的工作环境中积累误差，导致定位精度降低，而且以上两种算法在对agv坐标进行估计时，能力有限，误差达不到指定的范围之内。
3.为了减小误差，有人提出利用无迹卡尔曼滤波去估计agv的坐标位置，例如中国专利申请号201910326780，发明名称为一种磁导航agv的高精度定位方法的公开说明书。扩展卡尔曼滤波方法在估计agv坐标时，需要对非线性系统方程进行线性化，会使估计精度进行降低，只保留一阶精度。而无迹卡尔曼滤波，则利用无迹变换选取sigma点，去均值近似和协方差近似非线性系统，尽可能的减少系统状态方程精度的丢失，可以达到二阶计算精度，但是当只有n个状态信息时，一般的无迹卡尔曼滤波进行无迹变换时需要选取2n个或者2n+1个sigma点，这无疑是提高了计算量，降低了估计速度，这不符合实际应用的期望。


技术实现要素：

4.为了克服已有技术的不足，本发明提供了一种基于改进无迹卡尔曼滤波的agv定位方法，改善现有agv定位方法，过程简单，可靠性强，稳定性好，可以应用于各种定位领域。
5.本发明解决其技术问题所采用的技术方案是：
6.一种基于改进无迹卡尔曼滤波的agv定位方法，包括以下步骤：
7.步骤1：采用电机增量编码器和角度编码器检测agv的前进速度vk和转向速度ωk；
8.步骤2：接收激光雷达扫描到的信息，生成反光板坐标序列；
9.步骤3：建立agv的运动模型用以估计agv位姿，xk是agv在k时刻世界坐标系下的位姿(xk,yk,θk)，θk是k时刻车身的偏航角，vk是k时刻车身的前进速度，ωk是k时刻车身转向角速度，wk是均值为0、协方差为q的系统噪声；
[0010][0011]
其中，t为每一时刻的时间，xk为每一时刻的状态，下标k为第k时刻；
[0012]
步骤4：将激光雷达扫描到的反光板坐标转换到世界坐标系下，建立如下的观测方程：
[0013]
[0014]
是激光雷达坐标系下的坐标，是激光雷达在车身坐标系下的坐标，lθ是雷达安装偏转角度，是在k时刻车身的坐标，其中xk代表车身在k时刻的横坐标，yk代表在k时刻的纵坐标，lmxk是反光板的世界坐标，vk是均值为0、协方差为r的测量噪声；
[0015]
步骤5：将基于步骤3所得的运动模型与基于步骤4所得的观测模型进行改进无迹卡尔曼滤波，实现agv定位。
[0016]
进一步，所述步骤5的过程如下：
[0017]
初始化状态向量和估计协方差：
[0018][0019][0020]
其中，为0时刻的估计值，等于0时刻状态值的期望，e(x)是对x求期望，即向量中的每一个元素乘其对应的概率然后累加得到的均值；
[0021]
定义sigma点对应的权值m(i)及相关向量
[0022][0023][0024]
其中，其中m(i)是i处sigma点的权重值，上标i表示权值的个数，而相关变量是由权重值进行计算得到的向量，其上标表示由第几个权重值计算而来，下标表示是向量序号；
[0025]
对于j＝2,
…
,n，按照下式(7)递推获得n维向量其中0
j-1
是包含j-1个0的列向量；
[0026][0027]
生成sigma点：
[0028]
[0029]
其中sigma点是由k-1时刻的状态估计值与状态估计值的协方差的开平方和向量的乘积之和计算的来的，为下一步进行推算所生成的数据，下标i代表第i时刻。
[0030]
根据系统运动学模型，将sigma点转化为
[0031][0032]
利用m(i)加权平均构成和其中是k时刻的估计值，是k时刻的协方差估计值：
[0033][0034][0035]
再次选取sigma点，得到量测的估计值：
[0036][0037]
其中是k是的sigma点，利用观测方程(2)和生成的点中的前两维，即进行量测更新；将车身坐标和激光雷达所观测到的反光板坐标(lmxk,lmyk)，代入(2)中，得到反光板在世界坐标系下的坐标的sigma点到反光板在世界坐标系下的坐标的sigma点
[0038]
所述步骤5中，基于量测的状态更新：
[0039]
利用m(i)加权平均获取k时刻的量测预测值，其中是反光板的坐标估计值：
[0040][0041]
估计量测预测的协方差p
yy
下标代表量测，考虑噪声rk：
[0042][0043]
估计与之间的协方差p
xy
，下标代表状态与观测，是k时刻的sigma点，是k时刻的估计值，是反光板坐标的sigma点，是反光板坐标的估计值：
[0044]
[0045]
用下面的方程进行状态量测更新：
[0046][0047][0048][0049]
其中kk是k时刻的卡尔曼增益，代表估计值量测预测的逆，代表k时刻真实的协方差，是k时刻估计的协方差，是k时刻的真实状态值，是k时刻估计的状态值，lmxk是真实的反光板坐标，反光板的坐标估计值。
[0050]
本发明的有益效果主要表现在：与传统技术中的定位方法不同，由于惯性导航容易受到外界影响和干扰，故采用激光雷达传感器采集数据，对agv的定位结果进行校正，计算过程中要处理非线性过程，相较于扩展卡尔曼滤波等其他算法，无迹卡尔曼滤波的精度更高，而且本发明的改进无迹卡尔曼滤波算法，与一般无迹卡尔曼滤波的精度类似，但计算量更小，计算速度快，可以有效减少量测误差，相较于其他定位方法，本发明采用改进的无迹卡尔曼滤波，过程简单，可靠性强，稳定性好，可以应用于各种定位领域。
附图说明
[0051]
图1是本发明的硬件结构示意图；
[0052]
图2是车辆的运动学模型；
[0053]
图3是本发明的整体流程图；
[0054]
图4是反光板对激光雷达的相对坐标，转化到世界坐标示意图。
具体实施方式
[0055]
下面结合附图对本发明作进一步描述。
[0056]
参照图1～图4，一种基于改进无迹卡尔曼滤波的agv定位方法，包括以下步骤：
[0057]
步骤1：采用电机增量编码器和角度编码器检测agv的前进速度vk和转向速度ωk；
[0058]
步骤2：接收激光雷达扫描到的信息，生成反光板坐标序列；
[0059]
步骤3：建立agv的运动模型用以估计agv位姿，xk是agv在k时刻世界坐标系下的位姿(xk,yk,θk)，θk是k时刻车身的偏航角，vk是k时刻车身的前进速度，ωk是k时刻车身转向角速度，wk是均值为0、协方差为q的系统噪声；
[0060][0061]
其中，t为每一时刻的时间，xk为每一时刻的状态，下标k为第k时刻；
[0062]
步骤4：将激光雷达扫描到的反光板坐标转换到世界坐标系下，建立如下的观测方程：
[0063]
[0064]
是激光雷达坐标系下的坐标，是激光雷达在车身坐标系下的坐标，lθ是雷达安装偏转角度，是在k时刻车身的坐标，其中xk代表车身在k时刻的横坐标，yk代表在k时刻的纵坐标，lmxk是反光板的世界坐标，vk是均值为0、协方差为r的测量噪声；
[0065]
步骤5：将基于步骤3所得的运动模型与基于步骤4所得的观测模型进行改进无迹卡尔曼滤波，实现agv定位，过程如下：
[0066]
初始化状态向量和估计协方差：
[0067][0068][0069]
其中，为0时刻的估计值，等于0时刻状态值的期望，e(x)是对x求期望，即向量中的每一个元素乘其对应的概率然后累加得到的均值；
[0070]
定义sigma点对应的权值m(i)及相关向量
[0071][0072][0073]
其中，其中m(i)是i处sigma点的权重值，上标i表示权值的个数，而相关变量是由权重值进行计算得到的向量，其上标表示由第几个权重值计算而来，下标表示是向量序号；对于j＝2,
…
,n，按照下式(7)递推获得n维向量其中0
j-1
是包含j-1个0的列向量；
[0074][0075]
生成sigma点：
[0076][0077]
其中sigma点是由k-1时刻的状态估计值与状态估计值的协方差的开平方和向量的乘积之和计算的来的，为下一步进行推算所生成的数据，下标i代表第i
时刻。
[0078]
根据系统运动学模型，将sigma点转化为其中为k时刻的sigma点：
[0079][0080]
利用m(i)加权平均构成和
[0081][0082][0083]
利用m(i)加权平均构成和其中是k时刻的估计值，是k时刻的协方差估计值：
[0084][0085]
其中是k是的sigma点，利用观测方程(2)和生成的点中的前两维，即进行量测更新；将车身坐标和激光雷达所观测到的反光板坐标(lmxk,lmyk)，代入(2)中，得到反光板在世界坐标系下的坐标的sigma点
[0086][0087]
利用m(i)加权平均获取k时刻的量测预测值，其中是反光板的坐标估计值：
[0088][0089]
估计量测预测的协方差p
yy
下标代表量测，考虑噪声rk：
[0090][0091]
估计与之间的协方差p
xy
，下标代表状态与观测，是k时刻的sigma点，是k时刻的估计值，是反光板坐标的sigma点，是反光板坐标的估计值：
[0092][0093]
用下面的方程进行状态量测更新：
[0094][0095]
[0096][0097]
其中kk是k时刻的卡尔曼增益，代表估计值量测预测的逆，代表k时刻真实的协方差，是k时刻估计的协方差，是k时刻的真实状态值，是k时刻估计的状态值，lmxk是真实的反光板坐标，反光板的坐标估计值。
[0098]
在图4中，最左方xoy坐标系为世界坐标系。(x,y)为世界坐标系下车身的坐标，θ是车身偏航角。(lmx,lmy)为世界坐标系下反光板的坐标。
[0099]
中间的lxo'ly坐标系为车身坐标(x,y)为原点o'，建立的坐标系。(lx,ly)为车身坐标系下雷达的坐标，lθ是雷达在车身上的安装偏差角度是固定的，可以通过标定得到。
[0100]
最右方lmxo”lmy的坐标系为雷达坐标系。系统的观测的输入为激光雷达扫描到反，(lmx,lmy)为激光雷达坐标系下反光板的坐标，(lmx,lmy)为世界坐标系下反光板的坐标。
[0101]
本设计的目的是将反光板在雷达的相对坐标转换到世界坐标系下。车身坐标(x,y)是在世界坐标系下标定，不用改变。
[0102]
雷达坐标(lx,ly)是基于车身坐标系确定的，且车身坐标系与世界坐标系之间有偏航角θ，故将雷达坐标向世界坐标系下投影要经过坐标变换，横坐标的投影是lx
·
cosθ-ly
·
sinθ,纵坐标的投影是lx
·
sinθ+ly
·
cosθ。
[0103]
雷达坐标系下的反光板关于雷达的相对坐标(lmx,lmy)是基于雷达坐标的，且雷达坐标系相对于世界坐标系之间有偏航角θ和安装偏差角lθ，故其横坐标的投影是lmx
·
cos(θ+lθ)-lmy
·
sin(θ+lθ),纵坐标的投影是lmx
·
sin(θ+lθ)+lmy
·
cos(θ+lθ)。
[0104]
最终得出坐标转移公式：
[0105][0106]
本实施例中，采用无迹卡尔曼滤波方法对激光雷达采集数据进行估计，进而用来修正agv定位的方法，本发明的整体流程如图3所示，实施过程如下：
[0107]
步骤一：在工作环境中安装反光板，预设反光板在工作环境中的坐标，称为世界坐标。
[0108]
步骤二：安装在agv上的激光雷达开始工作，对工作环境进行激光扫描，并接收反光板反射的激光信号，生成反光板对于激光传感器的相对坐标。
[0109]
步骤三：从agv的电机增量编码器和角度编码器，收集车身的前进速度和转向速度。
[0110]
步骤四：建立以车身为原点的坐标系和以激光雷达为原点的坐标系，将雷达扫描到的反光板的关于雷达的相对坐标转化到世界坐标系下，生成一组测量的反光板世界坐标，具体参照图4。
[0111][0112]
步骤五：通过(3)和(4)，进行初始化获得初始状态和协方差，利用(5)式生成sigma点的权值，然后用过(6)和(7)去生成向量γ，将(3)、(4)、(7)得到的信息代入到(8)中生成
sigma点，初始化完成。然后将(8)中生成的sigma点代入到(9)中，完成从k-1时刻到k时刻的时间更新。
[0113]
此刻已经完成时间更新过程，需要为量测更新作准备，将(9)中的生成的sigma点和(5)中生成的权值带入到(10)中，生成量测更新的后验估计，然后将状态的后验估计代入到(12)中得到k时刻的状态的sigma点，利用k时刻状态的sigma点代入到(13)中生成量测k时刻的量测的sigma点，然后将(13)中的量测sigma点通过(14)加权完成量测更新。
[0114]
最后通过(15)、(16)计算卡尔曼增益(17)，然后通过(18)和(19)完成状态估计。
[0115]
本说明书的实施例所述的内容仅仅是对发明构思的实现形式的列举，仅作说明用途。本发明的保护范围不应当被视为仅限于本实施例所陈述的具体形式，本发明的保护范围也及于本领域的普通技术人员根据本发明构思所能想到的等同技术手段。