CN108426566A - 一种基于多摄像机的移动机器人定位方法 - Google Patents

Abstract

本发明公开了一种基于多摄像机的移动机器人定位装置，用以解决现有视觉定位系统存在的局限性问题。分散在移动机器人前后左右四个方向的四个相机同时拍摄到的图片中的纹理都较少（比如白墙）的概率较低，或者移动机器人四周都存在相对运动（比如人走动）的概率也比较低，当其中几个（小于4个）相机拍摄到的纹理较少或存在相对运动时，采用剩下摄像机拍摄到的图像进行分析，从而达到较为准确的定位。

Description

一种基于多摄像机的移动机器人定位方法

技术领域

本发明涉及一种机器人定位的方法，更具体地讲，涉及使用四个不同方向的摄像机准确地识别移动机器人的位置的移动机器人定位方法。

背景技术

视觉定位是通过单个相机或多个相机作为输入估计自身运动的过程。应用领域涵盖机器人、可穿戴计算、增强现实和自动驾驶。

一般的视觉定位根据所使用的相机数目的不同，可以分为单目视觉定位装置和多目视觉定位装置。

单目视觉定位装置：当拍摄到的图片纹理较少，或存在相对运动时，单目视觉定位装置并不能还原出真实的运动距离以及轨迹，这使得单目视觉定位装置无法较准确定位。

多目视觉定位装置：目前的多目视觉定位装置不具备单目视觉定位装置的宽广视野，三维点建立的范围较小，当视野内物体不具备明显特征点时，视觉定位装置容易失效。

发明内容

鉴于上述的分析，本发明旨在提供一种基于多摄像机的移动机器人定位装置，用以解决现有视觉定位系统存在的局限性问题。分散在移动机器人前后左右四个方向的四个相机同时拍摄到的图片中的纹理都较少(比如白墙)的概率较低，或者移动机器人四周都存在相对运动(比如人走动)的概率也比较低，当其中几个(小于4个)相机拍摄到的纹理较少或存在相对运动时，采用剩下摄像机拍摄到的图像进行分析，从而达到较为准确的定位。

本发明采用以下技术方案：

1.一种基于多摄像机的移动机器人定位方法，将四个摄像机安装在移动机器人前、后、左、右四个方向上，四个摄像机分别在相同时刻采集连续图像传送到计算机，通过图像处理算法融合四个摄像机的信息，得到机器人每一时刻的精确运动信息。所述图像处理算法包含以下步骤：

(1)对四个摄像机的相邻两帧图像分别进行特征点提取和匹配，并计算得到各自的本质矩阵E，将本质矩阵E分解为旋转矩阵R和平移向量t。

(2)根据摄像机之间的坐标转换关系，可根据一个摄像机的R和t推导得出其他几个摄像机的运动信息，并求得在其他摄像机的图像中符合该运动信息的特征点点对(称为内点)的数目。

(3)对多个摄像机分别执行第(2)步，求得使总的内点数目最大的摄像机，然后利用该摄像机的图像的内点，根据非线性最小二乘算法再计算本质矩阵，分解得到旋转矩阵和平移向量，根据摄像机和车体坐标系的转换关系，得出机器人运动信息。

2.作为优选，在机器人前后左右的四个摄像机实时采集移动机器人四个方向的图像，每个相机连续拍摄两帧图像。对拍摄到的图像进行预处理(矫正、去噪、滤波、锐化、背景补偿等)。

3.作为优选，对处理后的每个相机的两帧图像进行特征提取和匹配得到多对特征点。四个相机分别获得n_u、n_v、n_w、n_p(u、v、w、p都不相等)对特征点。

4.作为优选，当特征点对n_i(i为1,2,3,4任意值)大于阈值T_n时，采用RANSAC(Random Sample Consensus)算法得到本质矩阵E。

1)只有一个摄像机拍摄到的图像匹配后得到的特征点对对数n_u大于阈值T_n，对该摄像机图像进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解E。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

2)有两个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v(u不等于v)大于阈值T_n，

对这两个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v。把R_u和t_u转换到摄像机v坐标系内为R′_u和t'_u，从而得到E′_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E′_u的内点数目。把R_v和t_v转换到摄像机u坐标系内为R′_v和t′_v，从而得到E′_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E′_v的内点数目。选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

