一种偏振光/双目视觉仿生组合导航方法 下载: 938次
0 引言
随着现代科学技术的不断发展,导航技术在交通运输、资源勘探、移动通信等许多领域发挥着越来越重要的作用。目前的常见的导航技术主要有惯性导航、卫星导航、天文导航、地磁导航等[1-4],惯性导航自主性好、短时精度高、不受干扰等优点,但是长时间积分过程会产生累积误差,而高精度的惯导价格昂贵;卫星导航具有全球性、全天候的特性,但易受干扰,在有高大建筑的街道、茂密的丛林中无法正常使用[5];天文导航根据已知天体位置来计算载体姿态,存在最大的缺点是精度低、集成度不高;地磁导航易受环境磁性材料产生磁场的影响。因此通过新的研究思路和方法,寻求新的导航方式成为现在研究的热门。
近些年实时定位与地图构建系统(Simultaneous Localization and Mapping,SLAM)得到深入研究,主要模仿生物双眼测距定位[6],已经成为移动机器人在未知环境下自主导航和规划的关键技术,它可以看作是移动机器人构建环境地图的过程,同时利用新构建的地图提取位置和方向[7],广泛应用于无人驾驶车辆[8-9]、智能机器人[10]、增强现实等领域[11-12]。常用于SLAM的传感器有单目相机[13]、双目相机[14]和深度相机等[15]。但是由于SLAM是纯视觉导航算法,对环境的适应性差,精度低,会产生纯旋转误差大等问题。
由于单一传感器导航能力有限,将多种导航方式进行合理组合,可有效的改善导航性能。DISSANAYAKE G等[16]提出了视觉与惯性组合的方法,使用立体相机和IMU通过最小化重投影和惯性优化函数来估计相机的姿态、速度和IMU偏差,这种组合导航方式成本高,会产生累积误差;SAMARASEKERA S等[17]提出一种采用单目摄像机、带有三轴陀螺仪和加速度计的MEMS型惯性测量单元(IMU)和GPS单元,以精确和高鲁棒的方式跟踪摄像机在6个自由度下的运动,但在GPS信号在不好的时候会产生严重丢失问题。ZHANG Xiao等[18]将MEMS惯性器件与RatSLAM进行组合,弥补了惯性器件误差随时间累积等缺点。生物学家研究发现自然界中许多生物有着神奇的导航能力,仿生偏振导航就是仿照昆虫研制的一种新导航方式[19-23],具有自主性好,抗电磁干扰的优点。WU Jin等[24]将偏振光/地磁/GPS/SINS组合起来,成功应用到四悬翼无人机上,提高抗磁干扰能力。WANG Daobin等[25]将偏振光与UKF_SLAM进行组合,一定程度上提高了SLAM导航精度,但使用的是滤波方法,下一时刻位置只与上一时刻位置有关,缺少考虑其它时刻。综上所述,目前大多将惯性器件等传感器与SLAM组合,偏振光与SLAM组合大都为基于滤波的松耦合,不利于异常诊断,抗干扰能力弱,偏振光与SLAM紧耦合的研究鲜有报道。
本文在视觉SLAM导航算法的基础上,引入了仿生偏振定向传感器,设计了一种仿生偏振定向传感器/双目视觉传感器组合导航系统,利用图优化将多传感器数据进行融合,完成多传感器紧耦合,设计了基于车载导航实验平台的室外导航实验,并与传统SLAM导航进行对比,证明了该组合导航系统具有较强的抗干扰能力、自主性好、精度高,能够为地面汽车提供准确的导航信息,未来具有广阔应用前景,也为组合导航提供一种新思路。
1 天空偏振分布理论及仿生偏振导航传感器测向原理
研究发现大气中主要包含气体分子为78%的N2和21%的O2,固体颗粒为气溶胶和冰晶等成分,当太阳光进入大气中由于与空气中的气体分子和气溶胶碰撞发生散射作用,在天空中分布规律且稳定的偏振光,在晴朗无云的天气条件下,大气中小直径的粒子占多数,可以采用瑞利散射理论模型[26],本文主要针对晴朗天空使用瑞利散射理论建立偏振光场分布模型。
本文选用东北天坐标系为导航坐标系(n),右前上坐标系为载体坐标系(b),将仿生偏振定向传感器与载体系固联,定义三个姿态角为俯仰角()、横滚角()、航向角(),见图1。由瑞丽散射理论,天空任意一点的偏振方向(偏振
偏振
为仿生偏振定向传感器的体轴与观测点偏振
通过方向余弦矩阵可以实现b系和n系之间相互转换,载体坐标系到导航坐标系的方向余弦矩阵为,即
方向余弦矩阵展开为
由观测点为天顶点,则、,实际过程中如果不能正对天顶点会影响仿生偏振定向传感器的测量精度,文献[29]提出一种补偿算法,有效的提高传感器的角度输出精度。传感器视线方向可表示为
hs,fs分别表示太阳高度角和太阳方位角,太阳视线在导航坐标系下的投影坐标可以表示为
由瑞丽散射理论可知
2 多传感器组合导航算法
该融合方案的系统结构设计如图2所示,包括双目相机、仿生偏振定向传感器。其中,仿生偏振定向传感器可提供航向信息,双目相机通过视觉SLAM算法可解算得到位置、速度等姿态信息[30]。由于地面移动导航主要取决于航向角、速度和位置信息,而视觉SLAM在导航过程中航向角在连续转弯的情况下误差会很大,双目相机在环境不好的情况下会丢失速度信息。仿生偏振定向传感器输出的角度信息可以用来修正航向角,以视觉SLAM算法解算的航向角与仿生偏振定向传感器所测得的航向角之差作为优化变量,采用非线性优化将误差优化到最小,得到最接近真实情况的航向角,达到精确导航的目的。
2.1 仿生偏振定向传感器数据融合
仿生偏振定向传感器测量角度用来修正SLAM偏航角误差,在载体坐标系下得到对应的偏振矢量,坐标为,其中K可取。本次使用仿生偏振定向传感器的体轴方向与地球正北方向重合,K的取值1,初始时SLAM解算的航向角为,组合导航系统初始位姿为
相邻两帧与的仿生偏振定向传感器数据为、,仿生偏振定向传感器测量两帧之间的相对角度变化为
构建图优化项,两传感器数据偏差为
2.2 光流矢量剔除误匹配点
本文利用光流算法计算相邻帧匹配点位移矢量剔除误匹配点,在相邻两帧之间,得到tk-1到tk时刻之间导航系统所有特征点位移矢量定义为
获得tk-1到tk时刻光流矢量为,为匹配特征点的集合,设定阈值,如果在之间就认为是正确匹配的特征点,不在该范围内就认为是误匹配点或运动的物体进行剔除。
2.3 构建图优化
首先定义图优化的顶点与边,顶点作为要优化的变量,边表示误差项[31]。
构建4维优化变量分别为双目视觉传感器所测量特征点的X、Y、Z,以及两传感器测的航向角,具体定义为
式中,为相机运动每帧图像,即相机姿态,为相机观察到的路标,为一个地球坐标系下的点转换到相机坐标系下投影到图像上所得的像素坐标。
式中,z为路标特征点的投影深度,d为视差,b为基线,f为焦距,与分别为统一特征点在左目和右目上的像素坐标。
双目视觉传感器可以获得路标的像素坐标(u,v)和深度信息
式中,e为误差项,u、v为像素坐标,X、Y、Z为特征点三维坐标,f为横焦距,与为像素原点在图像平面上横纵向平移,b为基线,p为偏振光传感器所测航向角。
优化函数中包含两部分,第一部分表示观测值与真实之间的误差,第二部分表示SLAM航向角与偏振光测量角度之间的误差,其中,假设图像观测点符合高斯分布。为偏振光数据作用于后的运动参数,为运动方程的协方差矩阵。通过优化使得这两项误差和达到最小,可以解得一组最优、和。
最后,根据构建的BA优化函数,计算出最优的位置,姿态信息,实现多传感器信息融合,见图3。
3 组合导航平台设计与实验
3.1 实验平台
自主搭建组合导航实验平台如图4所示。组合导航平台的主体为两层分隔的铝合金板支架,用于安装传感器、电源等其它相关设备,其中控制系统采用笔记本电脑上搭建ROS机器人操作系统进行控制,ROS上搭建了双目视觉传感器驱动程序、偏振光传感器驱动程序、高精度的光纤惯导SPAN-CPT驱动软件和改进的SLAM程序,笔记本上搭载有NVIDIA GeForce GTX1050的GPU,可以快速处理双目视觉传感器采集到的图片信息,进行位姿解算;本课题组研发的偏振光传感器安装在平台的正上方,无遮挡,并且传感器体轴方向组合导航平台的正前方保持一致,通过USB串口与ROS之间进行数据传输,用来提供航向信息进行修正航向角;高精度的光纤惯导SPAN-CPT安装在组合导航平台中间层,它的GPS安装在组合导航平台上方,偏振光传感器旁边的位置上,它的测量值作为偏振光/SLAM组合导航控制系统姿态的标准参考信息;移动电源可以为整个导航系统提供电能,提高续航能力。组合导航平台使用传感器性能参数如表1所示。
表 1. 传感器参数
Table 1. Sensor parameters
|
3.2 室外导航实验与结果
室外车载导航实验时,任何传感器的误差都会影响导航的精度,对于地面导航系统,航向误差对整个导航系统的影响更为突出,一旦航向角出现误差会使载体轨迹与实际行驶轨迹发生严重偏离,并且随着时间的累积,偏离程度会越来越大。偏振光传感器可以为地面载体导航提供航向信息,并且自主性好,无累积误差,抗干扰能力强,可以稳定准确的提高导航精度。
本文采用自主搭建的仿生组合导航实验平台分别进行角度对比试验和车载导航实验,在有连续转弯的环境条件下分别进行了基于偏振光/双目视觉的组合导航系统和传统双目视觉SLAM导航系统的对比实验,描述如下。
3.2.1 角度对比实验
实验地点选择大连理工大学机械东楼停车场,经纬度分别为北纬38.880 383°,东经121.532 249°,时间为11月26日下午3点30分,天气晴朗,太阳高度角为9.75°,太阳方位角为52.73°,本文中该实验分别将偏振光传感器和双目视觉传感器固定在精密转台上,电脑给定转台标准角度,分别记录双目视觉传感器解算的角度和偏振光传感器所测量的航向角度。实验中,转台连续缓慢转从0°开始旋转到180°,再继续从-180°转到0°,每经过90°停止30 s,连续旋转一周,如图5所示。
图6是不同传感器解算的航向角。可以明显看出,采用偏振光解算出航向角误差小于使用双目视觉传感器解算出的航向角,在进行角度测试时,一方面因为误差导致,而另一方面双目视觉SLAM在120~160 s和200~220 s时因为采集画面时远处有车辆行驶,造成双目视觉传感器在解算角度时出现较大误差,角度误差对比数据如表2所示。
表 2. 两种导航方式下误差对比
Table 2. Result of two type integrated navigation system
|
可以看出偏振光的导航解算航向角精度较传统双目视觉SLAM导航解算航向角精度提高了。实验结果也说明了在环境中有运动物体干扰对SLAM角度解算会产生较大影响,偏振光传感器则具有较强的抗干扰能力,实时性好,但只能提供二维航向角信息,将偏振光传感器应用于双目视觉SLAM系统上,可以为SLAM提供更精确的航向角信息。
3.2.2 室外车载导航实验
实验地点选择辽宁省大连市甘井子区高新园小平岛别墅中心广场,经纬度分别为北纬38.832°,东经121.503°。实验时间为2020年11月11日下午3点,天气晴朗,太阳高度角为16.50°,太阳方位角为50.09°。实验实际轨迹是一个半径为70 m的近似圆形,周长约为377 m,绕行5圈,车载平台如图7所示,实验记录地点如图8所示。
实验数据记录采用ROS机器人操作系统的rosbag功能包,同时录制偏振光传感器、双目视觉传感器和SPAN-CPT实时数据,偏振光传感器采用20 Hz,双目视觉传感器采用10 Hz, SPAN-CPT采用100 Hz,SPAN-CPT可作为组合导航系统姿态的标准参考信息,录制数据集可以线下实时重复播放模拟实际场景,避免实际运行时每次轨迹不一致对不同组合导航系统性能评估造成影响。
车载导航轨迹如图9所示,航向角数据如图10所示,黑色线条表示理论轨迹和理论航向角,红色轨迹表示偏振光/双目视觉传感器组合导航系统的轨迹和航向角,蓝色轨迹表示双目视觉SLAM导航的轨迹和航向角。SLAM轨迹评估主要利用ATE(绝对轨迹误差)和RPE(相对位姿误差)两种方法。评估方法描述如下:ATE通过直接计算SPAN-CPT位姿的真实值分别与偏振光/SLAM组合导航系统和视觉SLAM导航系统估计值之间的差,首先根据位姿的时间戳将SPAN-CPT真实轨迹和组合导航系统估计值进行对齐,然后计算每对位姿之间的差值。RPE用于计算相同两个时间戳上的位姿变化量的差,在用时间戳对齐之后,SPAN-CPT真实位姿和估计位姿每隔1 m计算位姿变化量,然后对该变化量做差,可以计算组合导航系统的飘移和轨迹误差,本文主要基于RPE性能评估,评估如图11~12所示。
角度和位置对比如表3~4所示,可以得到偏振光/SLAM组合导航系统的平均位置误差为0.83 m,平均角度误差为0.33°,纯视觉SLAM导航系统的平均位置误差为0.91 m,平均角度误差为0.59°,在车载导航轨迹图(-75,-30)处,航向角图(200,-90)处因为环境特征点少、纯视觉SLAM导航的数据波动较大,对比采用偏振光/双目视觉组合导航系统的轨迹具有明显偏差,在环境特征点少角度解算误差大的时候偏振光/SLAM组合导航系统不受影响。通过图10和表3可以看出在连续转角的情况下,相比于纯视觉SALM轨迹精度提高了8.6%,航向角精度提高了38.6%,由上述实验可以看出采用偏振光/SLAM组合导航系统具有较高的精度,且实时性好、误差不随时间累积。
表 3. 两种导航方式下角度误差对比
Table 3. Comparison of angle error in two navigation modes
|
表 4. 两种导航方式下位置误差对比
Table 4. Comparison of position error in two navigation modes
|
4 结论
本文提出了一种仿生偏振光传感器与SLAM系统中多传感器融合的方法,设计了基于偏振光/双目视觉传感器的组合导航算法,搭建了组合导航实验平台,在室外进行了车载实验,并与传统视觉SLAM导航方式进行了对比。实验结果表明:传统SLAM导航系统平均位置误差与角度误差分别为0.91 m、0.54°,偏振光/双目视觉传感器组合导航系统平均位置误差与角度误差分别为0.83 m、0.33°。航向角精度较传统SLAM导航系统提高了38.9%,位置精度提高了8.9%,可以证明偏振光/双目视觉传感器组合导航系统优于传统纯视觉SLAM导航精度。
本文提出的组合导航方法可以用于地面二维空间导航控制。基于偏振光/双目视觉传感器的组合导航与传统视觉SLAM导航相比,可以显著提高导航精度,一定程度上解决了视觉SLAM连续转角误差大和纯旋转不能初始化的问题,使用偏振光传感器后系统具有实时性好、抗干扰能力强等优点,同时该组合导航系统使用纯光学器件,更加接近生物导航机理,为仿生导航提供了一些参考,目前点源式仿生偏振定向传感器适用于晴朗无云的环境条件下,少量云雾对该传感器的影响不大。光线强弱只会在传感器正对太阳光时候有影响,在阴天和多云天气条件会存在影响,后续工作研究穿透性较强的紫外波段偏振光[32]、成像式偏振光传感器以及去云算法的研究,以增强偏振光传感器的使用范围。
[2] 魏伟, 武云云. 惯性/天文/卫星组合导航技术的现状与展望[J]. 现代导航, 2014, 5(1): 62-65.
WEI Wei, WU Yunyun. Status and prospect of INS/CNS/GNSS integrated navigation technology[J]. Modern Navigation, 2014, 5(1): 62-65.
[3]
[4] 周军, 葛致磊, 施桂国. 地磁导航发展与关键技术[J]. 宇航学报, 2008, 29(5): 1467-1472.
ZHOU Jun, GE Zhilei, SHI Guiguo, 等. Key technique and development for geomagnetic navigation[J]. Journal of Astronautics, 2008, 29(5): 1467-1472.
[5] 刘艳亮, 张海平, 徐彦田. 全球卫星导航系统的现状与进展[J]. 导航定位学报, 2019, 7(1): 18-21.
LIU Yanliang, ZHENG Haiping, XU Yantian, 等. Development status and trend of global navigation satellite system[J]. Journal of Navigation and Positioning, 2019, 7(1): 18-21.
[6] 赵云秀, 权巍, 韩成. 基于立体舒适度的立体图像感知距离估计[J]. 光子学报, 2020, 49(2): 0215001.
[7] Past CADENA C.. Toward the robust-perception age[J]. IEEE Transactions on Robotics, 2016, 32(6): 1309-1332.
[8] 许允喜. 基于广义正交迭代算法的立体视觉定位[J]. 光子学报, 2011, 40(8): 1225-1230.
[12] DAVISON A J. Mono SLAM: Real-time single camera SLAM[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 2007(6): 1052-1067.
[13] LIN Y, GAO F, QIN T, et al. Autonomous aerial navigation using monocular visual-inertial fusion[J]. Journal of Field Robotics, 2017, 34(12): 21732.
[14] MUR-ARTAL R, TARDOS J D. ORB-SLAM2: an open-source SLAM system for monocular, stereo and RGB-D Cameras[J]. IEEE Transactions on Robotics, 2016, 33(5): 1255-1262.
[16] DISSANAYAKE G, NEWMAN P, CLARK S, et al. A solution to the simultaneous localization and map building (SLAM) problem[J]. IEEE Robotics & Automation Magazine, 2013, 17(3): 229-241.
[18] 张潇, 胡小平, 张礼廉. 一种改进的RatSLAM仿生导航算法[J]. 导航与控制, 2015, 14(5): 73-79,47.
ZHANG Xiao, HU Xiaoping, ZHANG Lilian, 等. An improved bionic navigation algorithm based on RatSLAM[J]. Navigation and Control, 2015, 14(5): 73-79,47.
[19] WEHNER R, GALLIZZI K, FREI C, et al. Calibration processes in desert ant navigation: vector courses and systematic search[J]. Journal of Comparative Physiology A: Sensory, 2002, 188(9): 683-693.
[21] REPPERT S M, ZHU H, WHITE R H. Polarized light helps monarch butterflies navigate[J]. Current Biology, 2004, 14(2): 155-158.
[22] DACKE M, DOAN T A, O'CARROLL D C. Polarized light detection in spiders[J]. Journal of Experimental Biology, 2001, 204(14): 2481-2490.
[23] 王光辉, 郭正东, 朱海. 偏振光天文导航定位能力分析[J]. 光子学报, 2012, 41(1): 11-14.
[24] 褚金奎, 武进, 李金山. 偏振光传感器在四旋翼飞行器中的应用[J]. 电子学报, 2020, 48(1): 198-203.
CHU Jinkui, WU Jin, LI Jinshan, 等. Application of polarization sensor in quadrotor helicopter[J]. Acta Electronica Sinica, 2020, 48(1): 198-203.
[26] 赵开春, 褚金奎, 姚弘轶. Rayleigh大气天空光偏振分布仿真与预测[J]. 四川大学学报:工程科学版, 2007, 59(s1): 287-291.
ZHAO Kaichun, CHU jinkui, YAO Hongyi, 等. Simulation and prediction for rayleigh skylight polarization distribution[J]. Advanced Engineering Sciences, 2007, 59(s1): 287-291.
[27] 褚金奎, 赵开春. 仿昆虫复眼偏振敏感测角光电模型研究[J]. 微纳电子技术, 2005(12): 541-545.
ZHAO Kaichun, CHU jinkui. Study of angle measurement optoelectronic model on emulating polarization-sensitive compound eye of insect[J]. Micronanoelectronic Technology, 2005(12): 541-545.
[29] ZHI W, CHU J K, LI J S, et al. A novel attitude determination system aided by polarization sensor[J]. Sensors, 2018, 18(1): 158.
[30] MUR-ARTAL R, TARDOS J D. ORB-SLAM2: an open-source SLAM system for monocular, stereo and RGB-D cameras[J]. IEEE Transactions on Robotics, 2016, 33(5): 1255-1262.
[32] ANDRAS B, GABOR H. Why is it advantageous for animals to detect celestial polarization in the ultraviolet? Skylight polarization under clouds and canopies is strongest in the UV[J]. Journal of Theoretical Biology, 2004, 226(4): 429-437.
Article Outline
褚金奎, 陈建华, 李金山, 佟坤, 李晋, 胡瀚珮. 一种偏振光/双目视觉仿生组合导航方法[J]. 光子学报, 2021, 50(5): 184. Jinkui CHU, Jianhua CHEN, Jinshan LI, Kun TONG, Jin LI, Hanpei HU. Polarized Light/binocular Vision Bionic Integrated Navigation Method[J]. ACTA PHOTONICA SINICA, 2021, 50(5): 184.