基于STLS的姿态信息空间配准方法

徐旋孜,朱建良,薄煜明

(南京理工大学自动化学院,江苏 南京 210094)

0 引言

现代社会的快速发展使智慧交通成为大势所趋,无人车的应用作为智慧交通发展的重要体现[1],能有效保障交通控制系统的稳定运行[2]。随着无人驾驶技术的发展,单一、同类型的传感器已无法满足无人驾驶车辆分析路况信息的需求,利用具有独特优势的不同传感器及其相应的导航方式(受外界干扰小、短时精度高的惯性导航,全天候高精度、技术成熟的卫星导航,设备精简、可构建地图实现避障的视觉导航等)进行多源融合导航成为无人驾驶领域中一项行之有效、必不可少的关键技术[3]。近年来,基于多源异构异质数据开发自主导航系统成为无人驾驶领域学界关注的热点[4]。多源信息融合的无人车导航系统能够有效提高导航性能,切实带动地图信息和空间结构的一致性和稳定性,然而如果直接对多源信息进行融合,若传感器的结构不一致、空间基准不一致,非但不能优化系统性能,反而可能造成性能的恶化。为对多传感器的量测信息进行综合整理从而更精确地估计出导航信息,在信息融合之前的数据预处理阶段须进行空间配准。多传感器空间配准误差问题主要有:空间基准不一致,即各传感器的导航数据依据不同坐标系测量而得,彼此存在误差;传感器安置存在误差,即各传感器安装完毕后彼此的对应坐标轴朝向不完全重合,安装的姿态存在偏差,造成安装的相对姿态误差;各传感器安装在车辆载体的不同位置存在杆臂效应,造成杆臂效应误差[5]。

相对姿态误差的参数获取通常也被称为标定,通常可分为在线标定和离线标定两种方法。在线标定一般基于滤波实时估计相对姿态参数,对算法要求高,当估计结果不收敛时将会影响其他滤波状态量,引起导航精度下降。谢宇婷等[6]、程然等[7]分别采用卡尔曼滤波、基于期望最大化的容积卡尔曼滤波法在线估计相对姿态误差角,但都是针对目标跟踪的量测数据进行分析,在导航领域的配准效果未知。离线标定法一般基于最优估计方法,或借助其他辅助的标定设备完成。胡雷[8]通过最小二乘类方法对相对姿态误差角进行估计,其中相对姿态仅针对纯方位雷达的方位角。吕志刚[9]指出,当更换水下航行器的某导航传感器时,其与捷联惯导的相对姿态关系发生变化造成系统误差,需要额外的角度测量仪进行标定以满足导航精度要求。何骞等[10]提出一种基于预积分的IMU/ODO外参估计方法,但是该方法中相对姿态误差的航向角不可观测从而无法估计;周阳林等[11]、叶珏磊等[12]均对激光雷达与GNSS/INS组合导航系统提出标定方法,前者提出的方法可快速获取相对姿态参数但需要经纬仪,后者提出的方法不需要经纬仪但基于点云信息,无法用于其他导航传感器。由此可见,由于存在空间配准的相对姿态误差问题,在组合导航领域缺乏适用于不同种类导航设备的空间配准方法,无法完成姿态信息的配准。

考虑到无人车的多种导航传感器与车体刚性连接,导航传感器之间的相对位姿关系参数固定,相对姿态误差是常值,因此,考虑使用离线标定方法对相对姿态误差进行估计,降低算法难度和计算量。本文基于传感器测量所得的姿态信息,利用对应的方向余弦矩阵作为中间变量,建立相对姿态误差模型,使用STLS(structured total least square,结构总体最小二乘)方法对模型进行迭代求解,获得三轴相对姿态误差对应的方向余弦阵。利用该方向余弦阵可反解出相对姿态误差对传感器测量的姿态信息进行补偿,仅依靠传感器的量测信息而无需借助标定辅助设备,可实现不同种类导航传感器的姿态信息空间配准,为信息融合做好准备工作,利于组合导航精度的提升。

1 STLS最优估计的基本原理

1.1 问题描述

超定方程Ax=B的系数矩阵A和观测矩阵B同时存在扰动误差,分别为E和e,采用STLS方法求解该超定方程,但基于奇异值分解的方法会破坏矩阵原有的结构。在此基础上,Moor和谢景超提出的STLS方法可以保留原数组的结构特征[13-14]。

使用STLS方法求解的问题可描述为:寻找r∈Rm,E∈Rm×n,使得‖[rE]‖F=min。其中

r=B-(A+E)x

(1)

式中:r称为残量;A与E的结构相同。

若A中有q(q≤mn)个元素存在误差,由E的q个元素构成的向量记为α=[α1,…,αq]T∈Rq,则α与E等价,已知α和A的结构特点即可唯一确定E。残量r可视为α与x的函数,r=r(α,x)。用加权对角矩阵D表示α各元素在E中的重复次数。使用STLS方法求解的问题可改述为:

(2)

