System.ArgumentOutOfRangeException: 索引和长度必须引用该字符串内的位置。 参数名: length 在 System.String.Substring(Int32 startIndex, Int32 length) 在 zhuanliShow.Bind()
【技术实现步骤摘要】
本专利技术属于无人机定位领域,具体涉及基于光流传感器和动态权重的无人机视觉惯性定位方法及系统。
技术介绍
1、定位和导航对于自主机器人系统至关重要,尤其是无人机。机器人的自主定位通常是通过集成机载传感器的测量值来估计机器人的状态实现的。然而,在长时间内或在纹理稀疏的环境中,视觉-惯性系统会出现显着的估计漂移,一种解决方案是融合没有累积误差的传感器来辅助定位。目前,主流的传感器融合方法关注的传感器主要为gps、uwb、激光雷达等。但是,gps在室内无法提供稳定的信号,uwb需要在环境中预先放置固定基站,并且覆盖范围有限。基于激光雷达的里程计通常需要很大的有效负载能力来承载激光雷达传感器,这与轻型四旋翼飞行器系统不兼容。而光流传感器可以通过计算连续图像中的像素变化来提供无累积误差的速度信息,从而有助于提高定位精度。
2、目前针对光流传感器的研究缺少预处理过程,由于光流传感器测量噪声的存在、传感器融合中频率不相同以及不同传感器之间存在相对旋转关系,直接利用传感器输出的数据存在较大的误差,仅将惯性导航系统(ins)与光流传感器测量数据融合不能实现较高的定位精度,无法满足对精度要求比较高的位姿估计。
3、而且基于光流传感器的定位方法研究主要集中在传统方法上,例如用于融合imu和光流传感器数据的扩展卡尔曼滤波器(ekf)以及容积卡尔曼滤波ckf。但是,上述方法都是基于传统的卡尔曼滤波,由于卡尔曼滤波的马尔可夫性,所以在预测过程中仅考虑上一帧的状态,并且缺少对系统可观性的分析和约束,更容易引入累积误差。
4
技术实现思路
1、本专利技术目的在于针对现有实验中环境纹理较弱情况下无人机定位精度较差的难题,提出基于光流传感器和动态权重的无人机视觉惯性定位方法。
2、本专利技术的目的是通过以下技术方案实现的:一种基于光流传感器和动态权重的无人机视觉惯性定位方法,该方法包括如下步骤:
3、构建两个滑动窗口分别进行光流传感器初始化和状态变量更新;所述光流传感器初始化包括使用互补滤波对光流传感器测量值进行预处理、线性插值对齐时间戳和相对旋转关系优化;
4、对相机获得的图像进行现有特征跟踪和排除异常点,并进行fast特征提取并新增至网格中;
5、对惯性传感器的测量数据进行积分后,对下一时刻的特征点状态进行估计,同时对视觉惯性系统进行线性化,计算协方差矩阵;
6、分别构建相机观测模型和光流观测模型,通过协方差矩阵和两个观测模型依次进行状态变量更新并对两个滑动窗口进行更新。
7、进一步地,所述使用互补滤波对光流传感器测量值进行预处理具体为:首先利用陀螺仪获得的角速度测量值来补偿旋转的影响,获得光流速度vof;同时,结合来自姿态和航向参考系统的加速度计读数和姿态估计来执行积分并计算积分速度vac;然后使用互补滤波融合vof和vac,从而得到当前状态测量vm。
8、进一步地,所述线性插值对齐时间戳具体为:对齐光流时间tk的测量值vkm=λvβm+(1-λ)vαm,其中λ=(tk-tα)/(tβ-tα);tk为接收立体图像的时刻;vαm和vβm分别为tα和tβ处获得的光流测量值,且tα<tk<tβ。
9、进一步地,所述相对旋转关系优化具体为:当滑动窗口开始删除最旧的帧时,记录历史估计的状态速度和光流传感器测得的速度,通过ceres-solver求解器,求解优化问题:
10、
11、其中k表示第k时刻,fvk,m是经过互补滤波得到的第k时刻的光流传感器的测量值,是状态估计的速度,是对应的旋转矩阵,通过求解优化问题获得对光流传感器和vio系统的相对旋转关系和时间差δtf的估计。
12、进一步地,所述对相机获得的图像进行现有特征跟踪和排除异常点包括:对于新获得的一帧图像,首先使用klt光流算法跟踪现有特征,根据几何投影关系,分别对左右图进行前后帧2-points ransac,剔除外点;只保留左右目都通过ransac的特征点。
13、进一步地,所述进行fast特征提取并新增至网格中包括:对新获得的一帧图像进行fast特征提取,在已有特征点越少的网格中保留越多特征点,在已有特征点已经达到阈值的网格中,不再加入新的特征点。
14、进一步地,所述对下一时刻的特征点状态进行估计具体为:状态变量由两部分组成:
15、
16、
17、
18、其中是k时刻从vio坐标系到imu坐标系的四元数;和分别表示vio坐标系中imu的位置和速度;bg和ba表示陀螺仪和加速度计偏差;是滑动窗口中的n个相机历史状态;ipc和是相机坐标系相对于imu坐标系的旋转和平移;为从vio到光流传感器坐标系的变换;
19、通过状态变量构建预测模型:
20、
21、
22、
23、
24、
25、其中ωm和am分别为陀螺仪和加速度计的测量值,g为重力向量,ω表示从到纯虚四元数的转化,c(.)为从四元数到旋转矩阵的转换;03为3*3的零矩阵。
26、进一步地,所述协方差矩阵的计算过程如下:对系统进行线性化:
27、其中~表示误差值,针对变量x,真实值、误差值和估计值之间的关系为nimu向量中为加速度和陀螺仪误差。
28、利用线性化的得到的f和g矩阵,计算协方差矩阵
29、
30、
31、
32、其中q为初始的噪声权重矩阵。
33、进一步地,所述相机观测模型的状态更新过程如下:构建相机观测模型:其中nf是视觉测量的零均值高斯白测量噪声,表示vio坐标系中跟踪特征位置的误差状态,hv,x和hv,f是残差rc相对于和的雅各比矩阵;
34、在每次进行更新前,对特征点的置信空间进行计算;首先通过协方差矩阵计算从图像跟踪像素误差到位姿误差的转换:
35、
36、其中q是更新中使用的特征点的数量,jh是视觉重投影误差对位姿的偏导数,∑i是特征点测量值的协方差矩阵,∑θ表示位姿空间的置信椭球体;标准ekf更新时的噪声矩阵通过下式计算得到:
37、
38、其中ir是单位矩阵,rn被建模为零均值的高斯噪声;rn矩阵写为:
39、
40、
41、其中λ是只与相机有关的参数;
42、当特征点不再被跟踪或者滑动窗口中状态数量达到上限的时候,将基于上述计算得到rn、hv,x和hv,f,通过标准ekf更新公式,对所有的状态变量vxk进行更新。
43、进一步地,所述光流观测模型的状态更新过程如下:构建光流观测模型本文档来自技高网...
【技术保护点】
1.一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,该方法包括如下步骤:
2.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述使用互补滤波对光流传感器测量值进行预处理具体为:首先利用陀螺仪获得的角速度测量值来补偿旋转的影响,获得光流速度vOF;同时,结合来自姿态和航向参考系统的加速度计读数和姿态估计来执行积分并计算积分速度vAC;然后使用互补滤波融合vOF和vAC,从而得到当前状态测量vm。
3.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述线性插值对齐时间戳具体为:对齐光流时间tk的测量值vkm=λvβm+(1-λ)vαm,其中λ=(tk-tα)/(tβ-tα);tk为接收立体图像的时刻;vαm和vβm分别为tα和tβ处获得的光流测量值,且tα<tk<tβ。
4.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述相对旋转关系优化具体为:当滑动窗口开始删除最旧的帧时,记录历史估计的状态速度和光流传感器测
5.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述对相机获得的图像进行现有特征跟踪和排除异常点包括:对于新获得的一帧图像,首先使用KLT光流算法跟踪现有特征,根据几何投影关系,分别对左右图进行前后帧2-points RANSAC,剔除外点;只保留左右目都通过RANSAC的特征点。
6.根据权利要求5所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述进行Fast特征提取并新增至网格中包括:对新获得的一帧图像进行Fast特征提取,在已有特征点越少的网格中保留越多特征点,在已有特征点已经达到阈值的网格中,不再加入新的特征点。
7.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述对下一时刻的特征点状态进行估计具体为:状态变量由两部分组成:
8.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述协方差矩阵的计算过程如下:对系统进行线性化:
9.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述相机观测模型的状态更新过程如下:构建相机观测模型:其中nf是视觉测量的零均值高斯白测量噪声,表示VIO坐标系中跟踪特征位置的误差状态,HV,x和HV,f是残差rc相对于和的雅各比矩阵;
10.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述光流观测模型的状态更新过程如下:构建光流观测模型:
...【技术特征摘要】
1.一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,该方法包括如下步骤:
2.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述使用互补滤波对光流传感器测量值进行预处理具体为:首先利用陀螺仪获得的角速度测量值来补偿旋转的影响,获得光流速度vof;同时,结合来自姿态和航向参考系统的加速度计读数和姿态估计来执行积分并计算积分速度vac;然后使用互补滤波融合vof和vac,从而得到当前状态测量vm。
3.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述线性插值对齐时间戳具体为:对齐光流时间tk的测量值vkm=λvβm+(1-λ)vαm,其中λ=(tk-tα)/(tβ-tα);tk为接收立体图像的时刻;vαm和vβm分别为tα和tβ处获得的光流测量值,且tα<tk<tβ。
4.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述相对旋转关系优化具体为:当滑动窗口开始删除最旧的帧时,记录历史估计的状态速度和光流传感器测得的速度,通过ceres-solver求解器,求解优化问题:
5.根据权利要求1所述的一种基于光流传感器和动态权重的无人机视觉惯性定位方法,其特征在于,所述对相机获得的图像进行现有特征跟踪和排除异常点包括:对于新获得的...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。