基于ROS的惯性导引与视觉信息融合的AGV导航系统及方法与流程

本发明属于无人驾驶，尤其涉及基于ros的惯性导引与视觉信息融合的agv导航系统及方法。

背景技术：

1、自动引导车(agv)是一种智能轮式移动机器人，其利用电磁或光学等自动导引装置，并沿规定的导引路径行驶完成搬运工作，具有行动快捷、可控制和安全性好等优势，广泛用于工厂自动化生产线、仓储物流、机场和港口中的物料传送。

2、惯性导航系统(inertial navigation system,ins)是一种不需要外界信息，就能实现载体导航和定位的完全自主性的导航系统，它利用加速度计测量载体加速度，对其积分获得速度和位置，并通过陀螺仪来建立空间基准和计算载体姿态。由于依赖惯性定律，它的自主性即不受电磁干扰的影响，也不向外辐射能量，具有很好抗干扰性和隐蔽性，因此，应用范围遍及武器制导，海洋探测和车辆、飞机等运输工具的导航等军事及民用领域，惯性导航技术逐渐成为国家发展的核心技术之一。

3、视觉导航是通过摄像机来拍摄周围环境的图像序列，并对图像分析来确定载体的位置和姿态的导航技术。根据传感器的数量，视觉导航可以分为单目视觉和夺目视觉，而夺目视觉中以双目居多。由于单目视觉导航系统结构简单，不考虑两个或多个摄像机间的距离及安装问题，使用较为方便。

4、但是，上述的单目slam在实际使用过程中存在初始化的尺度问题和追踪的尺度漂移问题；惯性导航数据稳定，但积累误差更为严重，因此，单目slam和惯性导航单独使用都无法满足精度的目的。

技术实现思路

1、针对现有技术的不足，本发明提供了基于ros的惯性导引与视觉信息融合的agv导航系统及方法，可以将单目视觉slam的高精度与惯性导航数据的稳定性结合起来，优势互补，达到满足精度的目的。

2、为了达到上述目的，本发明的技术方案是：

3、基于ros的惯性导引与视觉信息融合的agv导航方法，包括以下步骤：

4、1)通过imu和视觉传感器分别采集agv的运动数据信息和视觉图像信息；

5、2)将获得的运动数据信息通过滤波器进行平滑处理，去除数据中的高频噪声和震荡；将获取的视觉图像信息通过sift算法提取、匹配相邻环境图像间的特征元素；

6、3)将平滑处理后的运动数据信息通过姿态矩阵进行实时姿态解算，解算出导航系统需要的姿态角、位置信息；通过基础矩阵计算匹配特征元素之间的关系，确定agv的相对位置和姿态信息；

7、4)在imu和视觉传感器的子卡尔曼滤波器中分别引入huber鲁棒估计方法，估计imu解算的姿态角、位置信息，以及视觉传感器确定的相对位置和姿态信息；

8、5)在主卡尔曼滤波器中使用imm多模型滤波算法将两个子卡尔曼滤波的估计结果进行融合优化，得到融合后的系统状态估计数据；

9、6)将得到的系统状态估计数据发送到ros机器人操作系统，实现agv控制。

10、优选的，步骤1)中，所述运动数据信息包括agv的加速度和角速度信息；所述视觉图像信息包括agv周围环境的图像序列。

11、优选的，步骤2)中，sift算法提取环境图像的特征元素的步骤如下：

12、201、使用dog函数寻找候选特征点，并利用dog函数在尺度空间的泰勒级数进行曲线拟合，精确定位特征点的位置；

13、所述dog函数寻找候选特征点的公式为：

14、d(x,y,σ)＝(g(x,y,kσ)-g(x,y,σ))*i(x,y)

15、＝l(x,y,kσ)-l(x,y,σ)

16、其中，k为尺度系数，是一个固定值；(x,y)为图像像素的位置；σ为尺度；d(x,y,σ)是dog函数，为两个高斯函数的差；l(x,y,σ)为一个图像的尺度空间；g(x,y,σ)是尺度可变高斯函数；i(x,y)是原图像；其中，

17、l(x,y,σ)＝g(x,y,σ)*i(x,y)

18、

19、其中，e为自然常数；

20、所述dog函数在尺度空间的泰勒级数的拟合函数为：

21、

22、其中，x＝(x,y,σ)t对上式求导并让它等于零得到：

23、

24、其中，为x的一阶导数，带回泰勒级数为：

25、

26、将小于0.03的不明显的关键点舍弃掉，得到可靠的特征点；

27、202、根据图像梯度方向，采集特征点在高斯金字塔图像3σ邻域窗口内像素的梯度和方向分布特征，分配特征点的方向；

28、所述梯度的模值和方向如下：

29、

