一种双轮侦查球图像反馈姿态校正系统的制作方法

本发明涉及图像校正，特别涉及一种双轮侦查球图像反馈姿态校正系统。

背景技术：

1、双轮侦查球是一种用于复杂环境侦察的球形机器人，其通过驱动轮在室内外复杂地面行走，搭载摄像头采集环境图像用于远程侦察与自主导航，在侦查球执行侦察任务时，其搭载的摄像头需要输出稳定、无畸变的图像以供操作人员观察或后续视觉任务使用；然而，侦查球在行走过程中由于驱动轮与地面摩擦、越障冲击、单轮打滑等因素，会产生随机的包括横滚、俯仰和偏航的三维姿态变化，导致输出的图像发生刚体变形，如倾斜、旋转，严重影响图像的可用性和操作人员的视觉体验；

2、现有技术通过融合摄像头与惯性测量单元的数据，能够实时估计设备的位姿，并基于位姿估计值对图像进行几何变换以校正姿态导致的图像变形；然而，在无纹理或光照剧烈变化等视觉退化场景下，纯视觉与惯性系统的位姿估计精度显著下降，导致输出图像校正效果不佳，为弥补这一缺陷，部分现有技术进一步引入轮速计，通过轮速数据提供额外的平面运动约束，期望提升位姿估计的鲁棒性；同时，为了改善相邻帧间大幅位姿变化导致的特征匹配困难，现有技术通常基于轮速与惯性测量数据（imu）融合后的单一运动预测值，对当前帧图像进行预变形校正（即图像预校正），使预校正后的图像更接近上一帧的视角，以降低帧间特征匹配的难度；

3、然而，现有技术仍存在以下缺陷：双轮侦查球在越障、单轮打滑或受到瞬时冲击时，轮速数据因轮地接触失效，其推算所得的位移量包含虚假分量，与imu测量的真实姿态变化产生冲突，而现有技术未能在图像预校正阶段主动检测这种多源传感数据冲突，仍直接将轮速推算的位移量与imu测量值融合用于求解单一单应变换矩阵，由此导致单应矩阵同时包含真实的姿态旋转和虚假的平面位移，使预校正图像产生局部拉伸和几何扭曲，进而造成后续特征点的像素位置失真、相邻帧特征匹配产生偏移，且现有技术中扩展卡尔曼滤波的过程噪声协方差矩阵采用固定值，无法根据前端预校正质量的下降自适应调整滤波器对预测模型的信任权重，导致错误的匹配结果持续污染后续帧的状态估计，最终形成图像姿态连续漂移。

4、为此，本发明提出了一种双轮侦查球图像反馈姿态校正系统。

技术实现思路

1、本发明的目的在于提供一种双轮侦查球图像反馈姿态校正系统，解决现有技术中双轮侦查球在复杂工作环境中因越障、单轮打滑或受到瞬时冲击时导致轮速与imu数据冲突进而造成图像预校正失真、特征匹配偏移和姿态估计连续漂移问题。

2、为实现上述目的，本发明提供如下技术方案：一种双轮侦查球图像反馈姿态校正系统，包括：多源传感采集模块，用于：采集双轮侦查球行走中的环境图像序列、惯性测量数据及轮速数据；时间同步与状态递推模块，用于：以环境图像序列采集时刻为基准对惯性测量数据进行时间插值校准获得对齐的姿态数据，递推计算每帧图像采集时刻的先验位姿估计值及对应误差协方差矩阵；数据一致性检验与预校正模块，用于：根据轮速数据计算当前帧相对上一帧的平面位姿变化量预测值，根据时间同步与状态递推模块输出的先验位姿估计值得到惯导位姿变化量预测值，将平面位姿变化量预测值与惯导位姿变化量预测值进行一致性检验，根据校验结果生成供特征提取使用的预校正图像；视觉观测与滤波更新模块，用于：从预校正图像中提取相邻帧特征点匹配关系构建视觉重投影误差观测方程，根据先验位姿估计值的误差协方差矩阵及重投影误差观测方程进行滤波更新，得到当前帧位姿估计值，并将当前帧位姿估计值及对应的误差协方差矩阵反馈至时间同步与状态递推模块；图像姿态校正模块，用于：根据当前帧位姿估计值对当前帧环境图像进行几何变换生成校正后的输出图像。

3、进一步地，所述多源传感采集模块采集的环境图像序列、惯性测量数据及轮速数据均搭载统一时间戳，以环境图像序列中每帧环境图像的采集触发信号作为全系统时间对齐的基准。

