[发明专利]基于UWB在线锚点的视觉惯性紧耦合里程计及计量方法

权利要求书

1.一种基于UWB在线锚点的视觉惯性紧耦合里程计，其特征是，结构如下：超宽带技术制成的电波测距模块UWB，部署在两个位置，其一是作为锚点部署在环境四周，锚点自身的位置通过在线锚点标定单元计算出，在线锚点标定步骤：

使用UWB的定位时，利用输出的里程计信息以及UWB的测距信息，标定出在无人机起飞初始时刻的坐标系下锚点的坐标，具体步骤如下：

其中d_i是第i次距离测量值，d_n是第n次距离测量值，其中：i＞n，γ是可调阈值,f是UWB模块测距更新速率，为当前无人机在世界坐标系下的位姿，v_thr为预先设定的速度阈值，如果满足该条件，则第i次距离测量值将被抛弃，将第i+1次距离测量值代入上式进行判断，以此类推直到第m次测量值不满足该条件后，将n赋值为m；

于是，针对第i次距离测量，得到关于测距信息的残差

其中d_i是第i次距离测量值，是由IMU预积分和视觉里程计得到的与UWB测距信息同一时间戳下的无人机世界坐标系下的位姿，是待估计的锚点在世界坐标系下的三维坐标，于是优化的代价函数E_r：

其中ρ()是一个pseudo-Huber损失函数，使用Ceres的自动求导功能，编写代价函数后确定待优化变量将参数d_i、传入接口后即可进行优化，判断优化终止的条件为前后两次优化得到的之差的二范数小于0.1；

其二是作为标签放置在无人机上，标签按照预设的顺序主动向锚点发送测距请求，根据TW-ToF测量原理获得两个模块之间的距离，通过离群值检测单元以滤除部分噪声后，作为输入送入紧耦合优化单元；IMU惯性测量单元用于高频测量无人机的加速度和角速度信息，IMU测量值经过低通滤波器模块处理后，同样作为输入送入紧耦合优化单元；双目摄像头的测量值经过视觉里程计的处理后，也作为输入送入紧耦合优化单元；在线锚点标定单元、离群值检测单元和紧耦合优化单元均为供嵌入式机载处理器调用的模块，嵌入式机载处理器利用所述的紧耦合优化单元，处理UWB模块之间的距离测量值、IMU测量值和双目摄像头测量值，用紧耦合优化的方式解析出无人机的位置与姿态信息。