一种基于双目视觉、IMU和UWB融合的AGV定位导航方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN114323002A(43)申请公布日2022.04.12(21)申请号202111611402.1(22)申请日2021.12.27(71)申请人浙江大学地址315400浙江省宁波市余姚市凤山街道冶山路479号科创大厦12楼(72)发明人卢建刚余天义陈金水(74)专利代理机构浙江杭州金通专利事务所有限公司33100代理人刘晓春(51)Int.Cl.G01C21/16(2006.01)G01C21/20(2006.01)权利要求书2页说明书5页附图3页(54)发明名称一种基于双目视觉、IMU和UWB融合的AGV定位导航方法(57)摘要本发明提供一种基于双目视觉、IMU和UWB融合的AGV定位导航方法，在空间部属一个UWB基站，且在AGV上搭载UWB定位标签、双目相机和IMU；通过UWB信号在UWB基站和UWB定位标签之间的传播时间计算出两者之间的距离，采集双目相机数据、IMU中加速度计数据和陀螺仪数据；同时利用图像特征提取和跟踪算法得到特征点的像素坐标，根据IMU中加速度计数据和陀螺仪数据进行相邻两图像帧之间的惯性预积分；建立UWB测距残差、双目视觉重投影残差和惯性预积分残差三者之和的目标优化函数，采用非线性优化方法求解，从而实现AGV位置求解。本发明通过三种传感器的融合，有效且准确的计算出AGV的定位信息，可以避免纯视觉定位中的累计误差和IMU本身的误差，并显著降低成本，使用方便。CN114323002ACN114323002A权利要求书1/2页1.一种基于双目视觉、IMU和UWB融合的AGV定位导航方法，其特征在于：在室内环境中选择固定点部署UWB基站，在AGV上搭载双目相机、IMU和UWB定位标签；所述方法包括以下步骤：步骤1，AGV移动过程中，UWB定位标签发送由超宽频脉冲信号组成的脉冲数据包给UWB基站，计算信号在UWB基站和UWB定位标签之间的传播时间T，从而根据光速c乘以传播时间T计算出k时刻UWB定位标签与UWB基站之间的距离d(k)＝c×T；步骤2，采集k时刻双目视觉传感器图像帧数据C(k)、IMU中加速度计数据和陀螺仪数据步骤3，根据双目视觉传感器图像帧数据C(k)，应用图像特征提取和跟踪算法，计算得到左目相机图像帧视觉特征点投影到对应的右目相机图像帧中的像素坐标；步骤4，对于采集到的IMU中加速度计数据和陀螺仪数据进行相邻两图像帧之间的惯性预积分；步骤5，建立基于UWB测距残差、双目视觉重投影残差和惯性预积分残差三者之和的目标优化函数，对建立的目标优化函数采用非线性优化方法进行求解，从而实现对AGV位置的求解；步骤6，输出AGV位置的数据，并返回步骤1。2.根据权利要求1所述的基于双目视觉、IMU和UWB融合的AGV定位导航方法，其特征在于：所述步骤1中，信号在UWB基站和UWB定位标签之间的传播时间是根据UWB基站在其时间戳上的Ta1时刻发射请求性质的脉冲信号，UWB定位标签在其时间戳上的Tb1时刻接收到该信号，然后UWB定位标签在Tb2时刻发射一个响应性质的信号被UWB基站在自己的时间戳Ta2时刻接收；由此计算出脉冲信号在UWB基站和UWB定位标签之间的传播时间为T＝0.5(Ta2‑Ta1)‑0.5(Tb2‑Tb1)。3.根据权利要求1所述的基于双目视觉、IMU和UWB融合的AGV定位导航方法，其特征在于，在所述步骤2中，所述加速度计数据和所述陀螺仪数据分别为：其中，a(k)和ω(k)分别为k时刻加速度计和陀螺仪测量的理想值，ba(k)和bω(k)分别为加w速计和陀螺仪的偏置值，na和nω为加速度计和陀螺仪的测量噪声，g为世界坐标系下的重力加速度，为k时刻世界坐标系到惯性坐标系的旋转矩阵。4.根据权利要求1所述的基于双目视觉、IMU和UWB融合的AGV定位导航方法，其特征在于，在所述步骤3中，所述图像特征提取和跟踪算法具体为：对于采集到的每一帧图片，提取Shi‑Tomasi角点，进行视觉特征点提取并跟踪；对于左目得到的当前图像帧的视觉特征点，利用KLT稀疏光流法追踪，获得右目图像上对应的特征点，然后对右目图像光流追踪获得的特征点再进行反向光流追踪，从而得到左目图像帧中对应的视觉特征点；最后，当左目反向光流追踪到的特征点和左目前一时刻图像帧合格特征点之间的像素点距离全部小于阈值时，确定为最终的合格特征点，然后将左目图像帧的特征点保存为左目图像帧的合格特征点，同时将右目图像帧的特征点保存为右目图像帧的合格特征点。2CN114323002A权利要求书2/2页5.根据权利要求1所述的基于双目视觉、IMU和UWB融合的AGV定位导航方法，其特征在于，在所述步骤4中，所述相邻两图像帧之间的惯性预积分的具体过程为：对于第bk帧和第bk+1