其中‖·‖p表示p-范数,p=1,2,…,∞,而当p=2时使用STLS方法求解两种描述等价。

1.2 STLS方法及迭代求解

1.2.1X的构造

定义X∈Rm×q,其组成元素为x的元素,令Xα=Ex。构造X的方法如下:

定义m个矩阵Pi(i=1,…,m)∈Rq×n,若αk位于E的(i,j)处,则Pi的(k,j)处元素为1,其余元素为0。由定义,E和X可由公式(3)计算:

(3)

1.2.2 迭代求解过程

考虑每轮迭代过程中E的微小增量分别为ΔE,对应的α的微小增量记为Δα,则XΔα=ΔEx。对每轮迭代的残量r=r(α+Δα,x+Δx)采用线性逼近的方法并忽略Δα和Δx的二阶项,得到:

r(α+Δα,x+Δx)=

B-(A+E+ΔE)(x+Δx)=

B-(A+E)x-ΔEx-(A+E)Δx-ΔEΔx≈

[B-(A+E)x]-ΔEx-(A+E)Δx=

r(α,x)-XΔα-(A+E)Δx

(4)

对式(2)考虑Δα和Δx后描述为

写成间接平差的形式为:

(5)

根据间接平差公式,求得微小增量Δα和Δx为

(6)

完整的STLS方法迭代过程总结如下:

1)确定参数:系数矩阵A,加权对角矩阵D,观测矩阵B以及允许误差ε;

2)确定迭代初始值:E=0,α=0,x=xLS;

3)根据1.2.1节所述,构造X,计算E;

4)计算残量r(α,x),根据平差公式(6)计算本轮迭代的微小增量Δα和Δx;

5)更新:α=α+Δα,x=x+Δx;

6)判断条件:满足max{‖Δα‖,‖Δx‖}≤ε时,停止迭代;不满足,则转到步骤3开始新一轮迭代,直至满足条件停止。

2 基于STLS的相对姿态误差角估计与补偿

姿态信息空间配准可分为两个过程:事先采集静态标定数据,通过参数估计方法计算出相对姿态误差角;使用得到的相对姿态误差角估计值,导航过程中,在进行多源融合之前对该相对姿态误差角进行补偿。在此之前,需要确定相对姿态误差角的基准传感器。

2.1 问题分析

2.1.1 空间基准选取

开展空间配准前的基础问题是确定空间基准。一般导航系统选取载体几何质心作为基准坐标原点,选取坐标系及正方向为右-前-上方向。该选取方式的问题在于,载体质心处的真实参考数据无法直接获取,只能通过安装在质心处的精度可靠的传感器来获取,然而这种方式在实际实施过程中难以实现。比如:精度较高的GPS需要安装天线才能通过RTK定位,而天线需要裸露在载体外部,难以安装在质心处;其余高精传感器也有类似或其他问题,因此获取质心处的真实参考数据较为困难。

在本文研究的无人车系统中,由于考虑拒止条件下将高精IMU作为最主要的导航传感器,其余各传感器作为辅助修正传感器,且IMU的安装位置靠近无人车的质心处,因此,空间基准原点选取为IMU处,空间基准坐标系选为右-前-上坐标系,各传感器刚体的姿态数据配准到空间基准坐标系下。

2.1.2 姿态角与方向余弦阵

首先对姿态角及对应的方向余弦阵等相关定义进行说明。选取导航坐标系n系为东-北-天坐标系(E-N-U),姿态角旋转顺序定义为绕固定轴z-y-x的顺序,即以导航坐标系n为初始姿态,先绕z轴转动一个角度为航向角θY,再绕y轴旋转一个角度为横滚角θR,最后绕x轴转动一个角度为俯仰角θP,旋转完毕得到载体坐标系b。

在此旋转顺序定义下,由b系(下角标)变换到n系(上角标)的方向余弦阵为

(7)

已知方向余弦矩阵也可以反求出姿态角,式(8)中arctan表示四象限的反正切。

(8)

2.2 相对姿态误差角的估计与补偿

2.2.1 基于姿态信息的模型建立

无人车多源融合导航系统一般装有多种传感器。此处使用的无人车装有IMU(惯性测量单元)、AHRS(航姿参考系统)、LiDAR(激光雷达)、CAM(视觉里程计)等传感器,考虑各种传感器均与前述选取的IMU基准坐标系存在相对姿态误差角,因此,需基于各种传感器的姿态数据与IMU的姿态数据建立模型。由于不同传感器的姿态数据不可直接进行数学计算,因此,需利用方向余弦阵作为建模对象。以CAM为例,估计CAM与IMU之间的相对姿态误差角(其他传感器同理)。

(9)

(10)

2.2.2 相对姿态误差角的STLS求解

根据1.2.2节,迭代过程的步骤如下:

2)迭代初始值E=0,α=0,x=xLS=(ATA)-1ATb,则xi为xLS的第i列;

3)构造与xi对应的Xi和Ei;

4)计算残量r(α,xi),根据平差公式计算本轮迭代的微小增量Δα和Δxi;

