多传感器融合定位
Source
Edit
History

GNSS-IMU数据仿真器使用说明

Catalogue
  1. 1. GNSS-INS-SIM
  2. 2. 基本使用方式
    1. 2.1. import
    2. 2.2. 选择或定义IMU误差模型
      1. 2.2.1. 选择现成的模型
      2. 2.2.2. 自定义误差模型
    3. 2.3. 设置运动轨迹
      1. 2.3.1. 初始值
      2. 2.3.2. 运动指令
    4. 2.4. 创建算法对象
      1. 2.4.1. alan
      2. 2.4.2. 积分
      3. 2.4.3. Manhony
      4. 2.4.4. loosely couple (松耦合) ——算法未完成
    5. 2.5. 创建仿真器对象
    6. 2.6. 开始仿真
    7. 2.7. 结果显示
    8. 2.8. 完整imu数据仿真示例
      1. 2.8.1. 生成6轴imu数据
    9. 2.9. 一些数据成员说明

GNSS-INS-SIM

GNSS-INS-SIM是GNSS/INS模拟项目，它生成参考轨迹，IMU传感器输出，GPS输出，里程表输出和磁力计输出。用户选择/设置传感器模型，定义航路点并提供算法，gnss-ins- sim可以为算法生成所需的数据，运行算法，布局仿真结果，保存仿真结果并生成摘要。

基本使用方式

import

1
2
3
4
5
import os
import math
import numpy as np
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim

选择或定义IMU误差模型

选择现成的模型

内置了三种IMU模型：“低精度”，“中精度”和“高精度”：

1
imu = imu_model.IMU(accuracy='mid-accuracy', axis=6, gps=False)

自定义误差模型

1
2
3
4
5
6
7
8
9
10
11
12
imu_err = {'gyro_b': np.array([5.0, -5.0, 2.5]) * 3600.0,
           'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
           'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,
           'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
           'accel_b': np.array([50.0e-3, 50.0e-3, 50.0e-3]) * 0.0,
           'accel_vrw': np.array([0.03119, 0.03009, 0.04779]) * 1.0,
           'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]) * 1.0,
           'accel_b_corr': np.array([200.0, 200.0, 200.0]),
           'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
          }
# do not generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)

设置运动轨迹

初始值

  • 初始位置应以LLA（纬度，经度和高度）形式给出。
  • 初始速度在车身框架中指定。
  • 初始姿态由ZYX顺序的欧拉角表示。
Ini lat (deg) ini lon (deg) ini alt (m) ini vx_body (m/s) ini vy_body (m/s) ini vz_body (m/s) ini yaw (deg) ini pitch (deg) ini roll (deg)
32 120 0 0 0 0 0 0 0

初始欧拉角为0度俯仰角，0度侧倾角和0度偏航角，这意味着车辆处于水平状态且其x轴指向北方

运动指令

运动命令定义了车辆如何从其初始状态开始运动。仿真将根据命令生成真实的角速度，加速度，磁场，位置，速度和姿态。结合传感器误差模型，这些真实值将用于生成陀螺仪，加速度计，磁力计和GPS输出。

您可以添加更多的运动命令来指定车辆的姿态和速度。 您还可以为每个命令定义车辆的GPS可见性。

支持五种命令类型:

命令类型 功能
1 直接定义欧拉角变化率(角速度)和速度变化率(加速度)。变化率在第2〜7栏给出。单位是度/秒和米/秒/秒。第8列给出了命令将持续多长时间。如果要完全控制每个命令的执行时间，则应始终将运动类型选择为1
2 定义要达到的绝对姿态和绝对速度。目标姿态和速度由第2〜7列给出。单位是deg和m/s。第8列定义了执行命令的最长时间。如果实际执行时间少于最大时间，则将不使用剩余时间，并且将立即执行下一条命令。如果命令不能在最大时间内完成，则在最大时间后直接执行下一个命令。
3 定义姿态变化和速度变化。姿态和速度变化在第2〜7列中给出。单位是度和m / s。第8列定义了执行命令的最长时间。
4 定义绝对姿态和速度变化。绝对姿态和速度变化由第2〜7列给出。单位是度和m / s。第8列定义了执行命令的最长时间。
5 定义姿态变化和绝对速度。姿态变化和绝对速度由第2〜7列给出。单位是度和m / s。第8列定义了执行命令的最长时间。

例子

创建算法对象

alan

1
algo = allan_analysis.Allan() # an Allan analysis demo algorithm

积分

