通过惯性传感器融合估计取向

取向

对象的定位描述其旋转相对于一些坐标系统,有时被称为父母坐标系统,在三维空间中。

下面的算法、固定、父母坐标系使用North-East-Down (NED)。NED有时被称为全球坐标系统或参考系。NED参考系,轴点北,y轴分东,z轴点向下。x - y平面内德被认为是地球的地方切平面。根据算法,北可能磁北或真北。在这个例子中使用磁北的算法。

如果指定了,下面的算法可以估计方向相对于East-North-Up (ENU表示)父母代替NED坐标系统。

一个对象可以被认为是拥有自己的坐标系统,通常称为本地或孩子坐标系统。这个孩子坐标系旋转的物体相对于父坐标系统。如果没有翻译,两种坐标系统的起源重叠。

取向量计算是一个旋转,需要大量的父母给孩子参考系参考系。由四元数表示旋转或旋转矩阵。

类型的传感器

方向估计,三种类型的传感器是常用的:加速度计、陀螺仪和磁力计。加速度计测量的加速度。陀螺仪测量角速度。磁力计测量当地的磁场。不同的算法用于融合的不同组合传感器估计取向。

传感器数据

在这个例子中,大多数人使用相同的传感器数据集。陀螺仪,加速度计和磁强计传感器数据被记录,同时设备旋转在三个不同的轴:首先在其本地轴,然后围绕z轴,最后绕着它的轴。设备的轴通常是向南指出实验的持续时间。

ld =负载(“rpy_9axis.mat”);acc = ld.sensorData.Acceleration;陀螺= ld.sensorData.AngularVelocity;杂志= ld.sensorData.MagneticField;页= poseplot;

Accelerometer-Magnetometer融合

的ecompass功能融合加速度计和磁强计数据。这是一个无记忆的算法,不需要参数调优,但该算法对传感器噪声极为敏感。

量化宽松政策= ecompass (acc, mag);为2 = 1:尺寸(acc, 1)集(pp、“定位”量化宽松政策(ii)) drawnowlimitrate结束

请注意,ecompass算法正确发现北的位置。然而,由于函数是无记忆的,估计运动不平稳。该算法可以作为一个初始化的步骤在一个方向过滤或提出的一些技术低通滤波器使用四元数SLERP取向可以用来光滑的运动。

Accelerometer-Gyroscope融合

以下对象估计方向使用误差状态卡尔曼滤波器或互补滤波器。的误差状态卡尔曼滤波器是标准的评估系统的过滤和允许很多不同的方面使用相应的噪声参数调优。互补滤波器可以用作代替系统内存限制,和最小的可调参数,允许更容易更好的成本优化的配置。

的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 = false);为2 = 1:尺寸(acc, 1) qimu = cfilt (acc (ii):),陀螺(ii):));集(pp、“定位”qimu) drawnowlimitrate结束

虽然imufilter和complementaryFilter算法产生显著平滑的运动估计,相比ecompass他们不正确的估计,北的方向。的imufilter不处理磁强计数据,所以它只是假定设备的轴是最初指向北。的运动估计imufilter是相对于初始估计取向。的complementaryFilter使相同的假设HasMagnetometer属性设置为假。

Accelerometer-Gyroscope-Magnetometer融合

姿态和航向参考系统(AHRS)由9-axis系统使用一个加速度计、陀螺仪和磁强计计算取向。的ahrsfilter和complementaryFilter系统对象™结合最好的前面的算法来产生一个平稳变化估计设备的取向,而正确地估计北的方向。的complementaryFilter使用相同的互补滤波算法,与一个额外的步骤包括磁强计和改善方向估计。就像imufilter,ahrsfilter算法还使用一个误差状态卡尔曼滤波器。除了陀螺仪偏见、ahrsfilter有一些检测和拒绝轻微的电磁干扰的能力。

ifilt = ahrsfilter (SampleRate = ld.Fs);为2 = 1:尺寸(acc, 1) qahrs = ifilt (acc (ii):),陀螺(ii):),镁(ii):));集(pp、“定位”qahrs) drawnowlimitrate结束

cfilt = complementaryFilter (SampleRate = ld.Fs);为2 = 1:尺寸(acc, 1) qahrs = cfilt (acc (ii):),陀螺(ii):),镁(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与一个LinearAccelerationNoise9 e - 3 $$(米/{\mathrm{年代}}^2{)}^2$$响应振动轨迹,而一LinearAccelerationNoise9的军医 $$(米/{\mathrm{年代}}^2{)}^2$$。

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,陀螺);

看到的效果的一种方式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属性使建模磁干扰(non-geomagnetic噪声源)一样LinearAccelerationNoise模型的线性加速度。

这两个衰减系数属性(MagneticDisturbanceDecayFactor和LinearAccelerationDecayFactor)模型的速率变化的噪音。慢变噪声源,设置这些参数值接近1。迅速变化,不相关的噪音,设置这些参数接近0。一个较低的LinearAccelerationDecayFactor使定位估计更快地找到“向下”。一个较低的MagneticDisturbanceDecayFactor使定位估计更快地找到北。

很大,短期磁干扰几乎完全的拒绝ahrsfilter。考虑一个脉冲(250 0)但从固定传感器应用在记录。理想情况下,应该没有改变方向估计。

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 *元素个数(qpulse) - 1);情节(timevec eulerd (qpulse“ZYX股票”,“帧”));标题([“固定轨迹方向欧拉角”换行符…“磁干扰反应”]);传奇(“Z-rotation”,“Y-rotation”,“X-rotation”);ylabel (“度”);包含(“秒”);

注意过滤器几乎完全拒绝这个磁脉冲干扰。任何磁场强度大于四倍ExpectedMagneticFieldStrength被认为是一个干扰来源和为这些样品磁强计信号将被忽略。

结论

这里给出的算法,当适当地调整,使估计的方向和健壮的对环境噪声来源。重要的是要考虑的情况下使用传感器,并相应地调整过滤器。