基于多源信息紧耦合的四足机器人平面导航方法

褚金奎,尉 达,张钟元,于 灏,宋 涛

(大连理工大学机械工程学院,辽宁大连 116024)

0 引言

随着科技的发展,智能移动机器人在服务业[1]、制造业[2]以及星际探索等领域已经获得了广泛的应用。传统的轮式机器人在实际应用过程中,越障能力差、地形适应能力差、转弯效率低,容易打滑,不够平稳,面对复杂地形时不能正常工作[3-5]。足式机器人的使用场景广泛与复杂,几乎可以适应各种地形,能够跨越障碍,有着良好的自由度,同时动作灵活、自如、稳定,在环境探索、灾害搜救中起到至关重要的作用[6-7]。然而目前绝大多数的四足仿生机器人都不携带自主导航系统,完全由人工实现远程操控,但随着机器人工作多样化,操作复杂化,解决这一问题变得重要起来。航姿参考系统(attitude and heading reference system,AHRS)是以电子罗盘修正陀螺仪角速度积分从而得到航向角,然而电子罗盘易受到磁场干扰,同时陀螺仪的零漂误差会随时间积累[8],所以在卫星拒止,电场干扰以及需要长时间运行的复杂环境中难以获得精确稳定的航向信息,鲁棒性较差[9]。

偏振光导航[10]是一种新兴导航方式,借鉴昆虫利用天空偏振光进行导航[11]的思路发展而来,具有无误差积累、实时性好的特点。因此,将偏振光传感器与其他传感器相结合来提高导航性能,受到国内外学者的关注。2009年,王洪青[12]搭建了轮式自主移动机器人平台,并且采用偏振光传感器实现其室外导航功能;2011年,Chahl等[13]根据蜻蜓复眼结构设计了天空偏振罗盘,并在无人机飞行测试中利用罗盘保持航向;2015年,支炜等[14]提出了一种偏振光及红外传感器辅助惯导定姿方法,利用偏振光传感器与红外传感器在惯性导航系统中进行修正位置与姿态信息解算,并且依据常见的惯导系统参数进行了动态仿真;2019年,Dupeyroux等[15]基于紫外光线性偏振设计了天体罗盘,并利用罗盘在六足步行机器人的直线行走实验上保持航向恒定;2020年,褚金奎等[16]实现了偏振光传感器在实际三维空间中的导航应用,并利用传感器完成了四旋翼飞行器的导航控制;同年褚金奎等[17]设计了一种偏振光传感器,微惯性测量单元及全球定位系统(global positioning system,GPS)的组合导航系统,并且实现无人船的自主导航与编队航行。

上述研究中,其导航方法利用纯偏振光获得航向信息,或采用松耦合方式,分别处理偏振光传感器与其他传感器数据后再做置信融合,并未将其优势完全发挥出来。偏振光传感器具有瞬时性,不存在累计误差,但探测范围存在2个半区,设备启动工作时不能靠自身判断出当前所在半区,而存在于惯性系统中的磁力计可以解决这个问题,同时偏振光传感器要正对天顶使用,对于近距离掠过其上方的飞行物感应较强烈,鲁棒性较差,易被环境干扰。2种导航系统各有一定的优缺点,利用各自传感器的原始信息数据,将两者紧耦合,以偏振光信息为主,惯性测量单元(inertial measurement unit,IMU)作为辅助,可以弥补各自的不足并发挥自身的优越性。

本文为了实现四足机器人在卫星拒止环境中的自主导航,并提升导航系统的抗干扰性。首先设计了偏振光传感器与九轴AHRS的紧耦合导航算法;然后,设计实验验证了该导航算法的抗环境干扰能力;最后,基于上述组合导航算法,与传统地磁惯性和偏振光导航方式进行了四足机器人的循迹对比实验,实验表明组合导航算法在光干扰和电磁环境干扰的情况下可以正常工作,并且循迹路径精度得到有效提升,验证了组合导航系统可以实现卫星拒止环境中的四足机器人自主导航。

1 偏振光导航原理与多源信息紧耦合方法

1.1 偏振光导航原理

太阳光在传播过程中遇到大气中的各种粒子时会发生散射现象,在天空形成稳定的偏振分布模式,利用大气瑞利散射理论模型,可以建立全天空在晴朗无云条件下的理想大气偏振模式。在某一时刻天空中任意一点光偏振方向即E矢量总是垂直于由太阳S、观测点O和被观测点P组成的平面。倘若处于地面O点的观察者可以探测出天空某点的E矢量方向与自身方向的几何关系,就可以确定出自身的方向。