积分接口如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
class FreeIntegration(object):
    '''
    Integrate gyro to get attitude, double integrate linear acceleration to get position.
    '''
    def __init__(self, ini_pos_vel_att, earth_rot=True):
        '''
        Args:
            ini_pos_vel_att: 9x1 numpy array containing initial position, velocity and attitude.
                3x1 position in the form of LLA, units: [rad, rad, m];
                3x1 velocity in the body frame, units: m/s;
                3x1 Euler anels [yaw, pitch, roll], rotation sequency is ZYX, rad.
            earth_rot: Consider the Earth rotation or not. Only used when ref_frame=0.
        '''
  • ini_pos_vel_att: 9x1的numpy数组，包括初始位置(LLA)、速度(body-frame)、姿态(ZYX)
  • earth_rot: 是否考虑地球自转

使用案例:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#### Algorithm
# Free integration in a virtual inertial frame
from demo_algorithms import free_integration_odo
from demo_algorithms import free_integration

ini_pos_vel_att = np.genfromtxt(motion_def_path+"//motion_def-long_drive.csv",\
                                delimiter=',', skip_header=1, max_rows=1)
ini_pos_vel_att[0] = ini_pos_vel_att[0] * D2R
ini_pos_vel_att[1] = ini_pos_vel_att[1] * D2R
ini_pos_vel_att[6:9] = ini_pos_vel_att[6:9] * D2R
# add initial states error if needed
ini_vel_err = np.array([0.0, 0.0, 0.0]) # initial velocity error in the body frame, m/s
ini_att_err = np.array([0.0, 0.0, 0.0]) # initial Euler angles error, deg
ini_pos_vel_att[3:6] += ini_vel_err
ini_pos_vel_att[6:9] += ini_att_err * D2R
# create the algorith object
algo = free_integration.FreeIntegration(ini_pos_vel_att)

Manhony

使用Manhony算法进行姿态解算

1
2
3
4
#### Algorithm
# ECF based inclinometer
from demo_algorithms import inclinometer_mahony
algo = inclinometer_mahony.MahonyFilter()

loosely couple (松耦合) ——算法未完成

1
2
3
4
#### Algorithm
# loosely couple INS algorithm
from demo_algorithms import ins_loose
algo = ins_loose.InsLoose()

创建仿真器对象

1
2
3
4
5
6
7
8
# start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                  motion_def_path+"//motion_def-90deg_turn.csv",
                  ref_frame=1,
                  imu=imu,
                  mode=None,
                  env=None,
                  algorithm=None)

具体接口描述如下:

  • fs: [fs_imu, fs_gps, fs_mag] , 包括imu采样频率，也是仿真频率，gps频率
  • motion_def: 设备日志或者仿真运动规划
  • ref_frame：参考坐标系
    • 0：表示使用NED坐标系，x轴指向正北(地理北，而不是磁北)，y轴指向东，z轴指向地，位置使用LLA表示
    • 1：虚拟的惯性坐标系?
  • imu: imu误差模型，如果使用的是设备的日志输出来仿真，则把imu误差模型设置为None,即imu=None
  • mode: 使用默认的None即可
  • env: 振动模型，有三种
  • algorithm: 算法模型

开始仿真

1
2
3
sim.run()     # run for 1 time
sim.run(1)    # run for 1 time
sim.run(100)  # run for 100 times

结果显示

1
2
3
4
5
6
7
8
9
10
# generate a simulation summary,
# and save the summary and all data in directory './data'.
# You can specify the directory.
sim.results('./data/')

# generate a simulation summary, do not save any file
sim.results()

# plot interested data
sim.plot(['ref_pos', 'gyro'], opt={'ref_pos': '3d'})

完整imu数据仿真示例

生成6轴imu数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
import os
import math
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim

# globals
D2R = math.pi/180

motion_def_path = os.path.abspath('.//demo_motion_def_files//')
fs = 100.0          # IMU sample frequency
fs_gps = 10.0       # GPS sample frequency
fs_mag = fs         # magnetometer sample frequency, not used for now

def test_path_gen():
    '''
    test only path generation in Sim.
    '''
    #### choose a built-in IMU model, typical for IMU381
    imu_err = 'mid-accuracy'
    # generate GPS and magnetometer data
    imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)

    #### start simulation
    sim = ins_sim.Sim([fs, fs_gps, fs_mag],
                      motion_def_path+"//motion_def-3d.csv",
                      ref_frame=1,
                      imu=imu,
                      mode=None,
                      env=None,
                      algorithm=None)
    sim.run(1)
    # save simulation data to files
    sim.results('')
    # plot data, 3d plot of reference positoin, 2d plots of gyro and accel
    sim.plot(['ref_pos', 'gyro', 'gps_visibility'], opt={'ref_pos': '3d'})

