一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法
阅读说明：本技术 一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法 (Insect-imitated vision integrated navigation method based on polarized light, optical flow vector and binocular vision sensor ) 是由 褚金奎 陈建华 李金山 张然 于 2020-11-17 设计创作，主要内容包括：一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法,对空间点像素坐标系和世界坐标系进行转换；根据ORB角点提取对空间某特征点进行相邻帧匹配；利用偏振光传感器测得偏航角和光流传感器获得一个相对位置的测量引导；计算光流矢量并利用光流矢量对运动物体和误匹配点进行剔除；进行BA优化,利用能量函数,将双目视觉传感器匹配点数据、偏航角数据和光流数据优化；利用BA优化后的参数求增广矩阵,解得变换矩阵,反求解矩阵得到姿态角和位移完成导航。本发明仿照昆虫利用偏振光、图像通过纯视觉进行导航定位,有着无线电导航和卫星导航等非自主导航方式所无法具有的隐蔽性、抗干扰性,具有高精度和较强的鲁棒性。(A method for simulating insect vision integrated navigation based on polarized light, optical flow vector and binocular vision sensor, which converts space point pixel coordinate system and world coordinate system; extracting and matching adjacent frames of a certain characteristic point in the space according to the ORB angular point; measuring a yaw angle and an optical flow sensor by using a polarized light sensor to obtain a measurement guide of a relative position; calculating an optical flow vector and removing a moving object and a mismatching point by using the optical flow vector; performing BA optimization, and optimizing binocular vision sensor matching point data, yaw angle data and optical flow data by using an energy function; and solving the augmented matrix by using the parameters after BA optimization to obtain a transformation matrix, and reversely solving the matrix to obtain an attitude angle and a displacement to finish navigation. The invention imitates the insect to utilize polarized light and image to navigate and position through pure vision, has concealment and anti-interference which can not be provided by non-autonomous navigation modes such as radio navigation and satellite navigation, and has high precision and stronger robustness.)
技术领域
本发明属于仿生组合导航领域，涉及一种仿昆虫组合导航系统及其定位方法，尤其涉及基于光流矢量、偏振光、双目视觉传感器组合导航系统测试平台搭建及定位方法。
背景技术
目前的常见的导航技术主要有惯性导航、卫星导航、天文导航、地磁导航等。惯性导航属自主式导航，具有短时精度高、不受干扰等优点，但是由于积分过程会产生累积误差，并且时间越长，误差就会越大，因此不适合远距离、耗时长的导航；卫星导航是一种非自主式导航方式，具有全球性、全天候的特性，但是它易受干扰，在有高大建筑的街道、茂密的丛林中无法正常使用，目前常用的有GPS和北斗卫星导航；天文导航是一种可以根据已知天体位置来计算载体当前航向和所处位置的导航技术，属于自主式导航，这种导航方式最大的缺点是精度低、集成度不高；地磁导航是利用地磁场分布规律测量并计算得到载体姿态信息，属于自主式导航，无误差累积，但它很容易受周围磁性材料产生磁场的影响。现有技术中公开的专利号：(CN103323005A)，提出一种SINS/GPS/偏振光组合导航系统多目标优化抗干扰滤波方法，将SINS、GPS、偏振传感器以及环境中的干扰进行分类，对不同类型的干扰进行干扰建模，可建模干扰通过设计干扰观测器来抵消，通过设计干扰观测器的方法以及鲁棒技术对干扰进行干扰抵消和干扰抑制，可用于提高地球大气层内运载体的导航精度；现有技术中公开的专利号：(CN109916394A)，提出融合光流位置和速度信息的组合导航算法，利用光流传感器输出的位置和速度信息以及MEMS IMU、磁力计、气压高度计、激光测距传感器的数据，采用扩展卡尔曼滤波器完成数据融合，解算出载体的位置、速度和姿态信息，因为采用磁力计，很容易受周围磁性材料产生磁场的影响，抗干扰能力不强；现有技术中公开的专利号：(CN109470237A)，提出基于偏振光与地磁组合导航姿态测量方法，但只能求解姿态角α，β，γ，不具备定位导航功能；现有技术中公开的专利号：(CN108362288A)，基于无迹卡尔曼滤波的偏振光SLAM方法，利用偏振光信息和激光雷达信息匹配互补、不受其他外界干扰的特性，一定程度提高了无人机SLAM系统的稳定性和精度，但无法进一步解决误匹配给导航带来的影响。
生物学家研究发现昆虫具有良好的导航能力，比如蜜蜂复眼对微小时间间隔内视觉信息的变化非常敏感，这种特点更合适处理动态信号，当蜜蜂在环境中移动时，图像的运动会在他的视网膜上形成一系列连续变化的光的“流动”信息，蜜蜂正通过分析这种动态的光流来进行导航，通过光流可以获得很多有用的信息提供导航使用；比如蟋蟀，眼部DRA区域的小眼结构的神经感杆内绒毛具有轴向规则一致，径向相互垂直的特点，该结构特点使视神经细胞具有较高的偏振光敏感特性，偏振光是天然基本物理场，携带有方位信息，蟋蟀具有三类POL-neurons，其主响应方向分别为10°，60°和130°，蟋蟀通过对三类POL-neurons输出信号进行综合处理，即可得出体轴与太阳子午线的夹角，从而实现导航功能；再比如多数昆虫具有双眼，根据双目测距原理，从不同角度观察一个场景时可以很容易的得到关于场景在距离上的感知。根据仿生导航原理当已知相机光心、两幅图片对应像素点及其距离时便可以很容易的通过三角关系得到实际场景中改点距离相机的距离，利用偏振光传感器，可以敏感的输出偏振方位角，具有不随时间累计等优点、自主性强，可以获得偏航角，通过光流可以测距，测速。
传统组合导航系统抗干扰能力比较弱，而无GPS或惯导组合的导航系统仅能测姿，无法定位，目前现有专利中仅将偏振光和双目传感器结合在一起使用，但并未加入光流模型和后端的优化。综上本文提出一种新型仿生组合导航方法，将双目视觉传感器、光流和偏振光传感器进行组合，提出一种新的组合导航方式，有着无线电导航和卫星导航等非自主导航方式所无法具有的隐蔽性、抗干扰性和更高的鲁棒性。
发明内容
针对现有导航系统的抗干扰能力弱，本发明提出一种新型的仿生组合导航方法，将偏振光传感器、光流和双目视觉传感器进行组合，利用卡尔曼滤波进行数据融合，后端通过BA优化，发明一款纯依靠视觉进行导航定位的方法，抗干扰性强，具有较强的鲁棒性。
本发明的技术方案：
一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法，包括以下步骤：
步骤1：基于双目视觉传感器测量得到点P的像素坐标为[u,v]T，已知物理成像平面到小孔的距离为f(焦距)，设地球坐标系下点P坐标[X,Y,Z]T，对应物理成像平面P'点的坐标为[X',Y',Z']T，像素坐标在u轴上缩放了α倍，在v上缩放了β倍，原点平移了[cx,cy]T，根据Z/f＝-X/X'＝-Y/Y'可得P'坐标与像素坐标[u,v]T的关系为：
设fx＝αf，fy＝βf，得到像素坐标到空间坐标的转换：
步骤2：双目相机由左目和右目两个水平放置的相机，OL、OR为左右相机的光心，f为焦距，z为场景深度，uL、uR为成像平面的坐标，PL、PR为左右相机内成像平面上的图像点，根据相似关系可以得到：
步骤3：根据相机在两帧图像I1、I2之间的相对运动关系为R与t，两帧图像对应特征点进行匹配，P点的空间齐次坐标为p＝[X,Y,Z,1]T，点P在I2对应点投影到像素平面上的坐标为x1＝[u1,v1,1]T，根据匹配点定义增广矩阵[R|t]，根据相邻图像之间的对应关系得到求解方程s·x＝[R|t]·p。
步骤4：双目传感器直接得到的偏航角和位移误差比较大，使用偏振光对航向角进行修正，基于多方向偏振光传感器检测单元得到的偏振角为在可以得到对应的偏振矢量矢量坐标为：组合导航系统初始时偏振光传感器测得的初始航向角为初始位姿为：R＝I、t＝0。当前偏振光传感器测得的航向角为组合导航平台的位姿关于地理正北方向的变化角为：将相邻两帧偏振光数据集记为
步骤5：在相邻两帧之间，得到tk-1到tk时刻之间导航系统所有特征点位移矢量为通过光流传感器获得tk-1到tk时刻光流矢量为设定阈值A，如果在和之间就认为是正确匹配的特征点，不在该范围内就认为是误匹配点或运动的物体进行剔除。
步骤6：BA优化，相机运动每帧图像可以恢复相机姿态为C1…Cm，相机观察到的路标为X1…Xn，将一个地球坐标系下的点Xj转换到相机坐标系下投影到图像上有hij＝(fxXij/Xij+cx,fyYij/Zij+cy)T，可以得到函数hij＝h(Ci,Xj)T，取相邻两帧(Ci,Ci+1)间所有偏振光数据集合Pi＝{p1…pni}，反解Ci得到航向角与偏振光数据耦合，得到函数fi＝f(Ci,Pj)T，求解BA优化函数：
其中Eij与Γij分别为运动方程的协方差矩阵和偏振光数据的协方差BA优化函数取得最小时，解得优化后的参数。
步骤7：如果BA优化后，匹配特征点太少，双目视觉传感器数据丢失，采用光流速度模型短暂定位导航，等到特征点匹配较多时恢复双目视觉模型，如果匹配点够用，双目视觉传感器未丢失数据，直接执行步骤8。
步骤8：将一对匹配点带入到式s·x＝[R|t]·p中得：
可以得到约束方程：
定义T的行向量：
t1＝(t1，t2，t3，t4)T
t2＝(t5，t6，t7，t8)T
t3＝(t9，t10，t11，t12)T
对于其它点相同表示得到方程组：
进而得到：
根据优化后的变换矩阵T解算出修正后载体的位置，速度和姿态信息。
本发明的有益效果：本发明采用偏振光、光流矢量和双目视觉传感器，设计纯依靠视觉进行导航定位的仿生导航系统，在不使用GPS和惯导的情况下，具有良好的导航能力，抗干扰能力强，可以在复杂环境下进行导航工作，是一种精度高、鲁棒性强的全自主导航方式。
附图说明
图1为本发明系统构成示意图；
图2为本发明方法导航计算流程图；
图3为本发明方法双目测距原理；
图4为本发明方法算法流程示意图；
图5为本发明系统轴测图；
图中：1双目视觉传感器；2偏振光传感器。
具体实施方式
以下结合附图和技术方案，进一步说明本发明的具体实施方式。
步骤1：用FAST算法进行角点检测选取特征点。
(1)在图像上选取像素P，假设它的亮度为Ip。
(2)设置阈值T。
(3)以像素P为中心，选取半径为3的圆上16个像素点。
(4)选取圆上亮度大于Ip+T或Ip-T的像素点为特征点。
步骤2：建立相机坐标系O-x-y-z，地球坐标系下空间点P，经过光心O投影之后，落在物理成像平面O'-x'-y'上，成像点为P'，P点的坐标为[X,Y,Z]T，P'点的坐标为[X',Y',Z']T，物理成像平面到小孔的距离为f(焦距)。根据三角形相似关系，可得：
化简可以得到：
在像素平面得到了P'的像素坐标：[u,v]T。像素坐标在u轴上缩放了α倍，在v上缩放了β倍，原点平移了[cx,cy]T。P'坐标与像素坐标[u,v]T的关系为：
令fx＝αf，fy＝βf：
则
将其写为矩阵的形式便得到了下面的等式：
把中间量组成的矩阵称为相机的内参矩阵，用K表示。由此可以得到像素坐标到空间坐标的转换。
步骤3：根据双目相机成像机理可得整理可得深度视差d＝uL-uR。
由像素坐标和深度信息可以的到世界坐标系下点P位置:
步骤4：特征点匹配，利用PnP求解，相机在两帧图像I1、I2之间的相对运动关系为R与t，相机中心分别为O1、O2，两帧图像之间有一组对应的特征点为p1、p2，两个特征点应为空间某一点在两成像平面中的投影，根据p1点的像素坐标并深度计算P点的空间齐次坐标为p＝[X,Y,Z,1]T，点P在I2对应点p2投影到像素平面上的坐标为x1＝[u1,v1,1]T，根据匹配点定义增广矩阵[R|t]，根据相邻图像之间的对应关系得到求解方程s·x＝[R|t]·p。
将上式展开可得：
可以得到约束方程：
定义T的行向量：
t1＝(t1，t2，t3，t4)T
t2＝(t5，t6，t7，t8)T
t3＝(t9，t10，t11，t12)T
对于其它点相同表示得到方程组：
通过6对匹配点即可以实现对矩阵T的线性求解，当匹配点大于6对时使用BA优化进行求解最优值。
步骤5：偏振光辅助修正偏航角误差，偏振光检测单元得到的偏振角为在该偏振光检测单元所建立的坐标系下便可以得到对应的偏振矢量矢量坐标为：其中K可取±1。通过摆放偏振光传感器的体轴方向与地球正北方向的关系便可以确定K的取值，组合导航系统初始时偏振光传感器测得的初始航向角为初始位姿为：R＝I、t＝0，其中I为单位矩阵。假设相机拍摄当前帧时，偏振光传感器测得的航向角为则机器人位姿关于地理正北方向的变化角为：
将相邻两帧(Ci,Ci+1)间所有偏振光数据集记为Pi＝{p1…pni}，Pi即为偏航角。
步骤6：光流辅助修正，通过光流可以获得X轴和Y轴方向的速度Vx,Vy，光流传感器到相机坐标系的位姿变换为Tco，由光流传感器获得的X、Y方向的位移可得到相机坐标系下X、Y方向的位移。由恒速模型可得到当前帧与上一帧位姿之间的相对位姿，在此基础上，用传感器获得的位姿信息纠正恒速模型，使得初始的位姿具有较好的初始值。恒速模型得到的相对位姿为：
光流传感器以获得一个相对位姿的测量引导，得到光流数据Mi＝{m1…mni}。步骤7：光流矢量去除运动物体对导航系统的影响，在相邻两帧之间，记tk-1时刻采集到关键帧特征点集合为(其中)，运动到tk时刻采集到关键帧特征点集合为(其中)，可以得到tk-1到tk时刻之间导航系统所有特征点位移矢量为通过光流传感器获得tk-1到tk时刻光流矢量为设定阈值A，比较光流矢量和位移矢量，如果在和之间就认为是正确匹配的特征点，如果不在该范围内就认为是误匹配点或运动的物体进行剔除。
步骤8：BA优化得到最优解，相机运动每帧图像可以恢复相机姿态为C1…Cm，相机观察到的路标为X1…Xn，Ci包含了相机的外参Ri和ti，Ri和ti将一个地球坐标系下的点Xj转换到相机坐标系下(Xij,Yij,Zij)＝Ri(Xj-ti)，投影到图像上有hij＝(fxXij/Xij+cx,fyYij/Zij+cy)T，可以得到函数hij＝h(Ci,Xj)T，将相邻两帧(Ci,Ci+1)间所有偏振光数据集记为集合Pi＝{p1…pni}，反解Ci得到航向角与偏振光数据耦合，得到函数fi＝f(Ci,Pj)T，求解BA优化函数：
其中f(Ci,Pi)为Pi作用于Ci后的运动参数，其中Eij与Γij分别为运动方程的协方差矩阵和偏振光数据的协方差，BA优化函数最小时，解得优化后的参数。
步骤9：根据BA优化得到得最优参数求解出增广矩阵[R|t]，然后再分解矩阵得到位姿R与t，通常相机内参矩阵K已知，求解增广矩阵便可以解出位姿，通过双目相机求解特征点的齐次三维坐标[X,Y,Z,1]T，根据匹配到第二幅图片的像素坐标[u,v]T。
可得：
根据约束方程：
定义T的行向量：
t1＝(t1，t2，t3，t4)T
t2＝(t5，t6，t7，t8)T
t3＝(t9，t10，t11，t12)T
解得t1，t2，t3，进而得到R和t，使用SE3表示：
到此根据变换矩阵T反解算得到载体的位置，速度和姿态信息。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：高精度的数字孪生场景中的激光融合定位方法