本文选用东北天坐标系为导航坐标系,偏振光传感器固联于载体坐标系为右前上坐标系(m),图1中Hs为太阳高度角,As为太阳方位角,E矢量为传感器观测点处光偏振方向,α为观测点处光偏振方向与传感器体轴夹角。

图1 偏振光导航原理图

(1)

(2)

太阳视线在导航坐标系下的投影为

(3)

(4)

1.2 偏振光传感器与九轴AHRS紧耦合算法

多源紧耦合算法设计如图2所示,包括偏振光传感器、陀螺仪、加速度计和磁力计。由于路径导航主要依赖于航向角以及位置信息,偏振光传感器提供的航向角鲁棒性差,对环境干扰敏感,基于卡尔曼滤波,提取各传感器原始数据,利用陀螺仪输出角速度进行角度预测与补偿,能够大幅提高其鲁棒性,在实际使用过程中,如果不能正对天顶会影响偏振光传感器的测量精度。根据Sabatelli等[18]提出的基于扩展卡尔曼滤波(extended Kalman filter,EKF)的IMU姿态角解算方法,计算出当前的翻滚角r、俯仰角p和磁航角y,然后根据Zhi等[19]提出的一种偏振光传感器姿态补偿算法进行航向修正,该方法可有效提高传感器的角度输出精度。磁航角可以在设备开机时帮助修正偏振光传感器的二义性:当输出值为0～1时,置偏振光传感器为上半区0～180°;输出值为-1～0时,置偏振光传感器为下半区180°～360°。

图2 多源紧耦合导航算法模型流程

在进行耦合时要解决偏振光传感器的半区映射问题:偏振光传感器只能输出0～180°的数据,所以在对应全方向上存在2个半区;相反的,陀螺仪的Z轴输出是全向变化的。所以需要将偏振光传感器数据对应映射到陀螺仪中。并且卡尔曼滤波的主要作用是去除跳变点,但是在传感器进行半区转换时,存在正常的跳变点,这种跳变点并不是由于环境干扰而产生的,所以相应的卡尔曼滤波不能将这种跳变点进行过滤。为了解决这些问题,首先设置了半区符号用来识别当前半区,接下来规定了有效卡尔曼滤波区间,其中滤波范围为10°～170°和190°～350°,其余范围是入跳变检测区域,当进入跳变检测区域时,关闭卡尔曼滤波,直接采用测量值,滤波范围可以随着采样频率的增加而扩大。映射流程如图3所示,过程中初始化卡尔曼滤波参数的目的在于半区转换后重新进入滤波范围时,滤波器的中间参数还停留在离开滤波范围的时刻,初始化可以防止中间参数对于新的滤波结果造成影响。

图3 半区映射流程

在实际应用过程中,偏振光传感器通常与载体固联安装,并不会始终保持水平,利用AHRS解算的姿态角进行偏振角补偿,之后利用外部的UTC时钟以及经纬度信息得到太阳高度角以及太阳方位角进行航向角求解。首先计算太阳信息:

sinHs=sinφ·sinδ+cosφ·cosδ·cost

(5)

cosAs=(sinHs·sinφ-sinδ)/(cosHs·cosφ)

(6)

式中:φ为地理纬度;δ为太阳赤纬;t为时角。

之后进行姿态补偿,求得航向角为

Ψ=arcsinx+arcsiny

(7)

x,y表达式分别为:

(8)

(9)

式中:α为前一步紧耦合计算出的偏振角;r和p分别为解算后的翻滚角与俯仰角。

1.3 偏振光传感器与陀螺仪的卡尔曼滤波方法

偏振光传感器虽然没有累计误差,但其对环境干扰非常灵敏,环境中的干扰会使解算数据有很大的波动,鲁棒性较差,因此在融合过程中,基于卡尔曼滤波,利用陀螺仪测量的Z轴角速度作为修正值,可以很大程度上解决环境噪音干扰的问题,有效提高了偏振光传感器的鲁棒性。卡尔曼滤波过程如图4所示。

图4 卡尔曼滤波过程

(10)

(11)

(12)

式中:Qangle为陀螺仪噪声协方差;Qbias为偏移噪声协方差。