3)有三个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v、n_w(u不等于v且不等于w)大于阈值T_n，对这三个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把E_w分解为R_w和t_w。把R_u和t_u转换到摄像机v坐标系内为R′_u和t′_u，从而得到E′_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E′_u的内点数目，同时，把R_u和t_u转换到摄像机w坐标系内为R″_u和t″_u，从而得到E″_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_u的内点数目。把R_v和t_v转换到摄像机u坐标系内为R′_v和t′_v，从而得到E′_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E_v'的内点数目，同时，把R_v和t_v转换到摄像机w坐标系内为R″_v和t″_v，从而得到E″_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_v的内点数目。把R_w和t_w转换到摄像机u坐标系内为R'_w和t'_w，从而得到E'_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E'_w的内点数目，同时，把R_w和t_w转换到摄像机v坐标系内为R″_w和t″_w，从而得到E″_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_w的内点数目。选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

4)四个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v、n_w、n_q(u、v、w、q都不相等)都大于阈值T_n，对这四个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w，摄像机q对应的最小二乘解为E_q。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把E_w分解为R_w和t_w,把E_q分解为R_q和t_q。把R_u和t_u转换到摄像机v坐标系内为R′_u和t'_u，从而得到E′_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E′_u的内点数目，同时，把R_u和t_u转换到摄像机w坐标系内为R″_u和t″_u，从而得到E″_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_u的内点数目，并且，把R_u和t_u转换到摄像机q坐标系内为R″′_u和t″′_u，从而得到E″′_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算q坐标系内符合E″′_u的内点数目。把R_v和t_v转换到摄像机u坐标系内为R′_v和t′_v，从而得到E′_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E′_v的内点数目，同时，把R_v和t_v转换到摄像机w坐标系内为R″_v和t″_v，从而得到E″_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_v的内点数目，并且，把R_v和t_v转换到摄像机q坐标系内为R″′_v和t″′_v，从而得到E″′_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算q坐标系内符合E″′_v的内点数目。把R_w和t_w转换到摄像机u坐标系内为R'_w和t'_w，从而得到E'_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E'_w的内点数目，同时，把R_w和t_w转换到摄像机v坐标系内为R″_w和t″_w，从而得到E″_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E″_w的内点数目,并且，把R_w和t_w转换到摄像机q坐标系内为R″′_w和t″′_w，从而得到E″′_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算q坐标系内符合E″′_w的内点数目。把R_q和t_q转换到摄像机u坐标系内为R'_q和t'_q，从而得到E'_q，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E'_q的内点数目，同时，把R_q和t_q转换到摄像机v坐标系内为R″_q和t″_q，从而得到E″_q，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E″_q的内点数目,并且，把R_q和t_q转换到摄像机w坐标系内为R″′_q和t″′_q，从而得到E″′_q，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″′_q的内点数目。选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

5)四个摄像机拍摄到的图像匹配后得到的特征点对对数都小于阈值T_n，对特征点对对数进行排序，其序列为n_u、n_v、n_w、n_q(u、v、w、q都不相等)。

