[发明专利]一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法

权利要求书

1.一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法，其特征在于，包括以下步骤：

步骤1：基于双目视觉传感器测量得到点P的像素坐标为[u,v]^T，已知物理成像平面到小孔的距离为f，设地球坐标系下点P坐标[X,Y,Z]^T，对应物理成像平面P'点的坐标为[X',Y',Z']^T，像素坐标在u轴上缩放了α倍，在v轴上缩放了β倍，原点平移了[c_x,c_y]^T，根据Z/f＝-X/X'＝-Y/Y'得P'坐标与像素坐标[u,v]^T的关系为：

设f_x＝αf，f_y＝βf，得到像素坐标到空间坐标的转换：

把中间量组成的矩阵称为相机的内参矩阵，用K表示；

步骤2：双目相机由左目和右目两个水平放置的相机，O_L、O_R为左右相机的光心，f为焦距，z为场景深度，u_L、u_R为成像平面的坐标，P_L、P_R为左右相机内成像平面上的图像点，根据相似关系得到：

步骤3：根据相机在两帧图像I₁、I₂之间的相对运动关系为R与t，两帧图像对应特征点进行匹配，P点的空间齐次坐标为p＝[X,Y,Z,1]^T，点P在I₂对应点投影到像素平面上的坐标为x₁＝[u₁,v₁,1]^T，根据匹配点定义增广矩阵[R|t]，根据相邻图像之间的对应关系得到求解方程s·x＝[R|t]·p；

步骤4：双目传感器直接得到的偏航角和位移误差比较大，使用偏振光对航向角进行修正，基于多方向偏振光传感器检测单元得到的偏振角为在得到对应的偏振矢量E，偏振矢量E坐标为：组合导航系统初始时偏振光传感器测得的初始航向角为初始位姿为：R＝I、t＝0；当前偏振光传感器测得的航向角为组合导航平台的位姿关于地理正北方向的变化角为：将相邻两帧偏振光数据集记为E_i＝{e₁…e_m}；

步骤5：在相邻两帧之间，得到t_k-1到t_k时刻之间导航系统所有特征点位移矢量为通过光流传感器获得t_k-1到t_k时刻光流矢量为设定阈值A，如果在和之间就认为是正确匹配的特征点，不在该范围内就认为是误匹配点或运动的物体进行剔除；

步骤6：BA优化，相机运动每帧图像可恢复相机姿态为C₁…C_m，相机观察到的路标为X₁…X_n，将一个地球坐标系下的点X_j转换到相机坐标系下投影到图像上有h_ij＝(f_xX_ij/Z_ij+c_x,f_yY_ij/Z_ij+c_y)^T，得到函数h_ij＝h(C_i,X_j)^T，取相邻两帧(C_i,C_i+1)间所有偏振光数据集合E_i＝{e₁…e_m}，反解C_i得到航向角与偏振光数据耦合，得到函数f_i＝f(C_i,E_i)^T，求解BA优化函数：

其中E_ij与Γ_ij分别为运动方程的协方差矩阵和偏振光数据的协方差，当BA优化函数取得最小时，解得优化后的参数；

步骤7：如果BA优化后，匹配特征点太少，双目视觉传感器数据丢失，采用光流速度模型短暂定位导航，等到特征点匹配较多时恢复双目视觉模型，如果匹配点够用，双目视觉传感器未丢失数据，直接执行步骤8；

步骤8：将一对匹配点带入到式s·x＝[R|t]·p中得：

得到约束方程：

定义T的行向量：

t₁＝(t₁，t₂，t₃，t₄)^T

t₂＝(t₅，t₆，t₇，t₈)^T

t₃＝(t₉，t₁₀，t₁₁，t₁₂)^T

对于其它点相同表示得到方程组：

进而得到：

根据优化后的变换矩阵T解算出修正后载体的位置，速度和姿态信息。