4、进一步地，所述时间同步与状态递推模块，以每帧环境图像的采集时间戳为基准，对惯性测量数据的角速度、姿态角数据进行插值校准，得到与每帧环境图像采集时刻对齐的姿态数据，根据姿态数据，通过惯性导航递推算法，递推得到每帧环境图像采集时刻的先验位姿估计值，并基于惯性测量噪声模型经误差传播计算对应的误差协方差矩阵。

5、进一步地，所述数据一致性检验包括：将所述平面位姿变化量预测值与所述惯导位姿变化量预测值转换至同一坐标系下，计算两者位移偏差量与航向角偏差量，当位移偏差量与航向角偏差量都未超过预设阈值时判定一致性检验通过，否则判定一致性检验未通过。

6、进一步地，所述位移偏差量根据轮速数据推算的平面位移与惯导位姿变化量预测值中的平面位移分量之差确定，所述航向角偏差量根据轮速数据推算的航向角变化量与惯导位姿变化量预测值中的航向角分量之差确定。

7、进一步地，所述一致性检验通过时，数据一致性检验与预校正模块融合平面位姿变化量预测值与惯导位姿变化量预测值得到全维度位姿变化量预测值，根据全维度位姿变化量预测值，基于深度相机提供的各像素深度值将当前帧图像中的每个像素点反投影至相机坐标系下的三维空间点，对三维空间点进行刚体变换后再投影回图像平面，逐像素生成预校正图像，以补偿相邻帧间平面运动与三维姿态变化共同导致的图像刚体变形。

8、进一步地，所述一致性检验未通过时，数据一致性检验与预校正模块仅根据惯导位姿变化量预测值中的旋转分量求解图像单应性变换矩阵，对当前帧环境图像执行反向像素映射变换，补偿相邻帧间三维姿态变化导致的图像刚体变形，生成预校正图像。

9、进一步地，所述视觉观测与滤波更新模块分别从当前帧预校正图像与上一帧预校正图像中提取角点特征及对应描述子，通过特征描述子匹配生成初步匹配点对，并采用随机抽样一致算法剔除误匹配点对，得到稳定的相邻帧特征点匹配关系。

10、进一步地，所述视觉观测与滤波更新模块基于相邻帧特征点匹配关系，以相邻帧匹配特征点的像素坐标偏差为观测量，根据先验位姿估计值，结合相机内参构建视觉重投影误差观测方程，并对重投影误差观测方程进行线性化处理得到观测矩阵与观测噪声矩阵。

11、进一步地，所述视觉观测与滤波更新模块采用扩展卡尔曼滤波执行状态更新，根据先验位姿估计值对应的误差协方差矩阵、观测矩阵与观测噪声矩阵计算卡尔曼增益，修正先验位姿估计值及误差协方差矩阵，得到更新后的当前帧位姿估计值及误差协方差矩阵，同时计算当前帧视觉重投影残差统计特性，根据统计特性调整惯性导航递推算法中的过程噪声协方差矩阵，以调节下一轮滤波融合中预测置信度，将当前帧位姿估计值及对应的误差协方差矩阵反馈至时间同步与状态递推模块，作为下一帧先验位姿估计值递推的初始条件。

12、与现有技术相比，本发明的有益效果是：本发明提出的一种双轮侦查球图像反馈姿态校正系统，现有技术中，双轮侦查球在越障、单轮打滑或受到瞬时冲击时，轮速推算的位移量因轮地接触失效而失真，与imu真实姿态变化产生冲突，而现有技术未能在图像预校正阶段主动检测多源传感数据冲突，直接将轮速推算的位移量与imu测量值融合求解单一单应变换矩阵，导致预校正图像产生局部拉伸和几何扭曲，进而造成特征匹配偏移和姿态估计连续漂移；而本发明通过数据一致性检验与预校正模块，将轮速推算的平面位姿变化量与惯导位姿变化量进行一致性检验，根据校验结果分支切换预校正策略，检验通过时融合全维度运动信息，检验未通过时主动弃用轮速数据中不可信的平移分量，仅保留惯导旋转分量进行预校正，避免矛盾数据注入单应矩阵导致图像局部拉伸，同时通过视觉观测与滤波更新模块根据视觉重投影残差统计特性动态调整过程噪声协方差矩阵，将预校正质量量化为滤波器信任度参数，并在预校正质量下降时主动降低对预测模型的依赖、增强视觉观测的修正权重，最终将更新后的过程噪声协方差矩阵、位姿估计值及误差协方差矩阵共同反馈至时间同步模块，系统性地解决了因轮速与imu数据冲突引发的图像局部拉伸、特征匹配偏移和姿态连续漂移问题。