系统的测量方程为Zt=HXt+Vt,其中测量值即为偏振光传感器解算值,由于偏振光传感器的噪声已经包含进输出值中,所以Vt=0,可以得出系统测量参数H=[1 0]T,根据卡尔曼增益计算公式,可以求得增益为

(13)

式中Rangle为偏振光传感器的角度测量噪声协方差。

根据最优值计算公式,可求得当前角度与误差最优估计值为:

(14)

(15)

式中measure为偏振光传感器解算值。

2 多源融合与四足机器人循迹实验

2.1 多源融合实验

根据传感器融合方法,首先设计半区映射采样实验,将偏振光传感器放在室外平台上,手动旋转平台多圈,利用机器人操作系统(robot operating system,ROS)进行数据采集并绘制映射前与映射后的偏振角,结果如图5所示。图中虚线为偏振光测量角度,实线为映射后的输出角度,可以看出2个半区被映射到了0～360°的范围上,并且跳变点也没有被卡尔曼滤波处理掉。

图5 半区映射结果

接下来测试组合传感器的抗环境光干扰能力,实验时将传感器静止平放置于室外,然后人为的在传感器上方多次制造光干扰或者遮挡,模拟真实环境中掠过传感器上方的物体造成的影响,实验中卡尔曼滤波器设置陀螺仪噪声协方差Qangle为0.001(°)/h,偏移噪声协方差Qbias为0.003(°)/h,卡尔曼增益中,根据偏振光传感器参数设置角度测量噪声协方差Rangle为0.03(°)/h。利用ROS进行数据采集并绘制融合前后的角度值,如图6所示。其中虚线为融合前偏振光传感器输出角度值,突变点为人为干扰点,可见融合后的突变大幅减小,角度值趋于稳定,标准差较融合前减少了64.8%。融合前后的误差结果对比如表1所示。

表1 环境光干扰误差对比 (°)

图6 环境光干扰输出对比

接下来测试磁干扰环境中组合传感器的鲁棒性,静置组合传感器并在其周围设置移动的干扰磁铁来模拟环境中的电磁干扰,利用ROS采集并输出AHRS解算磁航角以及融合后输出航向角,将其映射到相同量程中绘制曲线,输出如图7所示。由于AHRS对环境中电磁信息极为敏感,所以在扰动处产生了很大波动,但是多源融合算法以偏振信息为主,磁力计只起到了开机判断二义性的作用,因此电磁干扰对于输出基本没有影响。误差对比如表2所示。

表2 磁干扰误差对比 (°)

图7 磁干扰输出对比

实验可见,多源融合算法,基于卡尔曼滤波利用陀螺仪进行了修正,因此环境中的光遮挡而产生的突变点能够被平滑处理,并且由于融合方法中以偏振信息为主导,所以能够抵抗环境的电磁干扰。实验表明,组合传感器在正常进行半区转换的同时也有效提高了鲁棒性。

2.2 四足机器人循迹实验

四足机器人实验平台采用Unitree A1型号四足机器人,能够满足崎岖路面行走的条件,四足机器人搭载了Nvidia-TX2控制板,提供外部开发者接口。在机器狗顶部搭载了Xsens-MTi-1惯性传感器(包含三轴陀螺仪、三轴加速度计、三轴磁力计)进行惯性位姿解算;搭载了偏振光传感器进行偏振方向解算;搭载无线模块,与上位机进行远程连接与信息交互;遥控器用来切换程序控制与手动控制。四足机器人实验平台如图8所示,组成部分各具体参数如表3所示。

表3 组合导航系统传感器与平台参数信息

图8 实验平台

实验地点为大连理工大学西部校区知方楼正门口广场(121°30′37″E,38°52′49″N)。实验时间为2022年10月18日16时30分。实验预设轨迹为10 m的正方形,四足机器人前进速度设定为0.6 m/s,转向速度设定为0.3 rad/s。实验如图9所示。

图9 循迹实验

本次实验共设置了3个实验组,沿预设轨迹连续行走2圈,采用的平面导航方法是基于路径的航迹推算,其中路径策略分为直线行进与直角转弯两部分,直线行进部分采用内部路径积分的方式,规定了机器人的前进速度与前进时间,用来限定前进距离,在前进过程中,利用PID控制实时修正前进航向,保证直线前进;直角转弯的部分采用航向检测的方式,检测到当前航向与转向前航向差值大于90°时完成转弯。实验中组一使用九轴AHRS解算磁航角作为航向信息,组二使用偏振光传感器解算的偏振角作为航向信息,组三使用“偏振光+惯性+地磁”多源融合方式决定航向。

