一种户外无人清洁车的定位和路径跟踪方法

1.本发明涉及清洁车定位与路径跟踪技术领域，具体涉及一种户外无人清洁车的定位和路径跟踪方法。


背景技术：

2.随着城市化进程的加速和人口增长，城市垃圾和污染问题日益突出。传统的清洁车需要人工驾驶，操作复杂，效率低下，重复性劳动属性强。因此，无人清洁车的出现成为了解决城市清洁问题的重要途径。
3.清洁车一般以较低速度进行清洁。由于需要对清洁区的高覆盖率，对清洁车的定位和车辆行驶路径有一定的要求，否则清洁区的卫生状况无法得到保障，清洁区内可能由于运动受限出现大块不能覆盖的区域，不能到达清洁的要求。
4.现有技术中，长时间在rtk精度下降的过程中单纯依靠imu导致累计误差越来越大、清洁车进行路径跟踪时陷入过度按序列追踪和跳转过多点的问题。


技术实现要素：

5.发明目的：针对现有技术中存在的问题，本发明提供一种户外无人清洁车的定位和路径跟踪方法，使用rtk和10轴imu进行融合定位，并结合粒子滤波器增加定位的抗干扰性和准确性，在结合已知的高精度坐标点之间的通信来矫正累计误差，使用弓形的全覆盖路径算法进行全局的路径规划，再利用改进的纯路径跟踪实现自主驾驶，该控制系统基于阿克曼模型，具有良好的转弯性能和稳定性，能够避免车辆陷入追逐无法满足运动学的目标点，也避免为了路径符合运动学而跳点过多的情况。
6.技术方案：本发明提供了一种户外无人清洁车的定位和路径跟踪方法，包括如下步骤：
7.步骤1：使用rtk模块和10轴imu模块获取清洁车的初始位置与初始姿态，结合粒子滤波器进行融合定位，同时对于rtk精度大于一定值，利用修正方法矫正累计误差；
8.步骤2：将步骤1最终获取的清洁车的最终wgs84坐标转换为高斯二维坐标系中的坐标；
9.步骤3：获取已经规划好的清洁车路径，利用启发式算法更新步骤2获取到的位置坐标与规划好的路径规划列表中各点的距离，按照预设的路径进行前进控制。
10.进一步地，所述步骤1中使用rtk和10轴imu获取清洁车的初始位置与初始姿态，结合粒子滤波器进行融合定位的具体操作为：
11.(1)首先接收rtk模块和imu模块的数据，然后定义描述rtk模块和imu模块的测量误差和状态误差的统计特性的协方差矩阵：
[0012][0013]
其中，p
t
是时刻t的协方差矩阵，n是粒子的数量，是第i个粒子在时刻t的状态，hatx
t
是时刻t的状态估计值，即加权平均：
[0014][0015]
其中是第i个粒子在时刻t的权重；
[0016]
(2)根据清洁车的初始位置和姿态初始化粒子集合，生成一些随机粒子来表示车辆的状态，初始化粒子集合的公式如下：
[0017]
x
0[i]
～p(x0),i＝1,...,n
[0018]
其中，x
0[i]
表示第i个粒子的初始状态，p(x0)表示初始状态的概率分布；
[0019]
(3)根据车辆的运动模型，对粒子进行状态预测，得到下一时刻的粒子状态：
[0020][0021]
其中，x
k[i]
表示第i个粒子在时刻k的状态，表示状态转移概率分布；
[0022]
(4)根据rtk模块和imu模块的测量结果，更新每个粒子的权重，将粒子权重进行归一化，使得所有粒子权重之和为1：
[0023][0024][0025]
权重表示该粒子状态与测量结果的匹配程度，表示第i个粒子在k时刻的权重，zk表示时刻k的测量结果，表示测量概率分布；表示第i个粒子在时刻k的归一化权重；
[0026]
(5)根据粒子权重，对粒子进行重采样，得到新的粒子集合，重采样时，权重较大的粒子被选中的概率较大，权重较小的粒子被选中的概率较小：
[0027]
其中，δ(
·
)表示dirac delta函数；
[0028]
(6)根据重采样后的粒子集合，计算车辆的位置和姿态估计值，更新车辆状态估计：
[0029][0030]
其中，表示时刻k的车辆状态估计值。
[0031]
进一步地，所述步骤1中利用修正方法矫正累计误差具体操作为：
[0032]
当rtk模块信号不好的时候，接收器锁定了低于5颗的卫星，定位精度大于0.5米，平面和高程误差在1-3米，实时定位模块与已知高精度基站做通信，在接收到固定基站的信号后，利用修正技术：基准站根据自己的已知位置和卫星的位置，计算出每颗卫星到自己的真实距离，然后与测得的清洁车与基准站的伪距比较，得出伪距改正数，所述伪距为信号传播时间乘以光速；基准站将伪距改正数通过无线电或互联网传输给清洁车，清洁车接收到后，用它来修正自己测得的伪距，清洁车根据修正后的伪距和卫星的位置，利用最小二乘法求解出自己的位置坐标，以此来矫正惯性导航定位：
[0033]
x＝(a
t
pa)-1at
pl
[0034]
其中，x是n
×
1的向量，表示移动站的位置坐标，a是m
×
n的矩阵，表示观测方程的
系数矩阵，p是m
×
m的矩阵，表示观测值的权矩阵，l是m
×
1的向量，表示观测值的改正数向量；n表示未知数的个数，m表示观测值的个数。
[0035]
进一步地，所述步骤2中将获取到输出的wgs84坐标系的坐标转换为高斯二维坐标系中的坐标，公式如下：
[0036][0037][0038]
其中，b是大地纬度，l(b)为赤道到投影点的子午线弧长；为卯酉圈半径；t＝tanb；l＝l-l0为经差；η＝e
′2cos
2 b，e
′
为椭球第二偏心率；l0为子午线经度，所得到的坐标就是车辆在高斯坐标系的实时坐标。
[0039]
进一步地，所述步骤3中利用启发式算法更新步骤2获取到的位置坐标与规划好的路径规划列表中各点的距离，按照预设的路径进行前进控制，具体包括如下步骤：
[0040]
(1)先接收全局规划的路径点集，清洁车获取当前坐标，按照启发式算法，更新清洁车当前坐标与路径路径规划列表中各点的距离，选择距离自己最近的点作为参考点；
[0041]
(2)再设置一个前视距离，将参考点下一个点与清洁车的距离同前视距离作比较，当比前视距离长时，则下一个点就是目标点，如果它没有前视距离长，则往后再取点，直到清洁车与目标点的距离比前视距离长为止；
[0042]
(3)当目标点确定后，将目标点的坐标减去清洁车的坐标，得到一条向量，求出向量与正东方向形成的夹角α：
[0043][0044]
其中，x2和y2是目标点坐标，x1和y1是清洁车的坐标。
[0045]
进一步地，由于高斯坐标系以正北方向为y轴的正方向，正东方向为x轴的正方向，目标点和清洁车的角度是以正东方向为0度，而imu模块则是以正北方向为0度，所以将两者的参考系的统一，由此计算目的点与清洁车当前坐标的角度差再将其带入阿克曼模型中，所述阿克曼模型假设车辆的前轮是转向轮，后轮是驱动轮，车辆的转向角度和前轮转角之间存在一定的关系，通过这个关系来计算车辆的运动轨迹和速度；阿克曼模型为一个刚体，车辆的前轮和后轮之间的距离为l，前轮的转向角度为δ，车辆的速度为v，则可以通过以
下公式计算车辆的运动轨迹和速度：
[0046]
前轮转角：
[0047]
为了避免跳过的点太多，对全局规划的路径点非转弯点进行标记，根据三个点构成的两个向量之间的夹角大小进行判断,小于60度的就是非转弯路径点。将其状态值存贮在路径规划列表中，当处在转弯模式时，对于非转弯点的序列跨度不能超过3个，这样就能按照预设的路径进行前进，同时也能符合刚体运动学；公式如下：
[0048][0049][0050][0051]
其中，x1,y1,x2,y2,x3,y3分别是三个点的横坐标值和纵坐标值。
[0052]
有益效果：
[0053]
本发明使用rtk和10轴imu进行融合定位，并结合粒子滤波器增加定位的抗干扰性和准确性，在结合已知的高精度坐标点之间的通信来矫正累计误差。在有了路径规划之后再利用改进的纯路径跟踪实现自主驾驶，基于阿克曼模型，具有良好的转弯性能和稳定性，相较于普通的纯路径跟踪算法，本算法利用启发式算法，能够避免车辆陷入追逐无法满足运动学的目标点，也避免为了路径符合运动学而跳点过多的情况。
附图说明
[0054]
图1为本发明整体定位和路径跟踪运行结构框图；
[0055]
图2为本发明清洁车获取初始位置与姿态的融合定位流程图；
[0056]
图3为本发明清洁车根据已有规划路径进行路径跟踪流程图；
[0057]
图4为利用本发明跟踪的效果图；
[0058]
图5为本发明清洁车整体工作的流程图。
具体实施方式
[0059]
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0060]
本发明提供了一种户外无人清洁车的定位和路径跟踪方法，包括如下步骤：
[0061]
步骤1：使用rtk模块和10轴imu模块获取清洁车的初始位置与初始姿态，结合粒子滤波器进行融合定位，同时对于rtk精度大于一定值，利用修正方法矫正累计误差。
[0062]
使用rtk和10轴imu获取清洁车的初始位置与初始姿态，结合粒子滤波器进行融合定位的具体操作为：
[0063]
(1)首先接收rtk模块和imu模块的数据，然后定义描述rtk模块和imu模块的测量误差和状态误差的统计特性的协方差矩阵：
[0064][0065]
其中，p
t
是时刻t的协方差矩阵，n是粒子的数量，是第i个粒子在时刻t的状态，
hatx
t
是时刻t的状态估计值，即加权平均：
[0066][0067]
其中是第i个粒子在时刻t的权重；
[0068]
(2)根据清洁车的初始位置和姿态初始化粒子集合，生成一些随机粒子来表示车辆的状态，初始化粒子集合的公式如下：
[0069]
x
0[i]
～p(x0),i＝1,...,n
[0070]
其中，x
0[i]
表示第i个粒子的初始状态，p(x0)表示初始状态的概率分布；
[0071]
(3)根据车辆的运动模型，对粒子进行状态预测，得到下一时刻的粒子状态：
[0072][0073]
其中，x
k[i]
表示第i个粒子在时刻k的状态，表示状态转移概率分布；
[0074]
(4)根据rtk模块和imu模块的测量结果，更新每个粒子的权重，将粒子权重进行归一化，使得所有粒子权重之和为1：
[0075][0076][0077]
权重表示该粒子状态与测量结果的匹配程度，表示第i个粒子在k时刻的权重，zk表示时刻k的测量结果，表示测量概率分布；表示第i个粒子在时刻k的归一化权重；
[0078]
(5)根据粒子权重，对粒子进行重采样，得到新的粒子集合，重采样时，权重较大的粒子被选中的概率较大，权重较小的粒子被选中的概率较小：
[0079]
其中，δ(
·
)表示dirac delta函数；
[0080]
(6)根据重采样后的粒子集合，计算车辆的位置和姿态估计值，更新车辆状态估计：
[0081][0082]
其中，表示时刻k的车辆状态估计值。
[0083]
利用修正方法矫正累计误差具体操作为：
[0084]
当rtk模块信号不好的时候，接收器锁定了低于5颗的卫星，定位精度大于0.5米，平面和高程误差在1-3米，实时定位模块与已知高精度基站做通信，在接收到固定基站的信号后，利用修正技术：基准站根据自己的已知位置和卫星的位置，计算出每颗卫星到自己的真实距离，然后与测得的清洁车与基准站的伪距比较，得出伪距改正数，所述伪距为信号传播时间乘以光速；基准站将伪距改正数通过无线电或互联网传输给清洁车，清洁车接收到后，用它来修正自己测得的伪距，清洁车根据修正后的伪距和卫星的位置，利用最小二乘法求解出自己的位置坐标，以此来矫正惯性导航定位：
[0085]
x＝(a
t
pa)-1at
pl
[0086]
其中，x是n
×
1的向量，表示移动站的位置坐标，a是m
×
n的矩阵，表示观测方程的系数矩阵，p是m
×
m的矩阵，表示观测值的权矩阵，l是m
×
1的向量，表示观测值的改正数向量；n表示未知数的个数，m表示观测值的个数。
[0087]
步骤2：将步骤1最终获取的清洁车的最终wgs84坐标转换为高斯二维坐标系中的坐标。
[0088]
由于得到的是wgs84坐标，而自动驾驶中常用的坐标系有车体坐标系、世界坐标系、局部地图坐标系等，这些坐标系都是以某一地点为原点建立的局部坐标系，因此需要将wgs84坐标转换为局部地图坐标系。将获取到输出的wgs84坐标系的坐标转换为高斯二维坐标系中的坐标，公式如下：
[0089][0090][0091]
其中，b是大地纬度，l(b)为赤道到投影点的子午线弧长；为卯酉圈半径；t＝tanb；l＝l-l0为经差；η＝e
′2cos
2 b，e
′
为椭球第二偏心率；l0为子午线经度，所得到的坐标就是车辆在高斯坐标系的实时坐标。
[0092]
步骤3：获取已经规划好的清洁车路径，利用启发式算法更新步骤2获取到的位置坐标与规划好的路径规划列表中各点的距离，按照预设的路径进行前进控制。
[0093]
(1)先接收全局规划的路径点集，清洁车获取当前坐标，按照启发式算法，更新清洁车当前坐标与路径路径规划列表中各点的距离，选择距离自己最近的点作为参考点；
[0094]
(2)再设置一个前视距离，将参考点下一个点与清洁车的距离同前视距离作比较，当比前视距离长时，则下一个点就是目标点，如果它没有前视距离长，则往后再取点，直到清洁车与目标点的距离比前视距离长为止；
[0095]
(3)当目标点确定后，将目标点的坐标减去清洁车的坐标，得到一条向量，求出向量与正东方向形成的夹角α：
[0096][0097]
其中，x2和y2是目标点坐标，x1和y1是清洁车的坐标。
[0098]
由于高斯坐标系以正北方向为y轴的正方向，正东方向为x轴的正方向，目标点和
清洁车的角度是以正东方向为0度，而imu模块则是以正北方向为0度，所以将两者的参考系的统一，由此计算目的点与清洁车当前坐标的角度差再将其带入阿克曼模型中，所述阿克曼模型假设车辆的前轮是转向轮，后轮是驱动轮，车辆的转向角度和前轮转角之间存在一定的关系，通过这个关系来计算车辆的运动轨迹和速度；阿克曼模型为一个刚体，车辆的前轮和后轮之间的距离为l，前轮的转向角度为δ，车辆的速度为v，则可以通过以下公式计算车辆的运动轨迹和速度：
[0099]
前轮转角：
[0100]
为了避免跳过的点太多，对全局规划的路径点非转弯点进行标记，根据三个点构成的两个向量之间的夹角大小进行判断,小于60度的就是非转弯路径点。将其状态值存贮在路径规划列表中，当处在转弯模式时，对于非转弯点的序列跨度不能超过3个，这样就能按照预设的路径进行前进，同时也能符合刚体运动学；公式如下：
[0101][0102][0103][0104]
其中，x1,y1,x2,y2,x3,y3分别是三个点的横坐标值和纵坐标值。
[0105]
参见图4，图4展示了利用本发明跟踪的效果图，从图4可以看出，对于已有的规划路径，清洁车能够较为准确的按照行进轨迹进行行进。
[0106]
上述实施方式只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所做的等效变换或修饰，都应涵盖在本发明的保护范围之内。

