基于多传感器融合的水下三维重建方法、装置及其设备与流程
本发明涉及水下同步定位与建图,具体涉及一种基于多传感器融合的水下三维重建方法、装置及其设备。
背景技术:
1、随着陆地矿产资源的日益枯竭,人们逐渐将目光转向海洋。深海蕴含着丰富的矿产资源,吸引着各国竞相快速发展深海科技,推进深海矿产资源开发利用。无人自主水下机器人(auv)是进行深海资源勘测开发的主要装备。auv要实现水下自主探测需要实时获取自身的位姿以及周围的环境信息,因此研究水下同步定位与建图(slam)技术具有重要意义,也是目前研究的热点和难点。水下slam常采用双目相机作为传感器,基于双目相机进行的水下定位往往精度不足,存在定位失败造成实时的三维重建无法继续的问题。
技术实现思路
1、为解决上述技术问题,本技术实施例提供一种基于多传感器融合的水下三维重建方法、装置及其设备,可提高水下定位精度,降低定位失败风险并实现水下三维重建。
2、本技术实施例第一方面提供一种基于多传感器融合的水下三维重建方法,包括:
3、水下机器人的处理器获取存储器中的初始三维地图、当前相机采样时刻的当前dvl测量速度值、当前imu数据、当前双目图像特征点,获取当前相机采样时刻相邻的多个历史相机采样时刻的水下机器人历史状态参数、历史双目图像特征点,获取多个历史imu数据和历史imu采样时刻的历史dvl测量速度值,其中,多个历史imu采样时刻介于最早的历史相机采样时刻和当前相机采样时刻之间,水下机器人基于dvl传感器采集dvl测量速度值,水下机器人基于imu传感器采集当前imu数据,水下机器人基于双目相机采集当前双目图像特征点,水下机器人历史状态参数、历史双目图像特征点、历史imu数据和历史dvl测量速度值为存储与存储器的既有数据;
4、处理器对于任一历史相机采样时刻,根据历史状态参数、预设状态参数初值、历史相机采样时刻至当前相机采样时刻的历史imu数据和历史dvl测量速度值预积分结果,得到当前相机采样时刻相对于历史相机采样时刻的位移残差;
5、处理器根据水下机器人的预设状态参数初值和当前dvl测量速度值确定的当前速度残差、历史状态参数和历史dvl测量速度值确定的历史速度残差、以及位移残差,得到历史相机采样时刻的dvl残差项;
6、处理器对于任一历史相机采样时刻,根据历史状态参数、预设状态参数初值、历史imu数据和当前imu数据确定imu残差项,并根据历史状态参数、预设状态参数初值、当前双目图像特征点和历史双目图像特征点确定相机残差项;
7、处理器根据各历史相机采样时刻的dvl残差项、imu残差项和相机残差项之和得到目标函数,以目标函数最小化为目标,对预设状态参数初值进行迭代优化,求解得到当前相机采样时刻的水下机器人当前状态参数,并根据当前状态参数和当前双目图像特征点更新初始三维地图,得到水下机器人的实时三维地图。
8、在本技术的一些实施例中,根据历史状态参数、预设状态参数初值、历史相机采样时刻至当前相机采样时刻的历史imu数据和历史dvl测量速度值预积分结果,得到当前相机采样时刻相对于历史相机采样时刻的位移残差包括:将历史状态参数、预设状态参数初值作为dvl位移测量模型的输入,得到第一位移;对于历史相机采样时刻至当前相机采样时刻的每个历史imu采样时刻,根据历史相机采样时刻至历史imu采样时刻的各历史imu数据,确定历史相机采样时刻至当前相机采样时刻的imu旋转预积分;根据各imu旋转预积分、dvl坐标系到imu坐标系的旋转矩阵和历史dvl测量速度值进行预积分,得到预积分结果,将预积分结果作为第二位移;根据第一位移和第二位移之差,得到当前相机采样时刻相对于历史相机采样时刻的位移残差。
9、在本技术的一些实施例中,任意两dvl测量速度值的采样周期大于任意两相机采样时刻的采样周期;第二位移为:
10、;
11、其中,表示imu坐标系下最早的历史相机采样时刻至当前相机采样时刻的第二位移,表示imu坐标系下最早的历史相机采样时刻至历史imu采样时刻的imu旋转预积分,表示dvl坐标系到imu坐标系的旋转矩阵,表示imu坐标系下最早的历史相机采样时刻对应的历史dvl测量速度值,表示imu数据更新时间间隔。
12、在本技术的一些实施例中,dvl残差项为:
13、;
14、;
15、;
16、;
17、其中,表示dvl残差项,表示历史速度残差,表示当前速度残差,表示位移残差,表示dvl观测模型,表示历史相机采样时刻的历史状态参数,表示当前相机采样时刻的预设状态参数初值,表示历史相机采样时刻到前相机采样时刻的dvl观测量,表示dvl速度测量模型,表示历史相机采样时刻的dvl观测量,表示当前相机采样时刻的dvl观测量,表示dvl位移测量模型,表示历史相机采样时刻的历史dvl测量速度值,表示基于dvl速度测量模型确定的历史相机采样时刻的dvl速度预测量,表示当前相机采样时刻的当前dvl测量速度值,表示基于dvl速度测量模型确定的当前相机采样时刻的dvl速度预测量,表示基于预设状态参数初值和历史相机采样时刻的历史状态参数确定的第一位移,表示预积分结果。
18、在本技术的一些实施例中,获取初始三维地图、当前相机采样时刻的当前dvl测量速度值、当前imu数据、当前双目图像特征点,获取当前相机采样时刻相邻的多个历史相机采样时刻的水下机器人历史状态参数、历史双目图像特征点,获取多个历史imu采样时刻的历史imu数据和历史dvl测量速度值包括:获取水下机器人进行三维重建时基于双目相机拍摄的首帧双目图像,并根据首帧双目图像提取多个第一特征点对;根据双目相机的标定参数对各第一特征点对进行三角测量,得到多个初始三维地图点;在多个初始三维地图点的总数量大于或等于预设地图点数量的情况下,基于各初始三维地图点得到初始三维地图;在得到初始三维地图的情况下,获取当前相机采样时刻的当前dvl测量速度值、当前imu数据、当前双目图像特征点,获取当前相机采样时刻相邻的多个历史相机采样时刻的水下机器人历史状态参数、历史双目图像特征点,获取多个历史imu采样时刻的历史imu数据和历史dvl测量速度值。
19、在本技术的一些实施例中,根据双目相机的标定参数对各特征点对进行三角测量,得到多个初始三维地图点包括:根据双目相机的标定参数对各第一特征点对进行三角测量,得到与各第一特征点对相对应的多个第一初始三维地图点;在多个第一初始三维地图点的总数量大于或等于预设地图点数量的情况下,将多个第一初始三维地图点作为初始三维地图点;在多个第一初始三维地图点的总数量小于预设地图点数量的情况下,获取下一相机采样时刻的下一帧双目图像、以及水下机器人基于声纳在下一相机采样时刻采集的声纳图像;根据下一帧双目图像提取多个第二特征点对,并根据双目相机的标定参数对各第二特征点对进行三角测量,得到多个第二初始三维地图点;根据声纳图像提取多个第三特征点,根据各第三特征点对应的声纳测距信息和角度信息确定声纳坐标系下的多个第三那特征点坐标;根据声纳和双目相机的坐标系变换矩阵将多个第三那特征点坐标转换至双目相机坐标系下的特征点坐标,得到多个第三初始三维地图点;根据多个第二初始三维地图点和多个第三初始三维地图点得到多个初始三维地图点。
20、在本技术的一些实施例中,在多个初始三维地图点包括多个第二初始三维地图点和多个第三初始三维地图点的情况下,三维重建方法还包括:在得到初始三维地图的情况下,获取当前相机采样时刻的当前声纳影像特征点,各历史相机采样时刻的历史声纳图像特征点;根据各历史相机采样时刻的dvl残差项、imu残差项和相机残差项之和得到目标函数,以目标函数最小化为目标,对预设状态参数初值进行迭代优化,求解得到当前相机采样时刻的水下机器人当前状态参数,并根据当前状态参数和当前双目图像特征点更新初始三维地图,得到水下机器人的实时三维地图包括:对于任一历史相机采样时刻,根据历史状态参数、预设状态参数初值、当前声纳影像特征点和历史声纳图像特征点确定声纳残差项;根据各历史相机采样时刻的dvl残差项、imu残差项、相机残差项和声纳残差项之和得到目标函数,以目标函数最小化为目标,对预设状态参数初值进行迭代优化,求解得到当前相机采样时刻的水下机器人当前状态参数,并根据当前状态参数和当前双目图像特征点更新初始三维地图,得到水下机器人的实时三维地图。
21、在本技术的一些实施例中,目标函数为:
22、;
23、其中,表示得到当前状态参数时的目标函数值,表示所有历史相机采样时刻和当前相机采样时刻的状态参数集合,表示第个历史相机采样时刻和当前相机采样时刻属于相机采样时刻集合,表示dvl残差项,表示dvl观测模型,表示历史相机采样时刻的历史状态参数,表示当前相机采样时刻的预设状态参数初值,表示历史相机采样时刻到前相机采样时刻的dvl观测量,表示imu残差项,表示imu观测模型,表示历史相机采样时刻到前相机采样时刻的历史imu数据和当前imu数据,表示声纳残差项,表示声纳观测模型,表示历史相机采样时刻到前相机采样时刻的历史声纳图像特征点和当前声纳影像特征点,表示相机残差项,表示相机观测模型,表示历史双目图像特征点。
24、本技术实施例第二方面提供一种基于多传感器融合的水下三维重建装置,三维重建装置包括:数据获取模块,用于获取初始三维地图、当前相机采样时刻的当前dvl测量速度值、当前imu数据、当前双目图像特征点,获取当前相机采样时刻相邻的多个历史相机采样时刻的水下机器人历史状态参数、历史双目图像特征点,获取多个历史imu数据和历史imu采样时刻的历史dvl测量速度值,其中,多个历史imu采样时刻介于最早的历史相机采样时刻和当前相机采样时刻之间;位移残差确定模块,用于对于任一历史相机采样时刻,根据历史状态参数、预设状态参数初值、历史相机采样时刻至当前相机采样时刻的历史imu数据和历史dvl测量速度值预积分结果,得到当前相机采样时刻相对于历史相机采样时刻的位移残差;dvl残差项确定模块,用于根据水下机器人的预设状态参数初值和当前dvl测量速度值确定的当前速度残差、历史状态参数和历史dvl测量速度值确定的历史速度残差、以及位移残差,得到历史相机采样时刻的dvl残差项;imu残差项和相机残差项确定模块,用于对于任一历史相机采样时刻,根据历史状态参数、预设状态参数初值、历史imu数据和当前imu数据确定imu残差项,并根据历史状态参数、预设状态参数初值、当前双目图像特征点和历史双目图像特征点确定相机残差项;三维地图更新模块,用于根据各历史相机采样时刻的dvl残差项、imu残差项和相机残差项之和得到目标函数,以目标函数最小化为目标,对预设状态参数初值进行迭代优化,求解得到当前相机采样时刻的水下机器人当前状态参数,并根据当前状态参数和当前双目图像特征点更新初始三维地图,得到水下机器人的实时三维地图。
25、本技术实施例第三方面提供一种电子设备,包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序,处理器执行计算机程序时实现如本技术第一方面提供的基于多传感器融合的水下三维重建方法。
26、本发明的有益效果是,本技术实施例提供的基于多传感器融合的水下三维重建方法首先获取初始三维地图,当前相机采样时刻的当前dvl测量速度值、当前imu数据、当前双目图像特征点,以及与当前相机采样时刻相邻的多个历史相机采样时刻的水下机器人历史状态参数、历史双目图像特征点,多个历史imu数据和历史imu采样时刻的历史dvl测量速度值。为实现融合dvl测量速度值、imu数据和双目图像特征点,进行局部跟踪,进而实现实时的三维重建。对于任一历史相机采样时刻,本技术实施例提供的基于多传感器融合的水下三维重建方法根据历史状态参数、预设状态参数初值、历史相机采样时刻至当前相机采样时刻的历史imu数据和历史dvl测量速度值预积分结果,得到当前相机采样时刻相对于该历史相机采样时刻的位移残差。根据水下机器人的预设状态参数初值和当前dvl测量速度值可确定当前速度残差,根据历史状态参数和历史dvl测量速度值可确定历史速度残差。结合上述的位移残差、当前速度残差和历史速度残差可得到历史相机采样时刻的dvl残差项。对于任一历史相机采样时刻根据历史状态参数、预设状态参数初值、历史imu数据和当前imu数据确定imu残差项,根据历史状态参数、预设状态参数初值、当前双目图像特征点和历史双目图像特征点确定相机残差项。基于上述不同历史相机采样时刻的dvl残差项、imu残差项和相机残差项之和可得到目标函数,以该目标函数最小化为目标调整预设状态参数初值,从而得到当前相机采样时刻的水下机器人当前状态参数,基于该当前状态参数可得知水下机器人的当前位置,从而结合当前双目图像特征点对初始三维地图进行更新,得到水下机器人的三维地图。因此,本技术实施例提供的基于多传感器融合的水下三维重建方法可结合dvl传感器测得的dvl测量速度值、imu传感器测得的imu数据和双目相机拍摄的双目图像进行局部跟踪,确定水下机器人实时的状态参数,从而基于实时的状态参数和双目图像特征点更新三维地图,实现基于多传感器融合的局部跟踪和三维重建,提高水下定位精度,降低定位失败风险,以实现实时的水下三维重建。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com