A.若n_u、n_v相加大于阈值T_n，对这两个摄像机图像分别进行RANSAC(Random SampleConsensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v。把R_u和t_u转换到摄像机v坐标系内为R′_u和t′_u，从而得到E′_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E′_u的内点数目。把R_v和t_v转换到摄像机u坐标系内为R′_v和t′_v，从而得到E′_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E_v'的内点数目。选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

B.若n_u、n_v相加小于阈值T_n，且加上n_w后大于阈值T_n，对这三个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把E_w分解为R_w和t_w。把R_u和t_u转换到摄像机v坐标系内为R′_u和t′_u，从而得到E′_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E′_u的内点数目，同时，把R_u和t_u转换到摄像机w坐标系内为R″_u和t″_u，从而得到E″_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_u的内点数目。把R_v和t_v转换到摄像机u坐标系内为R′_v和t'_v，从而得到E′_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E′_v的内点数目，同时，把R_v和t_v转换到摄像机w坐标系内为R″_v和t″_v，从而得到E″_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_v的内点数目。把R_w和t_w转换到摄像机u坐标系内为R'_w和t'_w，从而得到E'_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E'_w的内点数目，同时，把R_w和t_w转换到摄像机v坐标系内为R″_w和t″_w，从而得到E″_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_w的内点数目。选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

C.若n_u、n_v、n_w相加小于阈值T_n，无论n_u、n_v、n_w、n_q是否大于阈值T_n，对这四个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w，摄像机q对应的最小二乘解为E_q。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把E_w分解为R_w和t_w,把E_q分解为R_q和t_q。把R_u和t_u转换到摄像机v坐标系内为R′_u和t′_u，从而得到E′_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E′_u的内点数目，同时，把R_u和t_u转换到摄像机w坐标系内为R″_u和t″_u，从而得到E″_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_u的内点数目，并且，把R_u和t_u转换到摄像机q坐标系内为R″′_u和t″′_u，从而得到E″′_u，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算q坐标系内符合E″′_u的内点数目。把R_v和t_v转换到摄像机u坐标系内为R′_v和t'_v，从而得到E′_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E′_v的内点数目，同时，把R_v和t_v转换到摄像机w坐标系内为R″_v和t″_v，从而得到E″_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″_v的内点数目，并且，把R_v和t_v转换到摄像机q坐标系内为R″′_v和t″′_v，从而得到E″′_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算q坐标系内符合E″′_v的内点数目。把R_w和t_w转换到摄像机u坐标系内为R'_w和t'_w，从而得到E'_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E'_w的内点数目，同时，把R_w和t_w转换到摄像机v坐标系内为R″_w和t″_w，从而得到E″_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E″_w的内点数目,并且，把R_w和t_w转换到摄像机q坐标系内为R″′_w和t″′_w，从而得到E″′_w，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算q坐标系内符合E″′_w的内点数目。把R_q和t_q转换到摄像机u坐标系内为R'_q和t'_q，从而得到E'_q，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算u坐标系内符合E'_q的内点数目，同时，把R_q和t_q转换到摄像机v坐标系内为R″_q和t″_q，从而得到E″_q，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算v坐标系内符合E″_q的内点数目,并且，把R_q和t_q转换到摄像机w坐标系内为R″′_q和t″′_q，从而得到E″′_q，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E″′_q的内点数目。选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

附图说明

图1是本发明型实施例多摄像机移动机器人的结构示意图的俯视图；

图2是本发明型实施例多摄像机移动机器人的结构示意图的正视图；

图3是本发明型实施例SIFT算法的流程框图；

图4是本发明型实施例多摄像机移动机器人的流程框图；

其中1、2、3、4-普通摄像机 5-移动机器人。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

图像采集：在机器人前后左右的四个摄像机实时采集移动机器人四个方向的图像，每个相机连续拍摄两帧图像。对拍摄到的图像进行预处理(矫正、去噪、滤波、锐化、背景补偿等)

特征提取与匹配：采用SIFT(Scale-invariant feature transform，尺度不变特征变换)特征对处理后的每个相机的两帧图像进行特征提取和匹配得到多对特征点。四个相机分别获得n_u、n_v、n_w、n_q(u、v、w、q都不相等)对特征点。

数据分析：当特征点对n_i(i为1,2,3,4任意值)大于阈值T_n时，采用RANSAC(RandomSample Consensus)算法得到本质矩阵E。

1)只有一个摄像机拍摄到的图像匹配后得到的特征点对对数n_u大于阈值T_n，对该摄像机图像进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解E。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。

2)有两个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v(u不等于v)大于阈值T_n，对这两个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v。然后根据摄像机之间的坐标转换关系分别计算E_u在摄像机v下的内点数目，E_v在摄像机u下的内点数目，选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。

3)有三个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v、n_w(u不等于v且不等于w)大于阈值T_n，对这三个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w。然后根据摄像机之间的坐标转换关系分别计算E_u在摄像机v和w下的内点数目，E_v在摄像机u和w下的内点数目，E_w在摄像机u和v下的内点数目，选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。

4)四个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v、n_w、n_q(u、v、w、q都不相等)都大于阈值T_n，对这四个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w，摄像机q对应的最小二乘解为E_q。然后根据摄像机之间的坐标转换关系分别计算E_u在摄像机v、w和q下的内点数目，E_v在摄像机u、w和q下的内点数目，E_w在摄像机u、v和q下的内点数目，E_q在摄像机u、v和w下的内点数目，选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。