技术特征：
1.一种户外无人清洁车的定位和路径跟踪方法，其特征在于，包括如下步骤：步骤1：使用rtk模块和10轴imu模块获取清洁车的初始位置与初始姿态，结合粒子滤波器进行融合定位，同时对于rtk精度大于一定值，利用修正方法矫正累计误差；步骤2：将步骤1最终获取的清洁车的最终wgs84坐标转换为高斯二维坐标系中的坐标；步骤3：获取已经规划好的清洁车路径，利用启发式算法更新步骤2获取到的位置坐标与规划好的路径规划列表中各点的距离，按照预设的路径进行前进控制。2.根据权利要求1所述的户外无人清洁车的定位和路径跟踪方法，其特征在于，所述步骤1中使用rtk和10轴imu获取清洁车的初始位置与初始姿态，结合粒子滤波器进行融合定位的具体操作为：(1)首先接收rtk模块和imu模块的数据，然后定义描述rtk模块和imu模块的测量误差和状态误差的统计特性的协方差矩阵：其中，p
t
是时刻t的协方差矩阵，n是粒子的数量，是第i个粒子在时刻t的状态，hatx
t
是时刻t的状态估计值，即加权平均：其中是第i个粒子在时刻t的权重；(2)根据清洁车的初始位置和姿态初始化粒子集合，生成一些随机粒子来表示车辆的状态，初始化粒子集合的公式如下：x
0[i]
～p(x0),i＝1,...,n其中，x
0[i]
表示第i个粒子的初始状态，p(x0)表示初始状态的概率分布；(3)根据车辆的运动模型，对粒子进行状态预测，得到下一时刻的粒子状态：其中，x
k[i]
表示第i个粒子在时刻k的状态，表示状态转移概率分布；(4)根据rtk模块和imu模块的测量结果，更新每个粒子的权重，将粒子权重进行归一化，使得所有粒子权重之和为1：化，使得所有粒子权重之和为1：权重表示该粒子状态与测量结果的匹配程度，表示第i个粒子在k时刻的权重，z
k
表示时刻k的测量结果，表示测量概率分布；表示第i个粒子在时刻k的归一化权重；(5)根据粒子权重，对粒子进行重采样，得到新的粒子集合，重采样时，权重较大的粒子被选中的概率较大，权重较小的粒子被选中的概率较小：其中，δ(
·
)表示dirac delta函数；(6)根据重采样后的粒子集合，计算车辆的位置和姿态估计值，更新车辆状态估计：
其中，表示时刻k的车辆状态估计值。3.根据权利要求1所述的户外无人清洁车的定位和路径跟踪方法，其特征在于，所述步骤1中利用修正方法矫正累计误差具体操作为：当rtk模块信号不好的时候，接收器锁定了低于5颗的卫星，定位精度大于0.5米，平面和高程误差在1-3米，实时定位模块与已知高精度基站做通信，在接收到固定基站的信号后，利用修正技术：基准站根据自己的已知位置和卫星的位置，计算出每颗卫星到自己的真实距离，然后与测得的清洁车与基准站的伪距比较，得出伪距改正数，所述伪距为信号传播时间乘以光速；基准站将伪距改正数通过无线电或互联网传输给清洁车，清洁车接收到后，用它来修正自己测得的伪距，清洁车根据修正后的伪距和卫星的位置，利用最小二乘法求解出自己的位置坐标，以此来矫正惯性导航定位：x＝(a
t
pa)-1
a
t
pl其中，x是n
×
1的向量，表示移动站的位置坐标，a是m
×
n的矩阵，表示观测方程的系数矩阵，p是m
×
m的矩阵，表示观测值的权矩阵，l是m
×
1的向量，表示观测值的改正数向量；n表示未知数的个数，m表示观测值的个数。4.根据权利要求1所述的户外无人清洁车的定位和路径跟踪方法，其特征在于，所述步骤2中将获取到输出的wgs84坐标系的坐标转换为高斯二维坐标系中的坐标，公式如下：骤2中将获取到输出的wgs84坐标系的坐标转换为高斯二维坐标系中的坐标，公式如下：其中，b是大地纬度，l(b)为赤道到投影点的子午线弧长；为卯酉圈半径；t＝tanb；l＝l-l0为经差；η＝e
′2cos2b，e
′
为椭球第二偏心率；l0为子午线经度，所得到的坐标就是车辆在高斯坐标系的实时坐标。5.根据权利要求1所述的户外无人清洁车的定位和路径跟踪方法，其特征在于，所述步骤3中利用启发式算法更新步骤2获取到的位置坐标与规划好的路径规划列表中各点的距离，按照预设的路径进行前进控制，具体包括如下步骤：(1)先接收全局规划的路径点集，清洁车获取当前坐标，按照启发式算法，更新清洁车当前坐标与路径路径规划列表中各点的距离，选择距离自己最近的点作为参考点；(2)再设置一个前视距离，将参考点下一个点与清洁车的距离同前视距离作比较，当比前视距离长时，则下一个点就是目标点，如果它没有前视距离长，则往后再取点，直到清洁车与目标点的距离比前视距离长为止；
(3)当目标点确定后，将目标点的坐标减去清洁车的坐标，得到一条向量，求出向量与正东方向形成的夹角α：其中，x2和y2是目标点坐标，x1和y1是清洁车的坐标。6.根据权利要求5所述的户外无人清洁车的定位和路径跟踪方法，其特征在于，由于高斯坐标系以正北方向为y轴的正方向，正东方向为x轴的正方向，目标点和清洁车的角度是以正东方向为0度，而imu模块则是以正北方向为0度，所以将两者的参考系的统一，由此计算目的点与清洁车当前坐标的角度差再将其带入阿克曼模型中，所述阿克曼模型假设车辆的前轮是转向轮，后轮是驱动轮，车辆的转向角度和前轮转角之间存在一定的关系，通过这个关系来计算车辆的运动轨迹和速度；阿克曼模型为一个刚体，车辆的前轮和后轮之间的距离为l，前轮的转向角度为δ，车辆的速度为v，则可以通过以下公式计算车辆的运动轨迹和速度：前轮转角：为了避免跳过的点太多，对全局规划的路径点非转弯点进行标记，根据三个点构成的两个向量之间的夹角大小进行判断，小于60度的就是非转弯路径点。将其状态值存贮在路径规划列表中，当处在转弯模式时，对于非转弯点的序列跨度不能超过3个，这样就能按照预设的路径进行前进，同时也能符合刚体运动学；公式如下：预设的路径进行前进，同时也能符合刚体运动学；公式如下：预设的路径进行前进，同时也能符合刚体运动学；公式如下：其中，x1,y1,x2,y2,x3,y3分别是三个点的横坐标值和纵坐标值。

技术总结
本发明涉及清洁车定位与路径跟踪技术领域，公开了一种户外无人清洁车的定位和路径跟踪方法，使用RTK和10轴IMU进行融合定位，并结合粒子滤波器增加定位的抗干扰性和准确性，在结合已知的高精度坐标点之间的通信来矫正累计误差，并将坐标位置转换到高斯二维坐标系中；在已有规划好路径的前提下，利用改进的纯路径跟踪实现自主驾驶，改进的纯路径跟踪基于阿克曼模型，利用启发式算法更新步骤2获取到的位置坐标与规划好的路径规划列表中各点的距离，按照预设的路径进行前进控制。与现有技术相比，本发明具有良好的转弯性能和稳定性，能够避免车辆陷入追逐无法满足运动学的目标点，也避免为了路径符合运动学而跳点过多的情况。况。况。


技术研发人员：孔荣双 艾米尔 肖绍章 李彪 丁楠 耿君佐 单劲松 周泓 费宏彦 顾家铭
受保护的技术使用者：淮阴工学院
技术研发日：2023.06.19
技术公布日：2023/8/14