if __name__ == '__main__':
    test_path_gen()

一些数据成员说明

名称 描述
'ref_frame' 参考坐标系用作导航框架和姿态参考。0：NED（默认），x轴指向地理北部，y轴指向东，z轴指向下。位置将以LLA形式表示，并且车辆相对于ECEF框架的速度将以本地NED框架表示。1：虚拟惯性系，其常数g，x轴指向地理或磁北，z轴指向g，y轴构成右手坐标系。在此帧中，位置和速度都将为[xyz]形式。注意：对于此虚拟惯性框架，位置确实是ecef中的初始位置与病毒性惯性框架中的相对位置之和。实际上，不应添加在不同帧中表达的两个向量。这是在此处以保留所有有用信息以生成.kml文件的方式完成的。如果使用此结果，请记住这一点。
'fs' IMU的采样频率，单位：Hz
'fs_gps' GNSS的采样频率，单位：Hz
'fs_mag' 磁力计的采样频率，单位：Hz
'时间' 时间序列对应于IMU样本，单位：秒。
'gps_time' 时间序列对应于GNSS样本，单位：秒。
'algo_time' 对应于算法输出的时间序列，单位：['s']。如果您的算法输出数据速率与输入数据速率不同，则应在算法输出中包含“ algo_time”。
'gps_visibility' 指示GPS是否可用。1表示是，0表示否。
'ref_pos' 导航框中的真实位置。当用户选择NED（ref_frame = 0）作为导航框时，位置将以[纬度，经度，高度]的形式给出，单位为['rad'，'rad'，'m']。当用户选择虚拟惯性框架时，位置（初始位置+相对于框架原点的位置）将以[x，y，z]的形式给出，单位为['m'，'m'，'m ']。
'ref_vel' NED帧中表示的导航/参考帧的真实速度，单位：['m / s'，'m / s'，'m / s']。
'ref_att_euler' 真实姿态（欧拉角，ZYX旋转顺序），单位：['rad'，'rad'，'rad']
'ref_att_quat' 真实态度（四元数）
'ref_gyro' 车架中的真实角速度，单位：['rad / s'，'rad / s'，'rad / s']
'ref_accel' 车身框架中的真实加速度，单位：['m / s ^ 2'，'m / s ^ 2'，'m / s ^ 2']
'ref_mag' 车架中的真实地磁场，单位：['uT'，'uT'，'uT']（仅当IMU对象中的axis = 9时可用）
'ref_gps' NED（LLA）的真实GPS位置/速度，['rad'，'rad'，'m'，'m / s'，'m / s'，'m / s']，['m'，'m '，'m'，'m / s'，'m / s'，'m / s']用于虚拟惯性框架（xyz）（仅当gps = IMU对象中为True时可用）
陀螺仪 陀螺仪测量结果“ ref_gyro”出现错误
'加速' 加速度计测量，“ ref_accel”有错误
'mag' 磁力计测量，“ ref_mag”有错误
'全球定位系统' GPS测量，“ ref_gps”有错误
'ad_gyro' 陀螺仪的Allan std，单位：['rad / s'，'rad / s'，'rad / s']
'ad_accel' 加速度的Allan std，单位：['m / s2'，'m / s2'，'m / s2']
“ pos” 来自算法的仿真位置，单位：对于NED（LLA）为['rad'，'rad'，'m']，对于虚拟惯性系（xyz）为['m'，'m'，'m']。
'vel' 来自算法的仿真速度，单位：['m / s'，'m / s'，'m / s']
'att_euler' 来自算法的模拟态度（Euler，ZYX），单位：['rad'，'rad'，'rad']
'att_quat' 来自算法的模拟态度（四元数）
'wb' 陀螺仪偏差估计，单位：['rad / s'，'rad / s'，'rad / s']
'ab' 加速度计偏差估计，单位：['m / s ^ 2'，'m / s ^ 2'，'m / s ^ 2']
'gyro_cal' 校准陀螺仪输出，单位：['rad / s'，'rad / s'，'rad / s']
'accel_cal' 校准的加速器输出，单位：['m / s ^ 2'，'m / s ^ 2'，'m / s ^ 2']
'mag_cal' 校准的磁力计输出，单位：['uT'，'uT'，'uT']
'软铁' 3x3软铁校准矩阵
'hard_iron' 硬铁校准，单位：['uT'，'uT'，'uT']