适用于全轮转向载体的“NHC+轮角”运动约束算法

发布时间:[2021-08-22] 来源:[牛小骥 张再兴] 点击量:[4147]

    车载组合导航系统中常会使用非完整性约束(Non-Holonomic Constraint, NHC)来抑制MEMS惯导的发散误差,以改善导航定位性能。但NHC约束对全轮转向载体(例如那种可以横行、斜行的机器人底盘)不成立,为此我们推导了一套适用于转向轮的运动约束算法,而传统的NHC约束只是它的一个特例。


    全轮转向机器人和特种车辆由于各轮都带有独立转向机构,因此相比于传统轮式载体更加灵活,可以在平面上旋转和任意方向平移,轻松地避开障碍物,在野外考察、矿井作业、安防巡检、特种作战等领域有着广泛的应用。在这些领域中,针对全轮转向载体的导航定位算法尤为重要。GNSS/INS具有良好的互补性,广泛应用于导航领域中;里程计是机器人、车辆等轮式载体常用的传感器,成本低且可靠性好,利用里程计对载体的速度进行约束,可以有效抑制GNSS失锁时INS误差的发散。传统的GNSS/INS/里程计组合导航算法依赖于载体的非完整性约束(NHC),即一般情况下载体只有沿车体前向的运动速度,而在车体的侧向和垂向速度为零。该约束在非转向轮上更加符合,因此里程计一般被安装在非转向轮上,与NHC一起构成车体系中的三维速度修正。然而在全轮转向机器人中,其各轮都能独立转向,不再有非转向轮,导致上述假设不成立,因此传统算法不再适用。为此,本文引入了轮角数据(车轮相对于车体的转向角度),提出了“NHC+轮角”运动约束,设计实现了一套GNSS/INS/里程计/轮角组合导航算法。


    以机器人为例,全轮转向机器人可以执行下图中的两种运动模型,第一种为转弯转向运动模型,中间的轮子为非转向轮,传统GNSS/INS/里程计算法仍然适用;第二种为全轮转向运动模型,所有轮子都同时转向,轮子的方向与车体运动方向一致。为了兼容这两种模型,本文定义了一个车轮坐标系(w系),其与车体坐标系(v系)存在夹角,即轮角。在转弯转向模型中,w系与v系重合,即轮角为零,NHC在v系和w系中都是成立的;而在全轮转向模型中,w系与v系不重合,轮角数据可通过机器人底盘反馈的舵机值获取,此时NHC只在w系中成立。在这两种运动模型中,一个共同点是车体运动方向始终与车轮坐标系(w系)的前向轴(x轴)保持一致,其运动速度可由里程计提供,而w系y轴、z轴上的速度为零,这就构成了w系中(而不是v系中)的三维速度修正

1629633927765059029.png    1629633957858056769.png

    算法的整体框架如下图所示,在经典GNSS/INS组合导航算法的基础上,我们利用里程计速度和轮角信息,应用NHC约束,为机器人提供w系的三维速度修正,构建速度误差观测向量和观测方程,送到扩展卡尔曼滤波器(EKF)中。另外,我们还对系统状态进行了扩维,增广了里程计比例因子误差和轮角误差,以实现在线估计和补偿。


1629634059905049725.png


    我们用实测数据验证了这种GNSS/INS/里程计/轮角组合导航算法的性能,对比了两种载体运动模型场景下它与传统GNSS/INS/里程计组合导航算法的效果差异,统计了GNSS短期(30秒)中断期间系统的位置漂移作为性能评估指标(见下表)。当机器人以普通转弯转向模型运动时(实验1),由于NHC在车体坐标系(v系)中成立,因此我们提出的GNSS/INS/里程计/轮角组合算法与传统算法取得了几乎相同的结果,NHC+里程计的修正效果都很显著。而当机器人以全轮转向模型运动时(实验2),传统算法就变得完全无法使用了;而我们的GNSS/INS/里程计/轮角组合算法仍然有效,仍能显著降低GNSS中断期间的位置漂移,相对于GNSS/INS结果的性能提升幅度与实验1相当。此外,我们还发现当轮角误差没有被补偿时,GNSS/INS/里程计/轮角组合算法的航向角误差会明显变大,这是由于机器人反馈的轮角数据本身往往带有偏置误差(也包括IMU相对于车体的安装角误差,参见我们公众号2021年6月26日文章),为此我们在系统状态中增广轮角误差估计,有效地解决了这一问题。


GNSS中断期间(30秒)位置漂移误差

实验1(转弯转向)

实验2(全轮转向)

GNSS/INS

1.52米

2.11米

传统GNSS/INS/里程计算法

0.15米

N/A

GNSS/INS/里程计/轮角算法

0.15米

0.17米

    

    上述实验结果表明:我们提出的GNSS/INS/里程计/轮角算法能适用于全轮转向载体,“NHC+轮角”约束和里程计修正能够有效发挥出应有的作用,与传统NHC/里程计修正算法用于传统车辆的效果相当。


相关成果发表在学术期刊《Measurement Science and Technology》上:

Zaixing Zhang, Xiaoji Niu, Hailiang Tang, Qijin Chen, Tisheng Zhang*. GNSS/INS/ODO/wheel angle integrated navigation algorithm for an all-wheel steering robot[J]. Measurement Science and Technology, 2021. DOI: 10.1088/1361-6501/ac17fb.

相关源代码和Demo数据共享在: https://github.com/i2Nav-WHU/GIOW-release.

附件为论文全文:  Zhang_2021_Meas._Sci._Technol._32_115122.pdf

版权所有:武汉大学多源智能导航实验室(微信公众号:i2Nav) 当前访问量: 技术支持:武汉楚玖科技有限公司