本发明属于机器人定位和建图，具体涉及一种基于vins-mono的水下核电环境移动机器人slam方法。
背景技术：
1、核能源作为可再生能源的一种，具有低碳、高效、清洁等优点，但是核能发电会产生放射性物质，因此确保核安全是发展核电的前提，而防止放射性物质外泄是保障核安全的根本。核电站设计了三道屏障防止放射性物质外泄，分别是核燃料组件包壳、一回路压力边界与安全壳，尤其是第一和第二道屏障，是保障核电站安全高效运行的基础。在核电站的寿期内，需要定期进行换料和设备维修，确保核燃料组件和一回路主设备正常工作。核电站一回路检修作业面临高辐照、硼酸水、空间狭小等复杂恶劣环境，检修人员难以高效完成作业任务。采用移动机器人可以实现少人化、自动化、智能化高质量检修作业，保障核电站的安全稳定运行。而定位和地图构建(simultaneous localization and mapping，slam)作为移动机器人自主导航的关键技术，对提高一回路检修作业质量至关重要。
2、vins-mono作为优秀的视觉惯性slam算法，在陆地和空中都能够为机器人提供高精度的定位。该系统在视觉前端提取shi tomasi角点并使用光流法对特征点进行追踪，利用预积分数据和滑窗内视觉帧间约束完成初始化，将滑窗中每一帧的位姿、速度、惯性测量单元(inertial measurement unit，imu)的零偏以及地图点的逆深度放入优化系统中执行非线性优化，同时系统使用了回环检测来减少累计误差。由于核电站一回路检修作业环境复杂，面临高辐照、狭小受限空间、水下弱硼酸腐蚀等复杂环境，同时在没有自然光的核电站安全壳内，检修作业环境较为昏暗，一般在环境中布置人工光源，导致相机获取的图像亮度不均匀，无法提取高质量的特征点。同时，由于水下光线和颜色衰减严重，相机有效工作距离缩短，获取的往往是无纹理图像，图像中可供提取且能用于定位的有效特征点数量急剧减少，导致移动机器人自主定位极为困难。
技术实现思路
1、针对现有技术的不足，本发明拟解决的技术问题是，提供一种基于vins-mono的水下核电环境移动机器人slam方法。
2、本发明解决所述技术问题采用如下的技术方案：
3、一种基于vins-mono的水下核电环境移动机器人slam方法，其特征在于，该方法包括以下步骤：
4、s1：采集水下环境图像，并进行预处理；将图像中的高亮区域设置掩码，对图像中掩码以外的区域进行特征点检测，通过光流追踪得到匹配特征点；
5、s2：利用匹配特征点和imu测量数据，进行旋转平移解耦的视觉惯导初始化；
6、imu预积分遵循式(1)～(3)：
7、
8、
9、
10、式中，分别为i、j时刻imu坐标系下的位置，分别为初始、i、j时刻imu坐标系下的速度，为imu坐标系下的重力向量，δtij为从i时刻到j时刻的时间间隔，分别为i、j时刻imu坐标系下的旋转矩阵，分别为i到j时刻平移、速度预积分量；
11、通过式(4)～(6)对imu测量数据进行预积分，得到平移、速度以及旋转预积分量；使用陀螺仪零偏更新旋转预积分量表示为式(7)；
12、
13、
14、
15、
16、式中，为i到j时刻旋转预积分量，为使用陀螺仪零偏bg更新后的i到j时刻旋转预积分量，分别为i到f、i到k时刻imu坐标系下的旋转矩阵，分别为f、k时刻imu坐标系下的加速度，δt为时间间隔，为k时刻imu坐标系下的角速度，为雅可比矩阵，exp(·)为从李代数到李群的映射；
17、将旋转预积分量从imu坐标系转换到相机坐标系，得到相机坐标系下相邻关键帧之间的旋转矩阵为：
18、
19、式中，为相机坐标系下从i时刻到j时刻的旋转矩阵，rbc为相机与imu之间的旋转外参，为相机与imu之间旋转外参的转置；
20、根据匹配特征点以及由式(4)、(8)解算得到的初始位姿，通过三角测量计算相邻关键帧之间共视的多个3d点，每个3d点与相邻关键帧的相机中心组成一个平面，则平面法向量表示为：
21、
22、式中，nu为第u个3d点对应的平面法向量，为从i时刻的相机中心指向第u个3d点的单位向量，为从j时刻的相机中心指向第u个3d点的单位向量；
23、令矩阵n＝[n1,…,nu,…,nn]，矩阵m＝nnt，为矩阵m的最小特征值，则相机坐标系下相邻关键帧之间的旋转矩阵的迭代值为：
24、
25、i、j时刻之间的矩阵m表示为：
26、
27、结合式(7)、(8)、(11)，i、j时刻之间的矩阵m进一步表示为：
28、
29、迭代求解式(12)，得到陀螺仪零偏的迭代值为：
30、
31、其中，μ为有共同特征的关键帧对集合；
32、根据陀螺仪零偏的迭代值通过式(6)、(7)计算滑动窗口内所有相邻关键帧之间的旋转矩阵；将第一张关键帧图像视为世界坐标系，使用线性全局平移约束求解关键帧图像在世界坐标系中的位置；再求解视觉惯导系统的初始状态变量为i、k时刻imu坐标系下的速度，s为尺度，为第0帧相机坐标系下的重力向量；
33、s3：对视觉惯导初始化得到的各参数进行非线性优化，得到移动机器人的位置和姿态；在系统运行过程中，记录滑动窗口内的图像中特征点跟踪次数大于等于两次的次数nt，以及图像中所有特征点的总跟踪次数ns，计算比例；
34、pt＝nt/nts (21)
35、ps＝ns/nss (22)
36、式中，nts为滑动窗口内的图像中提取的特征点总个数，nss为滑窗内的图像中所有特征点的最大跟踪次数，pt为特征点跟踪次数大于等于两次的比例，ps为所有特征点总跟踪次数的比例；
37、若pt小于当前关键帧处于恶劣环境的判断阈值，且ps小于滑动窗口时间段内移动机器人处于恶劣环境的判断阈值，则根据式(23)更新视觉约束的信息矩阵；
38、
39、式中，p、分别为更新前、后的视觉约束的信息矩阵。
40、与现有技术相比，本发明的有益效果是：
41、第一，对水下环境图像进行预处理，提取高亮区域，对图像中的高亮区域进行掩码处理，从掩码以外的区域提取特征点并进行特征点匹配，提升了水下环境图像特征点的提取质量。
42、第二，在初始化部分提出了一种旋转平移解耦的视觉惯导初始化方法，首先，利用视觉与惯导信息解出陀螺仪零偏，计算出滑动窗口内各关键帧的旋转矩阵与平移向量；其次，利用位移、速度预积分量求解速度、尺度和重力矢量，极大提升了视觉惯导初始化的速度与准确性。
43、第三，提出了一种信息矩阵动态调整策略，利用特征点跟踪次数判断视觉信息的可信度，动态调整视觉约束的信息矩阵，能够更好的适用于核电站一回路的复杂作业环境并拥有较好的定位精度。
1.一种基于vins-mono的水下核电环境移动机器人slam方法，其特征在于，该方法包括以下步骤：
2.根据权利要求1所述的基于vins-mono的水下核电环境移动机器人slam方法，其特征在于，步骤s2中，imu预积分观测模型的平移向量与速度计算式为：
3.根据权利要求1或2所述的基于vins-mono的水下核电环境移动机器人slam方法，其特征在于，所述预处理包括：遍历图像的所有像素点，若像素点的灰度值大于高亮阈值，则将像素点位置记录为高亮区域；使用中值滤波去除图像中的噪点，使用自适应直方图对去噪后的图像进行均衡化处理。
