基于惯性/偏振光/光流的六足步行机器人自主导航方法研究

曾云豪，杜 涛，星 艳

(北方工业大学信息学院，北京 110144)

0 引言

近年来，导航技术在航空、航天、航海、地面交通、测绘等领域已经得到广泛应用。目前，应用最广泛的组合导航系统为惯性导航系统(Inertial Navigation System，INS)与全球卫星导航系统(Global Navigation Satellite System，GNSS)的组合[1]，即INS/GNSS组合导航系统。在INS/GNSS组合导航系统中，其导航自主性受限于GNSS信号的通信传输，在面临城市峡谷等遮挡或干扰环境时，GNSS信号可能发生丢失或多径效应，造成导航系统精度下降，甚至无法正常工作[1]。

近年来，学者发现自然界中某些生物具备高超且精准的导航定位能力。例如，沙蚁拥有利用天空偏振光完成长达数百米的随机觅食与直线归巢的能力[3]；帝王蝶可以借助天空偏振光实现长达数千千米的迁徙导航[4]。以上研究表明,沙蚁和帝王蝶具备精准感知大气偏振光的能力，可以借助天空中稳定的偏振分布模式获取导航信息，实现高精度、长时间的导航与定位[5-6]。

受生物导航原理的启发，国内外学者开展了仿生偏振光导航方法研究。2007年，卢鸿谦等[7]提出了偏振光/GPS/SINS组合导航方法，利用偏振光实现了对GPS/SINS组合导航的辅助，提高了组合导航系统的可观测度和精度。2015年，胡小平等[8]对偏振光定向算法进行了误差分析，结果表明偏振光定向误差主要来自仿生偏振光传感器误差和载体的水平角误差。2019年，何晓峰等[9]针对地面无人平台，提出了仿生偏振光定向算法，实现了地面无人平台航向角确定。2019年，Dupeyroux等[10-11]对沙蚁的导航策略开展了研究，研制了仿沙蚁机器人，复现了沙蚁的运动模式和导航策略。2020年，褚金奎等[12]基于动态步长搜索机制，提出了基于天空光偏振模式的导航姿态最优化解算方法，进一步提高了偏振光导航系统的稳定性与可靠性。

本文从提高组合导航系统的自主性出发，引入仿生偏振光传感器和光流传感器，以辅助惯性导航系统，设计了无迹Kalman滤波(Unscented Kalman Filter, UKF)方法，并应用在六足步行机器人上。

1 六足机器人组合导航系统建模

为了实现六足步行机器人的组合导航，首先，分别建立了基于机器人运动学的状态模型和基于仿生偏振光、光流传感器的量测模型；其次，建立了惯性/偏振光/光流的组合导航系统模型。

1.1 六足步行机器人运动学建模

文中涉及的坐标系为机器人载体坐标系(以载体质心为中心，载体运动方向右方为x轴，前方为y轴，上方为z轴，构成右手坐标系)，即b系；全局坐标系(以初始时刻载体质心为中心，初始时刻载体运动方向右方为x轴，前方为y轴，上方为z轴，构成右手坐标系)，即w系。机器人位于起始位置时，假定w系和b系重合;同时假定偏振光传感器和光流传感器与六足步行机器人的安装误差和杆臂效应等已经被标定补偿，偏振光传感器和光流传感器安装后的测量坐标系与载体坐标系重合。

Xk=f(Xk-1,uk-1)+wk-1

(1)

其中

1.2 基于仿生偏振光传感器量测方程建模

天气晴朗时，天空偏振光具有稳定的分布模式：不同时间，不同地点，偏振角和偏振度各不相同；而在同一时间，同一地点，同一观测方向，偏振角和偏振度具有可重复性[14]。基于偏振敏感(Polarization Sensitive, POL)神经元模型[15]的六通道光电偏振光传感器(图1所示)，其中每2个通道构成一个对立的偏振单元，类似于POL神经元，可以获取偏振角和偏振度等信息。单个通道偏振传感器量测方程如下

Ii(φ)=KIt(1+dcos(φ-αi)),i=1, 2,…, 6