(11)

5)更新:α=α+Δα,xi=xi+Δxi;

6)判断条件:满足max{‖Δα‖,‖Δxi‖}≤ε时,停止迭代;不满足,则转到步骤3开始新一轮迭代,直至满足条件停止。

2.2.3 相对姿态误差角的补偿

由上述2.2.1、2.2.2节可以通过离线的方式完整求解出相对姿态误差角(常量误差),采集的数据均为估计误差所使用的数据,但还未对实际的数据进行误差补偿。

使用基于姿态信息的结构总体最小二乘法补偿相对姿态误差角的完整过程如图1所示。

图1 姿态配准过程示意图

3 实验结果与分析

开展MATLAB仿真实验和半实物仿真实验,对上述相对姿态误差角补偿的空间配准方法性能进行验证。

3.1 MATLAB仿真实验

在MATLAB R2020a的环境下进行仿真实验,其中使用STLS方法迭代估计的流程如图2所示。

图2 STLS迭代估计相对姿态误差角流程图

表1 仿真实验不同方法相对姿态角估计结果 (°)

表1数据显示,估计的相对姿态误差值都较为准确,彼此之间差异较小,就参数估计而言,几种方法的区别并未很好地体现。由于仿真实验已知准确真值,可以进一步考察单位权中误差。单位权中误差是权值为1的观测值均方根差,可以反映等精度测量情况下观测值偏离真值的程度。分析表1最后一行,发现LS方法的单位权中误差大于TLS方法、大于STLS方法,单位权中误差值越小说明估计结果精度越高,因此,STLS方法优于TLS方法、TLS方法优于LS方法。

3.2 半实物仿真实验

3.2.1 相对姿态误差角估计实验

半实物仿真无人车如图3所示。

图3 半实物仿真无人车

根据2.2.1节所述,采集估计相对姿态误差角所需要的多组标定数据,采集每组数据时,无人车至少运动10 m距离以便LiDAR、CAM有足够的识别目标进行SLAM建图,随后静止一段时间采集STLS方法所使用的静态数据。采集完毕后选取北、东北、东、西、南、上坡6组静态数据作为相对姿态误差角估计的使用数据。

使用MATLAB软件进行姿态信息的配准工作。对上述标定数据使用STLS方法,过程同仿真实验的流程图(图2),估计结果如表2所示。

表2 半实物仿真相对姿态角估计结果 (°)

3.2.2 相对姿态误差角补偿实验

实验数据采集地点为学校广场,跑车数据采集时间大于60 s。使用3.2.1节求得的相对姿态误差角对跑车数据进行补偿。以CAM(视觉里程计)为例,配准前、后跑车数据姿态角误差情况如图4所示。

(a)CAM俯仰角配准前后误差

(b)CAM横滚角配准前后误差

(c)CAM航向角配准前后误差图4 配准前、后跑车数据CAM姿态角误差图

比较配准前后,AHRS、LiDAR、CAM的俯仰角、横滚角、航向角误差的均值、RMS(均方根)值,由表3配准前后跑车数据姿态角误差数据,可以看出,3种传感器的俯仰角、横滚角误差均减小,但是航向角误差增大。

分析误差增减百分比情况,发现待配准传感器与基准传感器之间的相对姿态误差角越大,配准效果越好。LiDAR和CAM的相对姿态俯仰角最大,在2°上下,配准前后误差均值、RMS值减小比例可达70%以上;其次是CAM的相对姿态横滚角,为0.8°左右,其误差均值减小比例超过60%。AHRS的俯仰角、横滚角,LiDAR的横滚角的误差均值、RMS值也均有不同程度的减小。但是3种传感器的航向角的误差均值和RMS值反而增大,我们认为是待配准传感器与基准传感器之间的相对航向误差角的数值过小,估计值不超过0.4°,并且配准前传感器测量所得姿态角与实际值之间的误差很小,均在0.1°上下,从而配准的结果容易受传感器随机测量噪声的影响。进一步,配准后3种传感器的航向角误差最大也仅为0.36°,在误差允许范围内。因此,本文所提出的姿态信息配准方法综合误差较小,能够满足无人车多源融合导航的空间配准要求。

表3 配准前后跑车数据姿态角误差 (°)

4 结论

本文针对多源融合导航空间配准中的相对姿态误差问题,提出一种基于姿态信息的STLS参数估计方法,先通过迭代方式求解出待配准传感器相对于基准传感器的相对姿态误差角,随后对所估计出的相对姿态误差角进行补偿。通过MATLAB无实物仿真实验,比较LS、TLS、STLS估计结果和单位权中误差,验证STLS迭代方法求解相对姿态误差角的性能。通过无人车实时跑车数据进行半实物仿真实验,发现相对姿态误差角度越大,配准效果越好;其中的相对姿态误差最大,俯仰角的配准效果最佳,配准前后误差减小70%以上。因此,本文所提出的姿态信息配准方法具有易于操作、可行性强的优点,可为后续多源融合导航做好准备工作。