CN116027266B Uwb与视觉紧耦合slam算法的定位方法及系统 （西北工业大学）

文.UWB与视觉紧耦合SLAM算法的定位方法及系统本发明提出一种UWB与视觉紧耦合SLAM算法的定位方法及系统，将可见光相机与UWB传感器UWB锚点；首先通过定位设备距离信息和图像信对图像数据进行视觉初始化，并建立UWB初始化信息帧，基于重投影误差与UWB距离误差建立联及缩放因子优化。本发明能够仅依赖一个UWB锚2过定位设备上的相机采集环境图像信息，同时实现环境图像信息与距离信息的时间对齐，新采集的信息帧加入UWB初始化队列；步骤5.1：当UWB初始化帧序列内信息帧的数目大于设定的UWB锚点初始化阈值时，对UWB初始化帧队列内的信息帧建立基于UWB距离误差的优化方程，所述UWB距离误差指计算距离与信息帧中的UWB测量距离之差，其中的计算距离指根据信息帧估计的位姿计算的定计算所述新的信息帧与其参考关键帧的相对位姿差值、UWB测量距离差值以及采集时键帧与全局地图内的关键帧相匹配时，建立匹配的关键帧之间的视觉与UWB信息紧耦合的中，根据图像帧前后的UWB测量帧，采用线性插值的方法得到图像帧对应时刻的距离测量3.根据权利要求2所述一种UWB与视觉紧耦合S以相机接收到的第二帧图像帧的时间为系统初始时间T0，以第一帧图像帧的前后两个3UWB测量距离帧的时间戳均值为UWB测量距离初始帧的后续第K帧图像帧的系统时间戳与第K帧UWB测量帧的系式中TCK为第K帧图像帧的采集时间戳，TC0为第一帧图像帧的采集时间戳，TUK为第K帧保存第K_1帧图像帧与第K+1帧图像帧之间采集时间区间内采集到的UWB测距信息，检式中ry"分别为采集第K帧图像时的前一个与n+1分别为第n帧与第n+1个UWB测量帧测量的距离，插值得到的dk为第K帧图像4如果运动模型有效，则建立局部地图点与当前帧中图像特如果运动模型失效，则进行当前帧中图像特征点与其参考关键帧中的图像特征点匹步骤4.2.3：计算当前帧的UWB锚点测距信息与UWB初始化帧队列内最新加入帧测距信根据所述新的信息帧中的UWB测量距离，以及基于匀速运动模型所估计位姿而得到的如果所述判断公式得到满足，表示可用匀速运动模型进行投影到所述新的信息帧的图像中，建立地图点与所述新的信息帧中图像特征点的匹配关如果所述判断公式不能得到满足，表示不可用匀速运动模匹配关系建立所述新的信息帧中图像特征点与地图点的将重投影误差与UWB距离误差加权求和，从而建立所述新的信息帧位姿联合优化方程5跟踪线程，完成系统初始化后，系统根据当前帧特征点与系统内存6[0004]超宽带(UltraWideBand，UWB)定位技术具有厘米级定[0005]UWB与视觉紧耦合的SLAM利用UWB测量距离能够消除视觉SL[0006]为解决现有技术存在的问题，本发明提出一种UWB与视觉紧耦合SLAM算法的定位7否将新采集的信息帧加入UWB初始化队列；对UWB初始化帧队列内的信息帧建立基于UWB距离误差的优化方程，所述UWB距离误差指计算距离与信息帧中的UWB测量距离之差，其中的计算距离指根据信息帧估计的位姿计算的[0017]计算所述新的信息帧与其参考关键帧的相对位姿差值、UWB测量距离差值以及采述新的信息帧及其观测到的地图点加入局部地图及全局的关键帧与全局地图内的关键帧相匹配时，建立匹配的关键帧之间的视觉与UWB信息紧耦两个UWB测量距离帧的时间戳均值为UWB测量距8[0028]保存第K_1帧图像帧与第K+1帧图像帧之间采集时间区间内采集到的UWB测距信影误差的小的位姿与地图点作为初始化帧的位姿及离，如果计算距离与当前帧中的UWB测量距离之差小于设定阈值，则表示系统运动模型有前帧中图像特征点的重投影误差建立光束平差方程9[0046]根据所述新的信息帧中的UWB测量距离，以及基于匀速运动模型所估计位姿而得征点匹配关系建立所述新的信息帧中图像特征点与地图点的[0050]将重投影误差与UWB距离误差加权求和，从而建立所述新的信息帧位姿联合优化[0054]步骤7.1：将加入关键帧队列的关[0055]步骤7.2：当优化帧序列中的[0069]本发明的方法仅依赖一个UWB锚点即可实现移动设备的高精度定位信息，且确保[0070]本发明采用UWB与视觉紧耦合SLAM方法实现估计移动设备的定位信息，仅在外界[0073]本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得[0079]由于UWB传感器的测量频率高于可见光相机的图像采集频率，而紧耦合算法需要[0080]这里根据图像帧前后的UWB测量帧，采用线性插值的方法得到图像帧对应时刻的的前后两个UWB测量距离帧的时间戳均值[0087]保存第K_1帧图像帧与第K+1帧图像帧之间采集时间区间采集到的UWB测距信息，[0089]式中:,r"分别为采集第K帧图像时的前一个与后一个UWB测量帧的时间戳，对影误差的小的位姿与地图点作为初始化帧的位姿及否将新采集的信息帧加入UWB初始化队列。[0097]由于UWB锚点位置初始化需要至少3个不共面的信息帧建立基于距离误差的优化离，如果计算距离与当前帧中的UWB测量距离之差小于设定阈值，则表示系统运动模型有前帧中图像特征点的重投影误差建立光束平差方程对UWB初始化帧队列内的信息帧建立基于UWB距离误差的优化方程，所述UWB距离误差指计算距离与信息帧中的UWB测量距离之差，其中的计算距离指根据信息帧估计的位姿计算的据第k帧信息帧视觉图像估计的定位设备位置，s为视觉SLAM尺度与公制单位间的缩放因[0115]这里采用基于匀速运动模型或参考关键帧的方式建立新的信息帧中图像特征点[0116]根据新的信息帧中的UWB测量距离，以及基于匀速运动模型所估计位姿而得到的投影到所述新的信息帧的图像中，建立地图点与所述新的信息帧中图像特征点的匹配关征点匹配关系建立所述新的信息帧中图像特征点与地图点的[0120]将重投影误差与UWB距离误差加权求和，从而建立所述新的信息帧位姿联合优化2_e)2[0126]计算所述新的信息帧与其参考关键帧的相对位姿差值、UWB测量距离差值以及采述新的信息帧及其观测到的地图点加入局部地图及全局[0131]步骤7.1：将加入关键帧队列的关的关键帧与全局地图内的关键帧相匹配时，建立