(2)

其中，Ii(φ) 是第i通道光电二极管的输出强度；K是未知尺度因子，需要标定；It是总光强度；d为偏振度；φ为偏振方位角；αi是传感器的偏振方向和参考方向之间的角度，由式(3)给出

(3)

图1 六通道光电偏振光传感器Fig.1 Six-channel photoelectric polarized light sensor

在不同的天气条件下，It具有不同的值，It值的变化可能会限制其在不同环境条件下的通用性。实际上，许多昆虫复眼具有偏振对立结构，可以消除不同环境下由于总光强不同带来的影响。受偏振对立单元结构的启发，将式(2)与式(3)结合，得到

(4)

(5)

(6)

其中，p1、p2和p3为偏振对立单元调整到0、π/3和2π/3这3个方向的输出。

(7)

(8)

式(7)和式(8)即为获取的偏振度和偏振方位角信息。针对二维平面运动，根据文献[16]，太阳方位角αs与偏振光传感器测量的偏振方位角φ、航向角θ的关系如下

(9)

其中，太阳方位角αs由文献[17]方法得到：当αs≥π/2时，β=1；αs<π/2时，β=-1。

k时刻偏振光传感器的量测方程为

zp, k=θk+vp, k

(10)

其中，量测zp,k=φk+β(αs-π/2)∈R；θk为k时刻航向角，可以由姿态四元数转换得到；vp,k为服从零均值，方差为Rp,k的高斯噪声。

1.3 基于光流传感器的量测方程建模

光流传感器基于像素运动场与载体速度之间的关系，获得载体运动速度，具体原理过程如下[18]。

记相机参考坐标系点Pc=[Xc,Yc,Zc]T，其在像素平面的投影puv为

(11)

其中，puv=[xuv,yuv,f]T，f为相机焦距；相机和Pc之间的相对运动为

Vc=-T-ωc×Pc

(12)

其中，ωc∈R3为角速度，T∈R3为运动的平移分量，对式(11)求导得

(13)

其中，Vc,z为Vc的z轴分量。式(13)中，将vuv根据焦距和传感器到场景距离进行缩放，得到平移速度vm[18]

(14)

其中，D可以由Pix4flow光流传感器获得，vm中包含对地垂直方向速度。由此，光流传感器的量测方程为

zof,k=CVk+vof,k

(15)

其中，量测zof,k=vm∈R3；C为k时刻全局参考系到载体系的转换；Vk为k时刻全局参考系中机器人运动速度；vof, k为服从零均值，方差为Rof, k的量测噪声。

综上所述，惯性/偏振光/光流组合导航系量测方程为

zk=h(Xk)+vk

(16)

由式(1) 和式(16) 可得惯性/偏振光/光流组合导航系统方程。

2 UKF设计

针对惯性/偏振光/光流组合导航系统方程，设计了UKF估计六足步行机器人的位姿。

UKF如式(17)～式(24)所示，主要流程分为以下2个步骤[13]。

(1)状态预测：

1)构造2n+1，n=10的sigma点集

2)将sigma点集通过状态转移函数映射到新的点集上

(17)

3)将新的sigma点集加权后用于状态的一步预测和协方差预测

(18)

(19)

(2)量测更新：

1)生成总数为2n+1的sigma点集

2)将新的sigma点集通过量测函数映射到新的点集上

3)将新的sigma点集加权后用于量测的估计值和协方差预测

(20)

4)计算UKF增益

(21)

(22)

5)更新系统状态和协方差矩阵

(23)

(24)

其中

3 仿真验证

为验证惯性/偏振光/光流组合导航算法的有效性，以六足步行机器人的位姿估计为对象，模拟了变加速度和变角速度仿真运动数据，且仿真数据中惯性测量单元(Inertial Measurement Unit, IMU)数据频率为100Hz，光流传感器数据频率为50Hz，偏振光传感器数据频率为20Hz，设计UKF，并与惯性/光流组合导航系统位姿估计进行对比。

在UKF过程中，系统噪声方差矩阵设置为