本文使用基于视觉的方法进行运动检测,并绘制可量化的机器人运动轨迹。实验过程中采用大疆四旋翼无人机,型号为经纬M300RTK,进行高空定点航拍。首先对航拍视频进行预处理,包括调整分辨率与边界范围;之后利用第三方库openCV识别视频中运动物体,生成并且显示该运动物体在视频当前帧中的坐标与轨迹,视频运动物体捕捉如图10所示,该坐标点即为四足机器人相对于视频边框的坐标;由于无人机在航拍时存在抖动与镜头漂移,所以需要将该坐标转换为相对于地面的绝对坐标,利用实验过程中摆放在场地3个角的绿色飞盘,使用openCV提取当前帧的飞盘坐标,联合记录的最初帧飞盘坐标计算当前帧的旋转矩阵,然后利用旋转矩阵反算出当前帧中机器人相对地面的绝对坐标;最后利用生成的坐标绘制路径轨迹,如图11所示。该方法不仅解决了小环境下GPS定位误差大的问题,还相较滴水或者漏沙的方法,量化了路径轨迹,为误差分析提供了有效数据。

图10 在航拍中生成的路径轨迹

(a)磁航角参考算法

误差计算采用即时定位与地图构建中绝对轨迹误差(absolute trajectory error,ATE)的计算方法,计算公式如式(16)所示。

(16)

式中:trans为矩阵的求迹计算;N为输出点个数;Tgt,i为实际轨迹坐标向量,通过坐标输出文件获得,每个向量包括时间、x轴坐标与y轴坐标;Tet,i为预设轨迹坐标向量。

首先根据预设方形轨迹,按照实际采集坐标点数,均匀等距离生成正方形轨迹,作为ATE中的估计轨迹,然后将实际轨迹的坐标点依据情况分为2圈,最后求解每圈ATE,结果如表4所示。

表4 不同导航方式的误差对比 m

通过图11与表4可以得到,传统惯性+地磁组的两圈平均误差为0.547 m,偏振光组为0.421 m,以偏振信息为主的多源融合组为0.273 m。融合组行走预设路径的ATE较磁航角参考方式减少了50.1%,较偏振角参考方式减少了35.2%。对于磁航角参考组,由于在试验场地周围存在软磁干扰,磁场分布不是均一的,所以在缺少先验信息的情况下,航向估计会出现非线性误差,引起运动偏移预设轨迹;对于偏振角参考组,虽然偏振光定向方法是无累计误差的,但是偏振角计算复杂,传感器解算速率较低,不能修正短时间内的方位漂移,因此引起运动偏移预设轨迹;对于融合组,以偏振角为估计量时,航向参考本身更准确,与陀螺仪紧耦合后,提高了偏振光定向的动态响应,修正了短时间内的方位漂移,同时通过惯性参考对偏振光传感器进行姿态的补偿,提高了由于四足机器人运动颠簸造成的估计量误差,路径精度较前2组都有了提升。由以上实验可以看出,组合导航系统具有较高的鲁棒性,且误差不随时间累积、实时性较好。

3 结束语

为了实现复杂环境中四足机器人的自主导航,提升导航系统的抗干扰性,本文首先设计了偏振光传感器与九轴AHRS的紧耦合算法,然后设计了偏振光传感器与陀螺仪的卡尔曼滤波方法,接下来设计了融合实验验证了组合传感器抗干扰的效果,最后搭建了四足机器人导航平台完成了循迹实验。实验结果表明:多源紧耦合算法在光干扰下标准差为0.19°,较融合前减小了64.8%,并且不受电磁干扰;四足机器人循迹实验中组合传感器组的ATE为0.273 m,ATE较磁航角参考算法减少了50.1%,较偏振角参考算法减少了35.2%。实验表明该导航算法可用于四足机器人在卫星拒止中的自主导航,并且拥有较好的鲁棒性。

目前,四足机器人导航平台仅应用于平坦地面,下一步的工作是对组合传感器写入波动补偿算法以提升四足机器人在复杂路面环境下自主导航的适应性,并在山地环境下进行相关实验。