室内飞行智能体惯性系统与激光测距仪组合导航改进方法
【专利摘要】室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于：传感器部分（6）包括：激光测距仪URG（1），IMU（7），气压高度计（4）和磁场强度传感器（5），陀螺仪（2），加速度计（3）；导航信息计算部分（14）包括：USB总线（8），导航计算机（9），转换器件（10），电气连线（11），总线（12），导航信息（13）。使用激光测距仪（1）获取位置信息，惯性导航系统获取姿态信息，采用无迹卡尔曼滤波方法UKF和直线特征提取算法，将位置信息与姿态信息融合在一起，最后通过捷联算法实现航迹推算。本发明可以实现飞行智能体在室内自主导航，有效解决飞行智能体组合导航系统发散问题和航迹推位失效问题，提高了导航精度。
【专利说明】室内飞行智能体惯性系统与激光测距仪组合导航改进方法
【技术领域】:
[0001]本发明属于多传感器信息融合领域。
【背景技术】
[0002]惯性导航是一种航迹推位算法，通过加速度计和陀螺分别测量载体的比力和角速率，并利用积分算法求解载体的位姿信息。捷联式惯性导航系统(Strap-down InertialNavigation System, SINS)是惯性导航的一种，能够连续提供载体的位姿信息。然而，惯导系统存在误差随时间迅速积累的缺点。全球导航定位系统(Global PositioningSystem, GPS)和SINS具有极好的互补性，利用GPS和SINS进行组合导航，可以降低成本提供全球精确导航能力，因此，SINS/GPS组合导航系统已经得到了广泛的应用。
[0003]然而，在室内环境中GPS信号质量受到了严重的恶化，GPS信号能量、到达时间和接收信噪比都比在普通环境中有很大程度的恶化，GPS接收机的定位精度、GPS数据的可用性和总体性能都大大下降，这样会影响室内导航系统的性能。
【发明内容】
[0004]为了解决上述问题，本发明公开了室内飞行智能体惯性系统与激光测距仪组合导航改进方法，以惯性导航系统为核心，与激光测距仪构成一套组合导航系统，实现室内无GPS信号情况下的姿态检测与定位。
[0005]本发明的主要思路是在室内无法接收GPS数据的情况下，采用无迹卡尔曼滤波(Unscented Kalman Filter, UKF)方法实现惯性测量单元和激光测距仪的信息融合。利用惯性测量单元(Inertial MeasurementUnit, IMU),通过惯性导航算法对系统位姿信息进行递推，并使用激光测距仪、磁罗盘和气压高度计等传感器对系统位姿的发散进行抑制。这种改进方法能有效解决室内飞行智能体组合导航系统发散问题和航迹推位失效问题，增强对外界扰动的稳定性，提高了导航系统的精度。
[0006]本发明的目的是这样实现的:
[0007]室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于:
[0008]飞行智能体组合导航系统结构的传感器部分6包括:激光测距仪URG1，IMU 7，气压高度计4和磁场强度传感器5，陀螺仪2，加速度计3 ;导航信息计算部分14包括:USB总线8，导航计算机9，转换器件10，电气连线11，总线12，导航信息13。
[0009]上述的室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于所述的IMU 7惯性测量单元包括陀螺仪2和加速度计3。
[0010]上述的室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于所述的导航信息计算部分14，激光测距仪URG I通过USB总线8将周围环境信息传给导航计算机9 ;其余传感器将采集到的数据通过电气连线11传给转换器件10，得到导航计算机9能够使用的数据格式；通过与导航计算机9连接的总线12传给导航计算机9，导航计算机9通过本发明提出的组合导航改进方法计算得到导航信息13。[0011]上述的室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于所述的飞行智能体组合导航系统算法流程图如图2所示，首先根据飞行智能体的特点，计算出组合导航系统的状态方程，观测方程。采用无迹卡尔曼滤波UKF算法，实现时间更新，根据激光测距仪获取的环境信息实现量测更新，并根据传感器部分的数据修正滤波器，对系统状态进行估计，最后由导航计算机完成系统的数据处理以及计算工作，具体步骤如下:
[0012]步骤1:基于四元数对飞行智能体姿态进行描述，建立系统的非线性状态方程；以环境直线特征为观测量，建立系统的非线性观测方程，这里，我们将激光测距仪能够检测到的信息统称为环境信息；
[0013]步骤2:对于系统状态的估计由一个UKF滤波器完成，在进行滤波之前，需要初始化滤波器参数，初始化全局地图信息；
[0014]步骤3:时间更新，即由惯性测量单元的数据和上一时刻系统状态，根据非线性状态方程，递推系统下一时刻的位姿信息；
[0015]步骤4:获取激光测距仪的观测数据，提取周围障碍物直线特征；
[0016]步骤5:量测更新，即根据载体的位姿信息和观测方程，得到由递推位姿信息推算出的出现在激光雷达视野中的环境直线特征估计；
[0017]步骤6:对步骤4中得到的递推直线特征和步骤5中根据激光测距仪提取的环境直线特征进行匹配，并根据传感器部分获得的其他参数进行滤波更新，得到估计的系统状态息;
[0018]步骤7:将新观测到的直线信息添加进全局地图中，并选择下一时刻要被观测的直线；
[0019]步骤8:由导航计算机完成计算并返回步骤3继续下一时刻的滤波计算估计新的参数。
[0020]其中步骤I中系统的状态量为载体的姿态，速度，位置，陀螺零偏；系统的观测量为激光测距仪视野中的障碍物直线特征信息。
[0021]步骤2中初始化滤波器需要初始化系统状态、误差协方差矩阵、系统过程噪声和观测噪声的统计特性；初始化地图是以初始点为原点，将环境直线特征作为地图信息保存在地图中，其中，环境直线特征为原点到该直线垂线所在垂点的极坐标参数。
[0022]步骤3中时间更新得到系统状态，即载体的位姿信息。
[0023]步骤4中使用激光测距仪数据拟合直线的方法如下:
[0024]Step I获取激光测距仪的数据；
[0025]Step 2得到若干连续障碍物点簇，首尾点连成直线，根据每一点到该直线的距离，分为若干局部最大值点；根据获取的局部最大值点，将点簇分为相应的若干组；对每组使用最小二乘法进行直线拟合；将所以直线首尾相连，得到连续一组连续的直线；
[0026]Step 3根据Step 2中拟合的直线获取特征参数，计算出飞行智能体到该直线垂线所在垂点的极坐标参数。
[0027]步骤5中量测更新，是根据当前时刻经过时间更新得到的飞行智能体的位置和激光测距仪的观测范围，确定将会被激光测距仪采集到的地图环境，并根据观测方程得到采集范围内地图环境相对于该时刻递推位置的直线特征。
[0028]步骤7中添加环境直线特征是环境相对于原点的直线特征。[0029]相比于传统组合导航方法，本发明具有以下优点:
[0030]第一，本发明采用了激光测距仪作为水平位置的观测器件，能弥补惯性组合导航系统在室内无法接收GPS数据所导致航迹推位发散的问题，在室内具有更强的实用性。
[0031]第二，本发明所使用的激光测距仪采样频率要大于GPS采样频率，能够最大程度地抑制SINS的发散。
[0032]第三，本发明所使用的激光测距仪测量精度大于GPS器件，能够提高系统的状态估计精度；在充分考虑系统先验信息的情况下进行直线特征匹配，可靠性更高，且能够使系统适用于更加复杂的环境；并能对激光测距仪的噪声有一定的抑制作用。
[0033]第四，针对非平面的运动过程中传统航迹推位失效问题，以及应用在SLAM算法中的绑架问题，本发明都能够很好的解决。
【专利附图】
【附图说明】
[0034]图1飞行智能体组合导航系统结构图；
[0035]图2飞行智能体组合导航系统算法流程图；
[0036]图2环境信息求取示意图；
[0037]图4激光测距仪数据拟合直线流程图；
[0038]图5全局地图直 线选择示意图
[0039]图中:1激光测距仪URG、2陀螺仪、3加速度计、4气压高度计、5磁场强度传感器、6传感器部分、7IMU、8USB总线、9导航计算机、10转换器件、11电器连线、12总线、13导航信息、14导航信息计算部分。
【具体实施方式】
[0040]下面结合附图对本发明的技术方案进行详细的说明:
[0041]惯性系统和激光测距仪组合导航系统结构如图1所示，传感器部分包括激光测距仪，IMU，气压高度计和磁场强度传感器，其中IMU惯性测量单元包括陀螺仪和加速度计，激光测距仪通过USB总线将周围环境信息传给导航计算机；其余传感器将采集到的数据通过电气连线传给转换器件，得到导航计算机能够使用的数据格式；导航计算机通过本发明提出的组合导航改进方法计算得到导航信息。
[0042]惯性系统和激光测距仪组合导航系统算法流程图如图2所示，根据采集到的传感器数据对系统位姿信息进行递推，并得到载体在递推状态下的观测量信息，进而根据激光测距仪获取的环境信息对系统进行滤波处理，具体步骤如下:
[0043]步骤I基于四元数，建立飞行智能体的系统模型，计算出飞行智能体组合导航系统的状态方程，具体方法如下:
[0044](a)定义飞行智能体的姿态四元数Q e R4为一个四维列向量:
【权利要求】
1.室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于:飞行智能体组合导航系统结构的传感器部分(6)包括:激光测距仪URG (I )，MU (7)，气压高度计(4)和磁场强度传感器(5)，陀螺仪(2)，加速度计(3);导航信息计算部分(14)包括:USB总线(8)，导航计算机(9)，转换器件(10)，电气连线(11)，总线(12)，导航信息(13)。
2.根据权利要求1所述的室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于:所述的IMU (7)惯性测量单元包括陀螺仪(2)和加速度计(3)。
3.根据权利要求1所述的室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于:所述的导航信息计算部分(14)，激光测距仪URG (I)通过USB总线(8)将周围环境信息传给导航计算机(9);其余传感器将采集到的数据通过电气连线(11)传给转换器件(10)，得到导航计算机(9)能够使用的数据格式；通过与导航计算机(9)连接的总线(12)传给导航计算机(9)，导航计算机(9)通过本发明提出的组合导航改进方法计算得到导航信息(13)。
4.根据权利要求1所述的室内飞行智能体惯性系统与激光测距仪组合导航改进方法，其特征在于:所述的飞行智能体组合导航系统算法流程图如图2所示，首先根据飞行智能体的特点，计算出组合导航系统的状态方程，观测方程；采用无迹卡尔曼滤波UKF算法，实现时间更新，根据激光测距仪获取的环境信息实现量测更新，并根据传感器部分的数据修正滤波器，对系统状态进行估计，最后由导航计算机完成系统的数据处理以及计算工作，具体步骤如下: 步骤I基于四元数，建立飞行智能体的系统模型，计算出飞行智能体组合导航系统的状态方程，具体方法如下: Ca)定义飞行智能体 的姿态四元数； (b)选取飞行智能体的位置、速度、姿态四元数和陀螺漂移作为系统状态量，建立系统的非线性状态和量测方程； (C)计算飞行智能体的观测方程； 步骤2建立符合步骤I所述模型的无迹卡尔曼滤波器，初始化无迹卡尔曼滤波器，初始化全局地图信息，将初始时刻的直线特征参数提取出来； 步骤3根据步骤2所述的无迹卡尔曼滤波器，实现时间更新，递推出下一时刻的系统状态； 步骤4获取激光测距仪的观测数据，提取特征并拟合直线，计算得实际观测直线的特征参数，从而获取飞行智能体的空间位置； Step I在计算机中读取k-Ι时刻的激光测距仪的数据； Step 2提取step I中数据的特征，本发明进行特征提取的方法分为两步:区域分割和特征提取；针对本发明中的飞行智能体，在区域的划分中采用基于激光数据点间欧氏距离的区域划分方法，将所采集到的激光数据点集分成不相连接的几个区域； Step 3根据Step2中拟合的直线获取特征参数，计算出直线与飞行智能体之间的空间距离和姿态； 步骤5对步骤2所述的无迹卡尔曼滤波器进行量测更新，更新系统的观测量； 步骤6根据步骤5递推计算得到的观测量，步骤4中获得的实际观测量和传感器部分测量的其他参数，实现滤波更新，对系统状态进行估计；步骤7根据步骤6估计出的飞行智能体的系统状态，可以计算得到由激光测距仪数据拟合成的直线在全局地图中的绝对位置，将新观测到的直线信息添加进全局地图，实现地图的更新，并选择下一时刻要被观测的直线； 步骤8返回步骤3，继续下一时刻的滤波计算，估计新的参数；由飞行智能体的导航计算系统根据步骤4更新后的导航系统状态方程和观测方程，完成组合导航的数据处理及解算，算法过 程见图2。
【文档编号】G01C21/20GK103808316SQ201210450567
【公开日】2014年5月21日 申请日期:2012年11月12日 优先权日:2012年11月12日
【发明者】王璐, 廖粤峰, 赵自超, 许可, 李冰, 马涛 申请人:哈尔滨恒誉名翔科技有限公司