多目立体视觉与惯性导航系统及相对位姿参数确定方法与流程
本申请涉及多目立体视觉与惯性导航领域，尤其涉及一种多目立体视觉与惯性导航系统及相对位姿参数确定方法。
背景技术：
基于视觉的移动测量系统，是指将多目立体视觉与惯性导航系统刚性固定在移动平台上(包括手持式、背包式、车载式、机载式等形式的移动平台)，进行空间位置信息测量的系统。其整体测量精度的核心是多目立体视觉与惯性导航系统的相对位姿参数的准确标定。
当前，多目立体视觉(multi-viewstereovision，简称为msv)与惯性导航系统(inertialnavigationsystem,简称为ins)的相对位姿参数标定方法大多采用最小二乘法，采用该方法存在测量精度不均匀和鲁棒性较差的技术问题。
技术实现要素：
为了解决上述技术问题或者至少部分地解决上述技术问题，本申请提供了一种多目立体视觉与惯性导航系统及相对位姿参数确定方法。
第一方面，本申请提供了一种多目立体视觉与惯性导航系统的相对位姿参数确定方法，包括：通过多目立体视觉系统的多个相机获取多个标定点的测量像素坐标，通过惯性导航系统获取多个标定点的测量惯性坐标；使用多个标定点的测量像素坐标，基于多目立体视觉系统的相机畸变系数和深度测量误差约束，确定多个标定点的修正相机坐标；基于惯性导航系统的误差修正多个标定点的测量惯性坐标，得到多个标定点的修正惯性坐标；基于多个标定点的修正惯性坐标和修正相机坐标，确定多目立体视觉与惯性导航系统的相对位姿参数，其中，相对位置参数包括：相机坐标变换到惯性坐标的旋转参数和相机坐标变换到惯性坐标的平移参数。
在一些实施例中，相对位姿参数还包括：相机坐标到惯性坐标的比例因子。
在一些实施例中，使用多个标定点的测量像素坐标，基于多目立体视觉系统的相机畸变系数和深度测量误差约束，确定多个标定点的修正相机坐标，包括：基于多目立体视觉系统的相机畸变系数修正多个标定点的测量像素坐标，得到多个标定点的修正像素坐标；基于标定点的修正像素坐标确定对应标定点的测量深度信息；基于标定点的深度测量误差约束修正测量深度信息，得到对应标定点的修正深度信息；基于标定点的修正像素坐标和修正深度信息将对应标定点的像素坐标转换为相机坐标，得到多个标定点的修正相机坐标。
在一些实施例中，相机畸变系数包括：径向畸变系数、切向畸变系和薄棱镜畸变系数；其中，基于多目立体视觉系统的相机畸变系数修正多个标定点的测量像素坐标，得到多个标定点的修正像素坐标，包括：基于径向畸变系数确定每个标定点的测量像素坐标的径向畸变参数；基于切向畸变系数确定每个标定点的测量像素坐标的切向畸变参数；基于薄棱镜畸变系数确定每个标定点的测量像素坐标的薄棱镜畸变参数；基于标定点的薄棱镜畸变参数、切向畸变参数和径向畸变参数修正对应标定点的测量像素坐标，得到多个标定点的修正像素坐标。
在一些实施例中，相机坐标变换到惯性坐标满足如下约束条件：pins＝λ(r*pmsv+t)，其中，pins为标定点的修正惯性坐标，pmsv为标定点的修正相机坐标，r为旋转参数，t为平移参数，λ为比例因子；其中，基于多个标定点的修正惯性坐标和修正相机坐标，按照约束条件确定多目立体视觉与惯性导航系统的相对位姿参数。
在一些实施例中，确定比例因子包括：基于多个标定点的修正相机坐标、修正惯性坐标和传感器相对位姿转换关系，通过最小二乘法确定比例因子。
在一些实施例中，确定旋转参数包括：基于多个标定点的修正相机坐标和修正惯性坐标，通过四元数法确定空间旋转矩阵；基于空间旋转矩阵的最大特征值对应的特征向量，确定旋转参数。
在一些实施例中，确定平移参数包括：基于比例因子和旋转参数，按照约束条件确定平移参数。
第二方面，本申请提供了一种多目立体视觉与惯性导航系统，包括：多目立体视觉系统，包括多个相机；惯性导航系统；以及存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序；计算机程序被处理器执行时实现多目立体视觉与惯性导航系统的相对位姿参数确定方法的步骤。
第三方面，本申请提供了一种计算机可读存储介质，计算机可读存储介质上存储有多目立体视觉与惯性导航系统的相对位姿参数确定程序，多目立体视觉与惯性导航系统的相对位姿参数确定程序被处理器执行时实现多目立体视觉与惯性导航系统的相对位姿参数确定方法的步骤。
本申请实施例提供的上述技术方案与现有技术相比具有如下优点：
本申请实施例提供的该方法，在确定多目立体视觉与惯性导航系统的相对位姿参数时，纳入了多目立体视觉深度测量误差、相机畸变误差、惯性导航系统误差的影响，解决了空间点精度分布不均以及惯性导航系统缺乏自主性误差补偿的问题，提高了多目立体视觉与惯性导航系统相对位姿参数的标定结果。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的多目立体视觉与惯性导航系统一种实施方式的硬件结构示意图；
图2为本申请实施例提供的多目立体视觉与惯性导航系统的相对位姿参数确定方法一种实施方式的流程图；
图3为本申请实施例提供的确定多个标定点的修正相机坐标的方法一种实施方式的流程图；以及
图4为本申请实施例提供的修正相机畸变误差的方法一种实施方式的流程图。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
在后续的描述中，使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身没有特定的意义。因此，“模块”、“部件”或“单元”可以混合地使用。
如图1所示，本发明实施例中提供的多目立体视觉与惯性导航系统100包括多目立体视觉系统101和惯性导航系统102。多目立体视觉与惯性导航系统100在基于时间同步的条件下，可提供移动测量平台连续不断的瞬时时刻位置姿态信息。
在一些实施例中，多目立体视觉系统101可包括多个相机(图中未示出)，一般地，包括两个及两个以上的相机。惯性导航系统102可包括陀螺仪和加速度传感器(图中未示出)等。本申请实施例中提供的多目立体视觉与惯性导航系统100或者多目立体视觉与惯性导航系统100耦合的设备、装置或系统等可以包括：存储器103、处理器104、以及电源105等部件。
存储器103可用于存储软件程序以及各种数据。存储器103可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器103可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
处理器104是控制中心，利用各种接口和线路连接整个可穿戴设备的各个部分，通过运行或执行存储在存储器103内的软件程序和/或模块，以及调用存储在存储器103内的数据，执行多目立体视觉与惯性导航系统100的各种功能和处理数据，从而对多目立体视觉与惯性导航系统100进行整体控制。处理器104可包括一个或多个处理单元。
应当理解，图1仅作为对多目立体视觉与惯性导航系统100的示例性说明，其还可包括一个或多个其他部件，例如传感器、通信接口，及与之相关的电路单元，本实施例对此不做赘述。
本申请实施例中提供的多目立体视觉与惯性导航系统的相对位姿参数确定方法，在确定多目立体视觉与惯性导航系统100的相对位姿参数时，纳入了多目立体视觉深度测量误差、相机畸变误差、惯性导航系统误差的影响，解决了空间点精度分布不均以及惯性导航系统缺乏自主性误差补偿的问题，优化了多目立体视觉与惯性导航系统相对位姿参数的标定结果。
如图2所示，本申请实施例中提供的多目立体视觉与惯性导航系统的相对位姿参数确定方法包括步骤s202至步骤s208。
步骤s202，通过多目立体视觉系统101的多个相机获取多个标定点的测量像素坐标，通过惯性导航系统102获取多个标定点的测量惯性坐标。
在本实施例中，测量像素坐标多目立体视觉系统101的多个相机中多个相机获取的标定点的像素坐标。像素坐标为标定点在图像坐标系中的坐标。测量像素坐标具有相机畸变误差。
在本实施例中，测量惯性坐标为惯性导航系统102获取的惯性坐标，测量惯性坐标为标定点在惯性导航坐标系中的坐标。测量惯性坐标具有惯性导航系统误差。
步骤s204，使用多个标定点的测量像素坐标，基于多目立体视觉系统101的相机畸变系数和深度测量误差约束，确定多个标定点的修正相机坐标。
在本实施例中，标定点的修正相机坐标为多目立体视觉系统101的主相机坐标系中的坐标。未经误差修正的相机坐标可称为测量相机坐标。
在一些实施例中，深度测量误差约束为深度测量误差加权约束。深度测量误差加权约束中，每个标定点分配偏差权重，所有标定点的偏差权重之和为1。优选地，深度测量误差越大的标定点分配的偏差权重越小，深度测量误差越小的标定点分配的偏差权重越大。
步骤s206，基于惯性导航系统102的误差修正多个标定点的测量惯性坐标，得到多个标定点的修正惯性坐标。
在一些实施例中，惯性导航系统102的误差可包括：加速度计的误差和/或陀螺仪的误差。
步骤s208，基于多个标定点的修正惯性坐标和修正相机坐标，确定多目立体视觉与惯性导航系统的相对位姿参数。
其中，确定的相对位置参数包括：相机坐标变换到惯性坐标的旋转参数和相机坐标变换到惯性坐标的平移参数。
在一些实施例中，上述相对位姿参数还包括：相机坐标到惯性坐标的比例因子，比例因子为变量。上述步骤s208还包括，确定相机坐标到惯性坐标的比例因子。
在另一些实施例中，相机坐标转换到惯性坐标的比例因子可为常数，优选地，可为大于且接近于1的常数。上述步骤s208中，可使用该常数进行相关计算。
在一些实施例中，相机坐标变换到惯性坐标满足如下约束条件：pins＝λ(r*pmsv+t)，其中，pins为标定点的修正惯性坐标，pmsv为标定点的修正相机坐标，r为旋转参数，t为平移参数，λ为比例因子；其中，上述步骤s208中，基于多个标定点的修正惯性坐标和修正相机坐标，按照该约束条件确定多目立体视觉与惯性导航系统的相对位姿参数。在一些实施例中，该约束条件中，比例因子为变量。在另一些实施例中，比例因子为常量。
进一步的，标定点的修正惯性坐标和标定点的修正相机坐标为输入，旋转参数、平移参数以及比例因子为标定输出。
应当理解，图2中虽然对步骤进行了编号，但这些编号并不是对步骤之间执行顺序的限定，实际上，在本实施例中，步骤可按照任意顺序执行，例如，步骤s204和步骤s206可同时执行，也可先执行步骤s206再执行步骤s204，本实施例对此不做限定。
在本实施例中，可采用多种方法确定多个标定点的修正相机坐标，只要其纳入了相机畸变误差和深度测量误差的影响都是可行的。
作为示例性说明，在一些实施例中，如图3所示，上述步骤s204中，使用多个标定点的测量像素坐标，基于多目立体视觉系统的相机畸变系数和深度测量误差约束，确定多个标定点的修正相机坐标，可包括步骤s302至步骤s308。
步骤s302，基于多目立体视觉系统101的相机畸变系数修正多个标定点的测量像素坐标，得到多个标定点的修正像素坐标。
在本实施例中，相机畸变系数可包括径向畸变系数、切向畸变系和薄棱镜畸变系数中至少之一或者任意组合。优选地，可基于径向畸变系数、切向畸变系和薄棱镜畸变系数修正测量像素坐标，得到修正像素坐标。但本实施例并不限于此。
在本实施例中，标定点的修正像素坐标，包括多目立体视觉系统101的多个相机中每个相机对应的修正像素坐标。
步骤s304，基于标定点的修正像素坐标确定对应标定点的测量深度信息。
在本实施例中，可基于多个相机对应的标定点的修正像素坐标，利用多目视觉测距原理，例如三角定位法等，确定标定点的测量深度信息。由此，可以得到多个标定点的测量深度信息。测量深度信息为基于修正像素坐标确定得到的深度信息，其未经深度测量误差约束修正。
步骤s306，基于标定点的深度测量误差约束修正测量深度信息，得到对应标定点的修正深度信息。
在本实施例中，通过上述步骤s306可得到多个标定点的修正深度信息。
步骤s308，基于标定点的修正像素坐标和修正深度信息将对应标定点的像素坐标转换为相机坐标，得到多个标定点的修正相机坐标。
在本实施例中，相机坐标为多目立体视觉系统101的主相机的坐标系中的坐标。
在本实施例中，可基于多目立体视觉系统101的主相机与其他相机之间的相机转换参数，得到标定点在主相机的相机坐标系中的相机坐标。其中，该相机转换参数包括比例因子、相机旋转矩阵和相机平移矩阵。
在一些实施例中，对于包括两个相机的多目立体视觉系统101，上述步骤s306中，按照以下方式确定标定点的深度测量误差：
其中，d(ψ+ζ)为标定点的成像景深，d(ψ)为标定点的真实景深，d为相机透镜的直径，f为透镜焦距，ψ为真实视差，ψ+ζ为成像视差，η为相机直径倍数。
在一些实施例中，进一步的，按照以下方式分配偏差权重来对深度测量误差进行约束：
其中，pj为标定点j的偏差权重，∑δd(ψ)为所有标定点的总偏差，δd(ψ)j为标定点j的偏差。
在一些实施例中，对于包括两个相机的多目立体视觉系统101，上述步骤s308中按照以下方式基于标定点的修正像素坐标和修正深度信息将对应标定点的像素坐标转换为相机坐标，得到多个标定点的修正相机坐标：
其中，zc为多目立体视觉系统的修正深度信息，δd(ψ)j为标定点j的偏差，pj为标定点j的偏差权重，(xr,yr)为主相机的修正像素坐标，(xl,yl)为辅相机的修正像素坐标，f为主相机焦距，d(ψ+ζ)＜d(ψ)用负号，反之，则加号。
其中，r1为相机之间的旋转矩阵，t1为相机之间的平移矩阵。
在本实施例中，可基于多种方式修正相机畸变误差。在一些实施例中，相机畸变系数包括：径向畸变系数、切向畸变系和薄棱镜畸变系数。
在一些实施例中，如图4所示，修正相机畸变误差的过程可包括步骤s402至步骤s408。
步骤s402，基于径向畸变系数确定每个标定点的测量像素坐标的径向畸变参数。
步骤s404，基于切向畸变系数确定每个标定点的测量像素坐标的切向畸变参数。
步骤s406，基于薄棱镜畸变系数确定每个标定点的测量像素坐标的薄棱镜畸变参数。
步骤s408，基于标定点的薄棱镜畸变参数、切向畸变参数和径向畸变参数修正对应标定点的测量像素坐标，得到多个标定点的修正像素坐标。
在一些实施例中，上述步骤s408中，按照以下方式进行标定点的相机畸变修正：
其中，(x,y)为修正像素坐标，(xd,yd)为测量像素坐标，(δxr,δyr)为径向畸变参数，(δxd,δyd)为切向畸变参数，(δxp,δyp)为薄棱镜畸变参数。
在一些实施例中，上述步骤s402中，按照以下方式确定标定点的测量像素坐标的径向畸变参数：
其中，r为标定点对应像素点到图像中心的距离，ni为径向畸变系数，i为阶数。
在一些实施例中，上述步骤s404中，按照以下方式确定切向畸变参数：
其中，mi为切向畸变系数，i为阶数。
在一些实施例中，上述步骤s406中，按照以下方式确定薄棱镜畸变参数：
其中，s1为薄棱镜畸变系数，i为阶数。
在一些实施例中，上述步骤s206中，惯性导航系统102的误差可包括：加速度计的误差和/或陀螺仪的误差。陀螺仪的误差包括陀螺仪漂移偏差和陀螺仪尺度因子漂移引起的误差。加速度计的误差包括加速度计漂移偏差和加速度计尺度因子漂移引起的误差。
在一些实施例中，上述步骤s206中，可对加速度计的误差进行补偿，并对陀螺仪的误差进行补偿，得到多目立体视觉与视觉惯性导航系统100的姿态信息，经过转换得到陀螺仪和加速度计漂移误差补偿下的惯性导航系统102的坐标系中的修正惯性坐标。
在一些实施例中，进一步的，按照以下方式对加速度计的误差进行补偿：
其中，ωφ(t)为加速度计补偿的误差，ωab*(*为e、n或u)是加速度计漂移偏差，maμ为加速度计尺度因子的漂移，φcomp.0是补偿过的加速度计输出量。
在一些实施例中，进一步的，按照以下方式对陀螺仪的误差进行补偿：
其中，
其中，
在一些实施例中，相机坐标到惯性坐标的相对位姿参数中的比例因子为变量。可选地，确定比例因子可包括：基于多个标定点的修正相机坐标、修正惯性坐标和传感器相对位姿，通过最小二乘法确定比例因子。
在一些实施例中，按照以下方式，通过最小二乘法确定比例因子：
其中，(xinsα,yinsα,zinsα)为标定点α的修正惯性坐标，(xmsvα,ymsvα,zmsvα)为标定点i的修正相机坐标，α为1、2、3、4、……n，n为标定点的数量。
在一些实施例中，相机坐标到惯性坐标的相对位姿参数中旋转参数可包括：基于多个标定点的修正相机坐标和修正惯性坐标，通过四元数法确定空间旋转矩阵n；基于空间旋转矩阵n的最大特征值对应的特征向量，确定旋转参数。可选地，空间旋转矩阵n为4阶矩阵。
在一些实施例中，按照以下方式通过四元数法确定空间旋转矩阵n：
其中，
其中，nxx为对应标定点在修正相机坐标x轴方向值和修正惯性坐标x轴方向值的乘积和，nyy为对应标定点在修正相机坐标y轴方向值和修正惯性坐标y轴方向值的乘积和，nxz为对应标定点在修正相机坐标x轴方向值和修正惯性坐标z轴方向值的乘积和，以此类推；xmsvα为标定点的修正相机坐标x轴方向值，xinsα为标定点的修正惯性坐标x轴方向值，以此类推。
在一些实施例中，按照以下方式基于空间旋转矩阵n的最大特征值对应的特征向量，确定旋转参数：
其中，q0，q1，q2，q3为空间旋转矩阵n的最大特征值对应的特征向量。
在一些实施例中，确定平移参数包括：基于比例因子和旋转参数，按照约束条件确定平移参数。
在一些实施例中，基于比例因子λ和旋转矩阵r按照以下方式确定得到平移参数t：
t＝pins-λrpmsv
其中，pins为标定点的修正惯性坐标，pmsv为标定点的修正相机坐标，r为旋转参数，λ为比例因子。
应当理解，本申请实施例中的步骤虽然具有编号，但这并不是对步骤执行顺序的限定。此外，上述实施例中的一些实施例方式虽然分别描述，但其可以任意组合，本实施例对此不做赘述。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如rom/ram、磁碟、光盘)中，包括若干指令用以使得一台终端(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。
- 还没有人留言评论。精彩留言会获得点赞！