这个例子展示了如何使用6轴和9轴融合算法来计算方向。用惯性测量单元(IMUs)和磁角速率重力单元(MARG)来计算方位有几种算法。这个例子涵盖了方向的基础知识以及如何使用这些算法。
对象的方向描述其相对于一些坐标系的旋转,有时称为父坐标系,三维。
对于以下算法,使用的固定的父坐标系是North-East-Down (NED)。NED有时被称为全局坐标系或参照系。在NED参考系中,x轴指向北,y轴指向东,z轴指向下。NED的X-Y平面被认为是地球的局部切平面。根据算法的不同,北可以是磁北或真北。本例中的算法使用磁北。
如果指定,以下算法可以估计相对于East-North-Up(ENU)父坐标系而不是NED的方向。
一个对象可以被认为有它自己的坐标系统,通常称为局部或子坐标系统。这个子坐标系与对象相对于父坐标系旋转。如果没有平移,两个坐标系的原点就会重叠。
所计算的方向量是从父参考帧获取到子参考帧的旋转。旋转由四元数或旋转矩阵表示。
对于定向估计,通常使用三种类型的传感器:加速度计,陀螺仪和磁力计。加速度计测量适当的加速度。陀螺仪测量角速度。磁力计测量局部磁场。不同的算法用于融合不同的传感器组合以估计方向。
在本例的大部分时间里,使用的都是同一组传感器数据。当设备围绕三个不同的轴旋转时,将记录加速度计、陀螺仪和磁强计传感器数据:首先围绕其本地y轴旋转,然后围绕其z轴旋转,最后围绕其x轴旋转。在整个实验过程中,设备的x轴一般都指向南方。
ld =负载(“rpy_9axis.mat”);ACC = LD.Sensordata.Acceleration;陀螺= ld.sensorData.AngularVelocity;mag = ld.sensordata.magneticfield;pp = poseplot;
的ecompass
功能熔断加速度计和磁力计数据。这是一个无记忆算法,不需要参数调整,但算法非常容易受到传感器噪声的影响。
Qe =罗盘(acc, mag);为2 = 1:尺寸(acc, 1)集(pp、“定位”量化宽松政策(ii)) drawnowlimitrate结束
请注意,ecompass
算法正确地找到了北方的位置。但是,由于功能无记忆,所以估计的运动不顺畅。该算法可以用作方向过滤器中的初始化步骤或所呈现的一些技术使用四元数SLERP的低通滤波器定位可以用来平滑运动。
下面的目标估计方向使用误差状态卡尔曼滤波器或互补滤波器。误差状态卡尔曼滤波器是标准的估计滤波器,允许使用相应的噪声参数对系统的许多不同方面进行调整。互补过滤器可以作为具有内存约束的系统的替代品,并且具有最小的可调参数,这允许以更精细的调优为代价进行更简单的配置。
的imufilter
和complementaryFilter
系统对象™熔断器加速度计和陀螺仪数据。的imufilter
使用内部错误状态卡尔曼滤波器complementaryFilter
使用互补过滤器。滤波器能够去除陀螺仪的偏置噪声,它会随时间漂移。
ifilt = imufilter (“SampleRate”, ld.Fs);为2 = 1:尺寸(acc, 1) qimu = ifilt (acc (ii):),陀螺(ii):));集(pp、“定位”qimu) drawnowlimitrate结束
%禁止磁强计输入。cfilt = complementaryFilter (“SampleRate”ld.Fs,“HasMagnetometer”、假);为II = 1:尺寸(ACC,1)QIMU = CFILT(ACC(II,:),陀螺(II,:));集(pp、“定位”qimu) drawnowlimitrate结束
虽然imufilter
和complementaryFilter
算法产生的运动估计明显更平滑,相比ecompass
,他们不能正确地估计北方的方向。的imufilter
不处理磁力计数据,所以它只是假设设备的x轴最初指向北方。给出的运动估计imufilter
相对于初始估计方向。的complementaryFilter
做同样的假设HasMagnetometer
属性设置为假
.
姿态和航向参考系统(AHRS)由一个9轴系统组成,该系统使用加速度计、陀螺仪和磁力计来计算方向。的ahrsfilter
和complementaryFilter
System Objects™结合了以前的算法中最好的,以产生平稳地改变设备方向的估计,同时正确估计北方的方向。的complementaryFilter
使用与之前相同的互补滤波算法,增加一个步骤,包括磁强计和改进方向估计。就像imufilter
,ahrsfilter
算法还使用错误状态卡尔曼滤波器。除了陀螺仪偏见去除,ahrsfilter
具有一定的抗干扰能力。
ifilt = ahrsfilter (“SampleRate”, ld.Fs);为II = 1:尺寸(ACC,1)QAHRS = IFILT(ACC(II,:),陀螺(II,:),MAG(II,:));集(pp、“定位”qahrs) drawnowlimitrate结束
cfilt = complementaryFilter (“SampleRate”, ld.Fs);为II = 1:尺寸(ACC,1)QAHRS = CFILT(ACC(II,:),陀螺(II,:),MAG(II,:));集(pp、“定位”qahrs) drawnowlimitrate结束
的complementaryFilter
,imufilter
, 和ahrsfilter
系统对象™都具有可调谐参数。根据所使用的指定传感器调整参数可以提高性能。
的complementaryFilter
参数AccelerometerGain
和MagnetometerGain
可以调整,以改变每个传感器的测量影响方向估计的量。当AccelerometerGain
被设定为0
,只有陀螺仪用于x轴和y轴方向。当AccelerometerGain
被设定为1
,只有加速度计用于x轴和y轴方向。当MagnetometerGain
被设定为0
,只有陀螺仪用于z轴方向。当MagnetometerGain
被设定为1
,只有磁力计用于z轴定位。
的ahrsfilter
和imufilter
系统对象™有更多的参数,可以让过滤器更紧密地匹配特定的硬件传感器。传感器的环境也是需要考虑的重要因素。的imufilter
属性的子集ahrsfilter
参数。的AccelerometerNoise
,GyroscopeNoise
,MagnetometerNoise
, 和GyroscopeDriftNoise
是测量噪声。传感器的数据表有助于确定这些值。
的LinearAccelerationNoise
和LinearAccelerationDecayFactor
控制过滤器对线性(平移)加速度的响应。摇动设备是增加线性加速度的一个简单例子。
考虑一个imufilter
与一个LinearAccelerationNoise
9 e - 3
对震动轨迹有反应,相比之下LinearAccelerationNoise
9的军医
.
ld =负载(“shakingDevice.mat”);accel = ld.sensorData.Acceleration;陀螺= ld.sensorData.AngularVelocity;highVarFilt = imufilter (“SampleRate”ld.Fs,...“LinearAccelerationNoise”, 0.009);qHighLANoise = highVarFilt(accel,陀螺仪);lowVarFilt = imufilter (“SampleRate”ld.Fs,...“LinearAccelerationNoise”,0.0009);qlowlanoise = lowvarfilt(Accel,Gyro);
有一种方法可以看出LinearAccelerationNoise
就是看输出的重力矢量。重力向量就是方向旋转矩阵的第三列。
rmatHigh = rotmat (qHighLANoise,“帧”);rmatLow = rotmat (qLowLANoise,“帧”);gravDistHigh =√sum (rmatHigh(:, 3:)[0, 0, 1])。^ 2,1));gravDistLow =√sum (rmatLow(:, 3:)[0, 0, 1])。^ 2,1));图;情节([挤压(gravDistHigh),挤压(gravDistLow)]);标题(《欧几里得引力距离》);传奇(“LinearAccelerationNoise = 0.009”,...“LinearAccelerationNoise = 0.0009”);
的lowVarFilt
有一个低LinearAccelerationNoise
,所以它期望处于一个低线性加速度的环境中。因此,它更容易受到线性加速度的影响,正如图中前面的大变化所示。然而,由于它预期处于一个低线性加速度的环境中,加速度计信号的可信度较高。因此,一旦震动结束,方位估计会迅速收敛到垂直方向。反之亦然highVarFilt
.该滤波器受震动的影响较小,但当震动停止时,方向估计收敛到垂直方向需要更长的时间。
的MagneticDisturbanceNoise
属性使模拟磁干扰(非地磁噪声源)的方式几乎相同LinearAccelerationNoise
模型的线性加速度。
两个衰减因子属性(MagneticDisturbanceDecayFactor
和LinearAccelerationDecayFactor
)模拟噪音的变化率。对于缓慢变化的噪声源,将这些参数设置为接近1的值。对于快速变化的、不相关的噪声,将这些参数设置为接近0。一个较低的LinearAccelerationDecayFactor
使方向估计更快速地找到“下”。一个较低的MagneticDisturbanceDecayFactor
使方向估计更快地找到北。
非常大的,短磁干扰几乎完全被拒绝ahrsfilter
.考虑从静止传感器记录时施加的[0250 0] UT的脉冲。理想情况下,取向估计应该没有变化。
ld =负载('magjamming.mat');hpulse = ahrsfilter (“SampleRate”, ld.Fs);len = 1:1;qpulse = hpulse (ld.sensorData.Acceleration (len:),...ld.sensorData.AngularVelocity (len:)...ld.sensordata.magneticfield(len,:));图;timevec = 0:ld.fs :( ld.fs * numel(qpulse) - 1);绘图(TimeVec,Eulerd(QPulse,“ZYX股票”,“帧”));标题([静止轨迹方向欧拉角新队...磁干扰响应的]);传奇(“Z-rotation”,“Y-rotation”,'X-rotation');ylabel (“度”);包含(“秒”);
请注意,滤波器几乎完全排除这种磁脉冲的干扰。任何磁场强度大于4倍的ExpectedMagneticFieldStrength
被认为是一个干扰源,对这些样品忽略磁强计信号。
这里提出的算法,当适当调整,可以估计方向和抗环境噪声源的鲁棒性。重要的是要考虑传感器使用的情况,并相应地调整滤波器。