5)四个摄像机拍摄到的图像匹配后得到的特征点对对数都小于阈值T_n，对特征点对对数进行排序，其序列为n_u、n_v、n_w、n_q(u、v、w、q都不相等)。

A.若n_u、n_v相加大于阈值T_n，对这两个摄像机图像分别进行RANSAC(Random SampleConsensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v。然后根据摄像机之间的坐标转换关系分别计算E_u在摄像机v下的内点数目，E_v在摄像机u下的内点数目，选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。

B.若n_u、n_v相加小于阈值T_n，且加上n_w后大于阈值T_n，对这三个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w。然后根据摄像机之间的坐标转换关系分别计算E_u在摄像机v和w下的内点数目，E_v在摄像机u和w下的内点数目，E_w在摄像机u和v下的内点数目，选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。

C.若n_u、n_v、n_w相加小于阈值T_n，无论n_u、n_v、n_w、n_q是否大于阈值T_n，对这四个摄像机图像分别进行RANSAC(Random Sample Consensus)算法(使用8点算法)，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w，摄像机q对应的最小二乘解为E_q。然后根据摄像机之间的坐标转换关系分别计算E_u在摄像机v、w和q下的内点数目，E_v在摄像机u、w和q下的内点数目，E_w在摄像机u、v和q下的内点数目，E_q在摄像机u、v和w下的内点数目，选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。

6.输出位置信息：输出位置信息旋转矩阵R和平移向量t，通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。

Claims (8)

1.一种基于多摄像机的移动机器人定位方法，将四个摄像机安装在移动机器人前、后、左、右四个方向上，四个摄像机分别在相同时刻采集连续图像传送到计算机，通过图像处理算法融合四个摄像机的信息，得到机器人每一时刻的精确运动信息。所述图像处理算法包含以下步骤：

(1)对四个摄像机的相邻两帧图像分别进行特征点提取和匹配，并计算得到各自的本质矩阵E，将本质矩阵E分解为旋转矩阵R和平移向量t。

(2)根据摄像机之间的坐标转换关系，可根据一个摄像机的R和t推导得出其他几个摄像机的运动信息，并求得在其他摄像机的图像中符合该运动信息的特征点点对(称为内点)的数目。

(3)对多个摄像机分别执行第(2)步，求得使总的内点数目最大的摄像机，然后利用该摄像机的图像的内点，根据非线性最小二乘算法再计算本质矩阵，分解得到旋转矩阵和平移向量，根据摄像机和车体坐标系的转换关系，得出机器人运动信息。

2.如权利要求1所述的基于多摄像机的移动机器人定位方法，其特征在于：对摄像机拍摄到的图像进行预处理，包括矫正、去噪、滤波、锐化、背景补偿等。

3.如权利要求1所述的基于多摄像机的移动机器人定位方法，其特征在于：四个摄像机分别获得n_u、n_v、n_w、n_p对特征点。

4.如权利要求1所述的基于多摄像机的移动机器人定位方法，其特征在于：

只有一个摄像机拍摄到的图像匹配后得到的特征点对对数n_u大于阈值T_n，对该摄像机图像进行RANSAC(Random Sample Consensus)算法，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解E。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

5.如权利要求1所述的基于多摄像机的移动机器人定位方法，其特征在于：

若有两个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v(u不等于v)大于阈值T_n，对这两个摄像机图像分别进行RANSAC(Random Sample Consensus)算法，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解；摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把R_u和t_u转换到摄像机v坐标系内为R′_u和t'_u，从而得到E′_u，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算v坐标系内符合E′_u的内点数目。把R_v和t_v转换到摄像机u坐标系内为R′_v和t'_v，从而得到E′_v，再根据公式其中x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E′_v的内点数目；选择总的内点数目最高的E，作为最终结果；

将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

6.如权利要求1所述的基于多摄像机的移动机器人定位方法，其特征在于：若有三个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v、n_w大于阈值T_n，对这三个摄像机图像分别进行RANSAC(Random Sample Consensus)算法，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解；摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把E_w分解为R_w和t_w；把R_u和t_u转换到摄像机v坐标系内为R′_u和t'_u，从而得到E′_u，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算v坐标系内符合E′_u的内点数目，同时，把R_u和t_u转换到摄像机w坐标系内为R″_u和t″_u，从而得到E″_u，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″_u的内点数目；把R_v和t_v转换到摄像机u坐标系内为R′_v和t'_v，从而得到E′_v，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E′_v的内点数目，同时，把R_v和t_v转换到摄像机w坐标系内为R″_v和t″_v，从而得到E″_v，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″_v的内点数目；把R_w和t_w转换到摄像机u坐标系内为R'_w和t'_w，从而得到E'_w，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E'_w的内点数目，同时，把R_w和t_w转换到摄像机v坐标系内为R″_w和t″_w，从而得到E″_w，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″_w的内点数目；选择总的内点数目最高的E，作为最终结果；将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置；坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

7.如权利要求1所述的基于多摄像机的移动机器人定位方法，其特征在于：四个摄像机拍摄到的图像匹配后得到的特征点对对数n_u、n_v、n_w、n_q都大于阈值T_n，对这四个摄像机图像分别进行RANSAC(Random Sample Consensus)算法，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w，摄像机q对应的最小二乘解为E_q；通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把E_w分解为R_w和t_w,把E_q分解为R_q和t_q。把R_u和t_u转换到摄像机v坐标系内为R′_u和t'_u，从而得到E′_u，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算v坐标系内符合E′_u的内点数目，同时，把R_u和t_u转换到摄像机w坐标系内为R″_u和t″_u，从而得到E″_u，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″_u的内点数目，并且，把R_u和t_u转换到摄像机q坐标系内为R″′_u和t″′_u，从而得到E″′_u，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算q坐标系内符合E″′_u的内点数目。把R_v和t_v转换到摄像机u坐标系内为R′_v和t'_v，从而得到E′_v，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E′_v的内点数目，同时，把R_v和t_v转换到摄像机w坐标系内为R″_v和t″_v，从而得到E″_v，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″_v的内点数目，并且，把R_v和t_v转换到摄像机q坐标系内为R″′_v和t″′_v，从而得到E″′_v，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算q坐标系内符合E″′_v的内点数目；把R_w和t_w转换到摄像机u坐标系内为R'_w和t'_w，从而得到E'_w，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向；计算u坐标系内符合E'_w的内点数目，同时，把R_w和t_w转换到摄像机v坐标系内为R″_w和t″_w，从而得到E″_w，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向；计算v坐标系内符合E″_w的内点数目,并且，把R_w和t_w转换到摄像机q坐标系内为R″′_w和t″′_w，从而得到E″′_w，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向；计算q坐标系内符合E″′_w的内点数目；把R_q和t_q转换到摄像机u坐标系内为R'_q和t'_q，从而得到E'_q，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E'_q的内点数目，同时，把R_q和t_q转换到摄像机v坐标系内为R″_q和t″_q，从而得到E″_q，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向；计算v坐标系内符合E″_q的内点数目,并且，把R_q和t_q转换到摄像机w坐标系内为R″′_q和t″′_q，从而得到E″′_q，再根据公式其中，x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向；计算w坐标系内符合E″′_q的内点数目。选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置；坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。

8.如权利要求1所述的基于多摄像机的移动机器人定位方法，其特征在于：四个摄像机拍摄到的图像匹配后得到的特征点对对数都小于阈值T_n，对特征点对对数进行排序，其序列为n_u、n_v、n_w、n_q：

A.若n_u、n_v相加大于阈值T_n，对这两个摄像机图像分别进行RANSAC(Random SampleConsensus)算法，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v。把R_u和t_u转换到摄像机v坐标系内为R′_u和t'_u，从而得到E′_u，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算v坐标系内符合E′_u的内点数目；把R_v和t_v转换到摄像机u坐标系内为R′_v和t'_v，从而得到E′_v，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E′_v的内点数目；选择总的内点数目最高的E，作为最终结果；将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置。坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置；

B.若n_u、n_v相加小于阈值T_n，且加上n_w后大于阈值T_n，对这三个摄像机图像分别进行RANSAC(Random Sample Consensus)算法，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把E_w分解为R_w和t_w。把R_u和t_u转换到摄像机v坐标系内为R′_u和t'_u，从而得到E′_u，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算v坐标系内符合E′_u的内点数目，同时，把R_u和t_u转换到摄像机w坐标系内为R″_u和t″_u，从而得到E″_u，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″_u的内点数目；把R_v和t_v转换到摄像机u坐标系内为R′_v和t'_v，从而得到E′_v，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E′_v的内点数目，同时，把R_v和t_v转换到摄像机w坐标系内为R″_v和t″_v，从而得到E″_v，再根据公式(x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向)计算w坐标系内符合E”_v的内点数目。把R_w和t_w转换到摄像机u坐标系内为R'_w和t'_w，从而得到E'_w，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E'_w的内点数目，同时，把R_w和t_w转换到摄像机v坐标系内为R″_w和t″_w，从而得到E″_w，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″_w的内点数目；选择总的内点数目最高的E，作为最终结果。将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置；坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置；

C.若n_u、n_v、n_w相加小于阈值T_n，无论n_u、n_v、n_w、n_q是否大于阈值T_n，对这四个摄像机图像分别进行RANSAC(Random Sample Consensus)算法，在对应的特征点集中随机选取8对特征点，根据8点算法计算基本矩阵，计算特征点集中所有点对在这个基本矩阵下的误差，在此基本矩阵下距离小于某个阈值的点称为内点，计算内点数目，并存储内点，重复随机选取8对特征点，重复操作，直至达到最大循环次数，找到使内点数目最大的本质矩阵E以及与之对应的所有内点，使用这些内点，求取本质矩阵的最小二乘解。摄像机u对应的最小二乘解为E_u，摄像机v对应的小二乘解为E_v，摄像机w对应的最小二乘解为E_w，摄像机q对应的最小二乘解为E_q。通过公式E＝t^R，把E_u分解为R_u和t_u，把E_v分解为R_v和t_v，把E_w分解为R_w和t_w,把E_q分解为R_q和t_q。把R_u和t_u转换到摄像机v坐标系内为R'_u和t'_u，从而得到E'_u，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算v坐标系内符合E'_u的内点数目，同时，把R_u和t_u转换到摄像机w坐标系内为R”_u和t″_u，从而得到E”_u，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E”_u的内点数目，并且，把R_u和t_u转换到摄像机q坐标系内为R″′_u和t″′_u，从而得到E″′_u，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算q坐标系内符合E″′_u的内点数目；把R_v和t_v转换到摄像机u坐标系内为R′_v和t'_v，从而得到E′_v，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E′_v的内点数目，同时，把R_v和t_v转换到摄像机w坐标系内为R″_v和t″_v，从而得到E″_v，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″_v的内点数目，并且，把R_v和t_v转换到摄像机q坐标系内为R″′_v和t″′_v，从而得到E″′_v，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算q坐标系内符合E″′_v的内点数目；把R_w和t_w转换到摄像机u坐标系内为R'_w和t'_w，从而得到E'_w，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E'_w的内点数目，同时，把R_w和t_w转换到摄像机v坐标系内为R″_w和t″_w，从而得到E″_w，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算v坐标系内符合E″_w的内点数目,并且，把R_w和t_w转换到摄像机q坐标系内为R″′_w和t″′_w，从而得到E″′_w，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算q坐标系内符合E″′_w的内点数目。把R_q和t_q转换到摄像机u坐标系内为R'_q和t'_q，从而得到E'_q，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算u坐标系内符合E'_q的内点数目，同时，把R_q和t_q转换到摄像机v坐标系内为R″_q和t″_q，从而得到E″_q，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算v坐标系内符合E″_q的内点数目,并且，把R_q和t_q转换到摄像机w坐标系内为R″′_q和t″′_q，从而得到E″′_q，再根据公式x₁为前一帧图像的光线方向，x₂为后一帧图像的光线方向，计算w坐标系内符合E″′_q的内点数目，选择总的内点数目最高的E，作为最终结果；将本质矩阵E分解为旋转矩阵R和平移向量t，根据上一时刻摄像机的位置信息来得到当前的摄像机位置信息，然后通过摄像机坐标和移动机器人坐标的转换关系可得移动机器人的位置；坐标转换关系如下：

摄像机坐标系为{A}，移动机器人坐标系为{B}，已知摄像机坐标，求移动机器人坐标：

其中^BP是空间点P在移动机器人坐标系为{B}的坐标；^AP是空间点P在摄像机坐标系为{A}的坐标；旋转矩阵，描述坐标系{A}相对于坐标系{B}的方位；位置矢量，描述坐标系{A}坐标原点相对于坐标系{B}的位置。