行人导航系统运动控制研究

行人导航系统运动控制研究

1人员自动定位行人跟踪是近年来改进的一种跟踪方法，引起了人们的关注。它利用惯性传感器不易受外界环境因素影响的特点,可以满足在封闭空间,室内等无GPS(GlobalPositioningSystem)信号情况下的定位要求,在发生火灾、楼宇坍塌等通信基础设施受到破坏的紧急情况下,为消防员、救援队员进行人员定位,保证其安全。文献介绍了使用惯性器件和磁阻传感器制成的测量装置固定于脚部,通过步态识别、步长估计计算出行走的轨迹;文献提出了将惯性测量单元(IMU,InertialMeasurementUnit)置于头部,融合航迹递推和GPS技术的定位算法,介绍了使用神经元网络计算步长的方法;文献提出了2种将MIMU置于脚部的定位算法,并介绍了在有无GPS信号情况下如何进行更准确的定位。上述文献提及的脚部测量方案,会因步速较快时加速度计输出震荡较大而对步态识别带来困难,且上述个别方法需要有GPS定位技术的支持。本文采用腰部MIMU测量方案,使用对单轴加速度计进行FIR低通滤波实现行进计步,提出了基于三轴加速度计测量信息的步长在线标定方法与步态分类识别算法,实现了行走距离的较精确测量,误差可以控制在5%以内。针对MIMU大惯性漂移问题,给出了1种有效的磁航向、捷联航向融合算法,大大提高了系统的定位精度,此算法在不需要其他定位技术支持的情况下,能完成具有较高精度的定位需求。2行人景观的方案确定本文使用荷兰Xsens公司的Mti(MotionTrack)作为惯性测量单元进行实验,分别将Mti固定于人的腰部和脚部,设置采样频率为100Hz进行行走测试,采集到的加速度输出如图1所示,可见,置于脚部行走时加速度计输出震荡较大,使用加速度积分计算速度时难以找准积分的上下限,会产生较大误差,而且步速过快时步态识别较为困难,无法使用零速校正修正误差,而置于腰部行走时的加速度计输出较为平缓、步速较快时规律性亦很明显,更易于步态的高准确率识别,而且置于腰部时陀螺受到的振动干扰小,计算的航向角误差和定位误差更小,因此采用了将Mti置于行人腰部的方案。3行人跟踪算法3.1速度计输出数值分析人在行走的过程中,先抬脚后落脚、重心先上升后下降,因此置于腰际的垂直向加速度计输出先增大后减小,而采集到的加速度计输出亦如分析所呈趋势,首先使用截止频率为5Hz的FIR低通滤波器对加速度计信号进行滤波,将滤波后的垂直向加速度计信号减去其静止时的输出后进行过零检测,加速度计输出值由负变为正时为抬脚,由正变为负时为落脚,辨识的结果如图2所示:图中虚线为原始的加速度计输出,实线为滤波后的信号,2个脉冲信号之间的区域为左右脚各迈一步。3.2步长与速度关系人行走时的步长不是1个固定值,它随人行走的速度、步伐频率等改变,参考国外学者使用加速度计输出计算步长的经验公式,在其基础上进行改进,首先对K值进行标定,根据加速度输出在线更新K值取代固定K值,可以减少人因步速、行走方式不同造成的误差,其次使用三轴加速度计输出计算步长,信息量更加丰富,减小了因测量装置放置倾斜带来的误差,公式如下所示:{A=√a2x+a2y+a2zAmax=max(A(tup,tdown))Amin=min(A(tup,tdown))stride(m)=Κ4√Amax-Amin(1)式中:Amax,Amin分别为滤波后的加速度计输出从抬脚时刻到落脚时刻之间的最大值和最小值;tup,tdown对应抬脚、落脚时刻,对于公式中的步长系数K使用最小二乘法进行标定,匀速行走固定的距离L,进行步态辨识并计算出Sn和平均步长—S如下式,列写方程求解:{Sn=4√Amax-Amin—S=Ln[—S—S⋮—S]=[S1S2⋮Sn]ΚΚ=(—SΤ—S)-1—SL(2)其中:n为行走步数,按式(2)可计算出步长系数K。在实际的计算过程中亦发现,K值因步速的快慢、行走方式的差异略有不同,因此应当建立K与步速的关系,在线更新K计算步长,由步速与K值相关、步速与加速度输出呈正比例关系,可建立如下关系式:Κ=a—A2max+b—Amax+c(3)其中:—Amax为单次匀速行走测试中的每一步从抬脚到落脚的时间区间内加速度计输出最大值构成序列的均值。分别按照较快、快速、中速、慢速等速度行走固定距离,按上述公式计算出K和—Amax后,使用最小二乘法计算式(3)的未知系数,进行曲线拟合。其后,在计算步长时,首先由加速度计的输出按式(3)计算出K后代入式(1)计算步长,拟合的结果和步长计算结果如图3所示。图3左图中,圆点为按照不同步速行走计算的K值,虚线是K的拟合曲线,右图为不同步速下步长的计算结果,由上至下步速依次减少,此图说明步速与步长、加速度计输出呈正比例关系,图中步长出现抖动是因为单次行走测试过程中每一步的速度略有不同,处于可以接受的误差范围内,不会对整体结果造成影响。分别采用固定的经验K值和按上述方法标定的K值计算同一段轨迹的距离,计算结果如下:第一次:实际距离为150m,标定前为142.33m,标定后为144.22m;第二次:实际距离为60m,标定前为56.78m,标定后为59.33m。3.3计算的行程3.3.1初始样本角的确定首先根据坐标变换次序计算出地理系到载体系的旋转矩阵Cnb,本文使用变换次序为地理系ψ→Ζθ→Yγ→X载体系,得姿态矩阵如下式所示,当载体处于非加速运动情况下,可以根据三轴加速度计输出ax,ay,az和当地重力加速度g确定载体姿态,之后由三轴磁阻传感器输出磁场强度mx,my,mz计算初始航向角:Cnb=[cosθcosψcosθsinψ-sinθsinγsinθcosψ-cosγsinψsinγsinθsinψ+cosγcosψsinγcosθsinθcosγcosψ+sinγsinψsinθcosγsinψ-cosψsinγcosγcosθ][axayaz]=Cnb[00g]θ=-arcsin(axg)(4)γ=arctan(ayaz)ψstart=arctansinγmz-cosγmycosθmx+sinγsinθmy+sinθcosγmz3.3.2航迹递推计算轨迹使用三轴陀螺输出的旋转角速率按照捷联惯导算法对姿态矩阵进行更新,计算姿态角:θ=-arcsin(Cnb(1,3))γ=arctanCnb(2,3)Cnb(3,3)ψ=arctanCnb(1,2)Cnb(1,1)(5)—ψgryo=ψ1+ψ2+⋯+ψΝΝ,ψ-2σ≤ψn≤ψ+2σ(6)在使用航迹递推计算轨迹时,单步仅需1个航向值计算轨迹,而若使用抬脚时刻的航向值可能会造成较大的偏差,因此取上一次落脚时刻到此次抬脚时刻这个时间区间里的所有航向值作为1个序列,按式(6),对这个序列中航向处于(—ψ-2σ,—ψ+2σ)范围内的值取平均得—ψgryo,—ψ,σ分别为这个序列的均值和方差。3.3.3磁联磁—磁航向首先由俯仰角θ、横滚角γ确定方向余弦矩阵Cbn,使用此矩阵将传感器轴向测量到的地磁场强度即mx,my,mz投影到当地地理系后计算磁航向:{[mΝmWmU]=Cbn[mxmymz]=[cosθsinγsinθsinθcosγ0cosγ-sinγ-sinθsinγcosθcosγcosθ][mxmymz]ψ=arctan-mWmΝ=arctansinγmz-cosγmycosθmx+sinγsinθmy+sinθcosγmz(7){Ηnormal=√m2x+m2y+m2zΗi=√m2x_i+m2y_i+m2z_i—ψcompass=ψ1+ψ2+⋯+ψΝΝψ-2σ≤ψi≤—ψ+2σ,0.5<|ΗiΗnormal|<1.5(8)计算磁航向—ψcompass亦取上次落脚到此次抬脚的时间区间内的所有磁航向为1个序列,Hnormal和Hi分别为无磁场干扰和步行时测量到的地磁场强度,若二者相差较大,则说明有外界磁场干扰,将此磁航向从序列中剔除,最后对这个序列中航向值处于(—ψ-2σ,—ψ+2σ)范围内的值取平均得—ψcompass。3.3.4磁风向与标准风向的关系捷联航向短时精度高,不受外界环境因素干扰,但长时稳定性差,易发散;磁航向长时则精度高,但易受外界因素影响,因此可以根据这2种航向的特点进行优劣互补,信息融合,从而提高精度。在步行的前30步至50步,捷联航向更为准确,即以此航向角计算轨迹,其后时间按下式计算:{ψi_final=0.5(—ψgryo+—ψcompass)|—ψgryo-—ψcompass|≤45°如果捷联航向与磁航向相差较大,说明受到了外界磁场的干扰,磁航向此时不可信,使用捷联航向计算轨迹,而如果二者的差值在预设的范围内,则取2种航向角的均值计算轨迹。按上述步骤计算出单步步长Ln和行进方向θn,并确定起点坐标P0(east),P0(north)后,使用航迹递推(DR)按下式计算行进轨迹:{Ρn(east)=Ρn-1(east)+LncosθnΡn(north)=Ρn-1(north)+Lnsinθn(10)按照上述方法进行计算,结果如图4所示,实线、点划线、虚线轨迹分别对应参考轨迹、捷联航向计算的轨迹和融合航向计算的轨迹,可见捷联航向计算的轨迹偏差较大,而融合航向的计算结果精度较高。4加速度值+步长大小定义图5为单次步行测试,在不同的时间段内按照不同步速行走的轨迹,其中圆点、方框、三角、X形图标分别代表按照较慢、慢速、中速、快速行走的单步,步速通过对加速度值和步长大小设定阈值进行划分。图6显示了步行测试地点和大致行走轨迹,右图为使用MATLAB计算的行走轨迹和参考轨迹的对比图,点划线为计算的行走轨迹,虚线为参考轨迹,由结果可知,计算出的轨迹与实际的行走轨迹大致吻合。5多传感器融合磁联风向修正本