30、θ(x,y)＝tan-1((l(x,y+1)-l(x,y-1))/(l(x+1,y)-l(x-1,y)))

31、其中，l为特征点所在的度空间值；梯度的模值m(x,y)按σ＝1.5σ_oct的高斯分布加成，σ_oct为关键点所在组的组内尺度；按尺度采样的3σ原则，邻域窗口半径为3*1.5σ_oct；

32、203、使用128维向量描述特征点，得到sift的特征点描述符。

33、优选的，步骤2)中，所述特征点的匹配是通过两点集合内关键点描述子的比对来完成。

34、优选的，步骤3)中，所述姿态矩阵实时姿态解算公式为：

35、

36、其中，roll为横滚；pitch为俯仰；k为比例系数；rollacc为横滚加速度计位姿；rollgyo为横滚陀螺仪位姿；pitchacc为俯仰加速度计位姿；pitchgyro为俯仰陀螺仪位姿。

37、优选的，步骤5)中，imm多模型滤波算法将两个子卡尔曼滤波的估计结果进行融合优化的方式为：

38、501、根据imm滤波其中的正确性概率，将每个模型相关的模型的估计状态及斜方差矩阵进行加权融合，得到融合后的目标在k-1时刻的最优目标状态及协方差矩阵：

39、

40、

41、其中，j0为相关模型的第0个元素，为融合的相关性系数，n为模型个数；

42、502、根据最优目标状态评价模型对当前状态估计的好坏，其计算如下：

43、

44、其中，代表e的次幂；λi,k为第i,k个数据的似然值；n是数据个数；ri,k为测量误差；si,k为测量误差协方差矩阵，其中，ri,k和si,k的计算如下：

45、

46、

47、其中，ri,k是系统误差；

48、503、根据计算的k时刻的每个模型的似然值，得到每个模型的描述的准确程度，实现模型可信度的更新，其公式表达为：

49、

50、其中为融合后的可信度；c是为了让μi,k实现归一化的分母系数；且和c的计算如下：

51、

52、

53、504、根据更新后的置信度μ可以依次将模型融合起来，得到imm滤波算法的输出目标状态及协方差矩阵：

54、

55、

56、一种基于ros的惯性导引与视觉信息融合的agv导航系统，该系统包括运动控制模块、通信模块、单目视觉相机模块、imu惯性测量模块、融合定位模块和绘图模块；

57、运动控制模块用于向融合定位模块发送运动指令，控制agv的运动；

58、单目视觉相机模块用于收集agv周围环境信息；

59、imu惯性测量模块用于收集agv机体的运动状态信息；

60、通信模块分别与imu惯性测量模块、单目视觉相机模块和融合定位模块通信，将imu惯性测量模块和单目视觉相机模块收集的数据信息传递给融合定位模块；

61、融合定位模块用于执行非线性优化进行数据融合，实现精确的机器人定位，并将数据发送到绘图模块；

62、绘图模块用于根据融合定位模块发送的数据进行绘图工作，绘制agv的导航线路。

63、8.根据权利要求7所述的基于ros的惯性导引与视觉信息融合的agv导航方法，其特征在于，所述单目视觉相机模块和imu惯性测量模块分别通过通信模块与融合定位模块连接；所述运动控制模块和绘图模块分别与融合定位模块连接。

64、本发明的技术效果和优点：

65、1、本发明提供的基于ros的惯性导引与视觉信息融合的agv导航方法，通过将单目视觉slam技术与惯性导航数据融合，将两者的优势互补，以达到提高导航精度的目的；单目视觉slam可以通过特征点提取、匹配和姿态估计等方法实时地构建环境地图和估计载体的位置姿态；惯性导航数据则可以提供稳定的运动状态信息，能够在短时间内提供较高的姿态和位置更新频率；通过融合这两种数据，可以克服单一导航方法可能存在的误差和漂移问题，提高导航精度和稳定性。

66、2、本发明提供的基于ros的惯性导引与视觉信息融合的agv导航方法，采用主卡尔曼滤波器进行数据融合优化，在数据融合过程中，imu惯性测量单元和视觉传感器分别作为两个子系统，各自处理自己的数据信息，并使用huber的鲁棒滤波算法处理imu数据，提高对噪声和异常值的鲁棒性；而imm(interacting multiple model)算法则用于对imu和视觉信息进行多模型融合，动态调整子滤波器的权重，根据不同的运动模式或传感器可靠性适应不同的导航场景。

67、3、本发明提供的基于ros的惯性导引与视觉信息融合的agv导航方法，通过将imu和视觉传感器信息的融合优化，可以实时解算载体的姿态角和位置信息，这些姿态角和位置信息用于实时的导航和控制算法，指导agv在环境中的运动，通过实时姿态解算和导航，可以准确地控制agv的运动轨迹，实现精确的导航和定位。