Qk-1=diag([0.1 0.1 0.1 0.06 0.06 0.06

0.03 0.03 0.03 0.03])2

速度量测噪声的协方差为

航向角量测噪声的协方差为

Rp,k=0.062

六足步行机器人的位置、速度和航向角估计以及估计误差如图2～图7所示。

图2 位置估计Fig.2 Position estimation

图3 速度估计Fig.3 Velocity estimation

图4 航向角估计Fig.4 Heading angle estimation

图5 位置估计误差Fig.5 Position estimation errors

图6 速度估计误差Fig.6 Velocity estimation errors

图7 航向角估计误差Fig.7 Heading angle estimation error

为说明引入偏振光量测信息后组合导航系统位姿估计的改进效果，本文对比了惯性/偏振光/光流组合导航系统和惯性/光流组合导航系统两种组合导航方法。为了消除随机噪声的影响，开展了50次蒙特卡罗仿真，结果如表1所示。

表1 50次实验平均均方根误差

仿真结果表明，在使用UKF算法的情况下，与惯性/光流组合导航系统相比，惯性/偏振光/光流组合导航系统在位置、速度和航向角等方面的估计值有明显改进，且更接近基准值，而惯性/光流组合导航系统仅速度估计接近基准值。从表1中可以得到，基于惯性/偏振光/光流的组合导航系统定位均方根误差在10cm左右，航向角均方根误差在0.05rad左右；而无偏振光量测的基于惯性/光流的组合导航系统定位均方根误差在40cm左右，航向角均方根误差在0.2rad左右。

4 六足机器人设计与实验

4.1 六足步行机器人设计

为了进一步验证基于惯性/偏振光/光流的六足步行机器人的自主导航方法，本文搭建了低成本六足机器人实验平台(图8所示)。机器人共有六足，每条腿均有3个关节，由18个MG996舵机驱动，安装在计算机数控(Computer Numerical Control，CNC)加工的结构件中。每个舵机的动作由Jetson Nano发送指令到18通道舵机控制板产生。六足机器人由 NIVIDIA Jetson Nano 微型计算机控制，所有传感器的信息都在此处理，其总体架构如图9所示。

图8 六足步行机器人Fig.8 Hexapod robot

图9 硬件架构Fig.9 Hardware architecture

4.2 室外实验

室外实验环境地点为北方工业大学运动场入口道路，在长为2.4m，宽为2.4m，地面纹理清晰的区域中进行，实验时间为15∶00，天气条件晴朗无云，实验场地无遮挡。组合导航系统中采用消费级IMU，型号ICM-20602,其性能参数为：陀螺零偏 1(°)/s，加速计零偏40mg。机器人运动路径如图10所示，沿2.4m×2.4m正方形路径运动，并返回起点。

从图11和图12可以看出，六足机器人的位姿估计效果较好。在无直接位置量测的情况下，其估计路径与预设运动路径接近。实测实验结果表明，基于惯性/偏振光/光流的六足步行机器人的自主导航方法性能优异，可实际应用。

图10 预设机器人运动路径Fig.10 Preset robot motion path

图11 实时显示滤波后的路径Fig.11 Real time display of filtered path

图12 路径估计Fig.12 Estimated paths

5 结论

本文针对常见的组合导航系统依赖无线电信号，系统自主性不足的问题，提出了一种基于惯性/偏振光/光流的六足步行机器人自主导航方法。分别建立了基于偏振光传感器和光流传感器的量测方程，并开展了仿真与实验，结果表明：

1)引入误差不随时间累积的偏振光传感器量

测信息可提升导航系统的航向角估计精度，并间接提升位置与速度估计精度。

2)本文利用无需外部信号源的传感器，提出了基于惯性/偏振光/光流的六足步行机器人自主导航方法，可有效降低位姿估计的累计误差，并具备高实时性和强自主性。

在未来研究中，将进一步考虑六足步行机器人的动力学模型，研究传感器误差等因素，以适应高精度导航的要求，并开展不同天气、不同光照、障碍物遮挡的情况对导航精度影响的研究，以及如何降低环境变化对偏振光导航精度的影响。