自我使用GPS车辆定位和IMU融合场景生成
这个例子展示了如何执行自我车辆定位融合全球定位系统(GPS)和惯性测量单元(IMU)传感器数据来创建一个虚拟的场景。
使用记录车辆数据,您可以生成虚拟驾驶场景再现一个真实场景。虚拟场景让你研究和可视化这些记录的场景。产生一个可靠的虚拟场景中,您必须有准确的自我车辆定位数据,包括位置和姿态信息。
GPS传感器可以显示车辆的位置,但取向信息并不总是可用的。此外,GPS数据,经常遭受噪音和偏见。如果你估计方向使用GPS数据,你可以得到准确的结果。IMU传感器加速度计和陀螺仪等不易噪音,但只能提供相关信息(即加速度和角速度)关于自我的运动车。这个例子展示了如何使用GPS和IMU执行信息融合传感器数据来准确评估自我的位置和姿态。然后使用这个准确的自我发展轨迹来创建一个走鹃场景。如果轨迹包含漂移,明白了自我定位使用车道检测和高清地图场景的一代例子来纠正。
负荷传感器数据记录
下载ZIP文件包含Comma2k19数据集的一个子集(1),然后解压文件。这个文件包含记录数据从相机、GPS和IMU传感器和地面真实自我的轨迹信息。在本例中,您使用的相机数据的视觉验证生成的场景。
dataFolder = tempdir;dataFilename =“CommaData.zip”;url =“https://ssd.mathworks.com/万博1manbetxsupportfiles/driving/data/”+ dataFilename;filePath = fullfile (dataFolder dataFilename);如果~ isfile (filePath) websave (filePath url)结束解压缩(filePath dataFolder)
GPS数据装载到工作区。
数据集= fullfile (dataFolder,“CommaData”);data =负载(fullfile(数据集,“sensorData”+ filesep +“gps.mat”));gpsData = data.gpsData;
gpsData这些列一个表:
时间戳——时间,以秒为单位,GPS数据收集。纬度——纬度坐标自我的车辆的价值。单位是在度。经度——经度坐标自我的车辆的价值。单位是在度。高度——高度协调自我的车辆的价值。单位是米。
可视化的GPS路径自我车辆使用geoplayer对象。
zoomLevel = 14;中心= ([gpsData意思。纬度gpsData。经度]); player = geoplayer(center(1),center(2),zoomLevel); plotRoute(player,gpsData.latitude,gpsData.longitude)
IMU数据加载到工作区。记录的数据包含线性加速度值使用加速度计使用陀螺仪传感器和角速度值记录。这些数据是在North-East-Down (NED)参考系。
data =负载(fullfile(数据集,“sensorData”+ filesep +“imu.mat”));imuData = data.imuData;
imuData这些列一个表:
时间戳——时间,以秒为单位,IMU数据收集。longitudinalAcceleration——纵向分量的线性加速度的自我。单位是米每秒的平方。lateralAcceleration——横向分量的线性加速度的自我。单位是米每秒的平方。verticalAcceleration——垂直分量的线性加速度的自我。单位是米每秒的平方。rollRate-角速度的滚转角速度分量的自我。单位弧度每秒。pitchRate球率组件角速度的自我。单位弧度每秒。yawRate-偏航率分量的角速度的自我。单位弧度每秒。
情节IMU数据使用helperPlotIMUData函数。
helperPlotIMUData (imuData)
融合GPS和IMU数据,这个示例使用一个扩展卡尔曼滤波(EKF)和曲调过滤器参数来得到最优的结果。你使用地面实况信息,这是Comma2k19给定数据集和获得的过程中描述(1),初始化和优化滤波器的参数。
地面实况数据加载,NED参考系,进入工作区。
data =负载(fullfile(数据集,“groundTruthData”+ filesep +“groundTruth.mat”));groundTruthData = data.groundTruthData;
groundTruthData这些列一个表:
时间戳——时间,在几秒钟内,地面实况数据的收集。位置——地面真理自我车辆的位置。单位是米。取向——地面实况取向的自我车辆(偏航、俯仰、滚)格式。单位的弧度。
数据进行预处理
得到GPS纬度、经度和海拔高度数据。
(gpsData gpsLLA =。纬度gpsData。经度gpsData。高度];
GPS数据转换为时间格式,因为过滤和调谐器支持这个格式。万博1manbetx
gpsData =时间表(秒(gpsData.timeStamp), gpsLLA);gpsData.Properties.VariableNames (1) =“全球定位系统”;
得到线性加速度和角速度数据从乌兹别克斯坦伊斯兰运动传感器。
加速度= [imuData。longitudinalAcceleration imuData。lateralAccelerationimuData。verticalAcceleration]; angularVelocity = [imuData.rollRate imuData.pitchRate imuData.yawRate];
IMU数据转化为时间格式,因为过滤和调谐器支持这个格式。万博1manbetx
imuData =时间表(秒(imuData.timeStamp)、加速度、angularVelocity);imuData.Properties.VariableNames (1) =“加速器”;imuData.Properties.VariableNames (2) =“陀螺”;
地面实况自我车辆的位置。
gtPosition = groundTruthData.position;
地面实况取向的自我。
gtOrientation =四元数(groundTruthData.orientation,“欧拉”,“ZYX股票”,“帧”);
一个四元数和其负面表示相同的取向。发现四元数与一个非负实数部分,转化他们使用helperConvertQuatPositive函数更好的调谐器收敛。
gtOrientation = helperConvertQuatPositive (gtOrientation);
地面实况数据转化为时间格式。
groundTruthData =时间表(秒(groundTruthData.timeStamp), gtOrientation gtPosition);groundTruthData.Properties.VariableNames (1) =“定位”;groundTruthData.Properties.VariableNames (2) =“位置”;
同步地面真理和传感器数据使用helperSyncGPSIMUWithGroundTruth函数。函数也清理地面实况数据包含通过删除行南值。你使用这些同步数据调整过滤器。
[sensorData, groundTruthData] = helperSyncGPSIMUWithGroundTruth (gpsData、imuData groundTruthData);
正常化传感器和地面实况数据的时间戳。
开始时间= sensorData.Time (1);sensorData。时间= sensorData。时间,开始时间;groundTruthData。时间= groundTruthData。时间,开始时间;
显示传感器数据的前五项。由于GPS数据的采样率小于IMU数据、GPS数据不可用时间戳。表代表失踪的GPS数据使用南值。
:sensorData (1:5)
ans =5×3的时间表时间加速度计陀螺仪GPS _______ __________________________________ _______________________________________ ____________________________ 0秒-0.11725 0.014359 9.1286 -0.003006 -0.013123 0.0018005南南南0.050007秒0.54796 -0.26561 9.131 0.0043182 0.0076447 0.00059509南南南0.10002秒1.0504 -0.033493 9.7603 0.0031128 0.024765 0.00059509南南南0.15002秒0.93559 0.0071716 10.047 0.014099 0.014969 -0.0018463 37.685 -122.47 23.824 0.20001秒0.10768 -0.10049 9.9757 0.023865 0.011322 -0.00064087南南南
地面实况数据的显示前五项。
:groundTruthData (1:5)
ans =5×2时间表时间取向位置_________________________________ _______ * * * 0秒1×1四元数0 0 0 0.050007秒1×1四元数-1.4219 -0.071088 0.057686 0.10002秒1×1四元数-2.8477 -0.141 0.11388 0.15002秒1×1四元数-4.2755 -0.21034 0.16785 0.20001秒1×1四元数-5.7056 -0.27938 0.21915
GPS和IMU传感器数据融合
这个例子使用一个扩展卡尔曼滤波器(EKF)异步融合GPS,加速度计和陀螺仪数据使用一个insEKF(传感器融合和跟踪工具箱)对象。创建传感器模型的加速度计、陀螺仪和GPS传感器。
直接使用= insAccelerometer;hgyro = insGyroscope;计= insGPS;
设置参考位置的GPS传感器模型,利用GPS数据对应于第一个地面实况数据点位置。因为GPS数据不可用时间戳,找到相对应的时间戳(非第一有效南)的GPS数据。然后,时间戳,得到地面实况的位置数据。Use这个值作为当地NED坐标系的原点估计GPS第一地面真理价值的位置。
firstNonNANGPSPosition =找到(~ isnan (sensorData.GPS), 1,“第一”);refLoc = ned2lla (groundTruthData.Position (1:)——groundTruthData.Position (firstNonNANGPSPosition:), sensorData.GPS (firstNonNANGPSPosition:)“椭球体”);计画。ReferenceLocation = refLoc;
如果你没有地面实况数据,估计参考位置使用这些选项之一:
发现前几的意思是GPS数据点。
如果你有从当车辆是静止的GPS数据,发现GPS数据的均值点的时间戳。
创建一个insEKF利用传感器模型对象。
过滤器= insEKF(使用、hgyro计)
过滤器= insEKF属性:状态:[22×1双]StateCovariance:[22×22双]AdditiveProcessNoise:[22×22双]MotionModel: [1×1 insMotionPose]传感器:{[1×1 insAccelerometer] [1×1 insGyroscope] [1×1 insGPS]} SensorNames:{“加速器”“陀螺”“GPS”} ReferenceFrame:“内德”
初始化位置的一部分在过滤器使用第一个地面实况的位置数据。初始化状态估计误差协方差矩阵的对角元素对应的位置状态。
stateparts(过滤器,位置= groundTruthData.Position (1:));statecovparts(过滤器,位置= 1 e);
如果你没有地面实况数据,初始化滤波器的位置状态的一部分,用GPS测量数据。最初几个GPS测量转换为本地笛卡尔坐标使用latlon2local函数的引用位置和输出坐标计算的均值。如果你这个例子中适应自己的应用程序和数据的准确性的不确定性用于初始化,然后设置的状态估计误差协方差矩阵的值高于那些在这个例子中使用。
初始化速度的一部分在过滤器使用的时间和位置数据。一个健壮的估计,这个例子中发现的意思是前十的速度值。
diffTime =意味着(diff(秒(groundTruthData.Time)));vtmp = diff (groundTruthData.Position)。/ diffTime;vinit =意味着(vtmp (1:10,:));stateparts(过滤器,速度= vinit);
初始化加速国家的一部分使用时间和速度数据进行过滤。健壮的估计,这个例子中发现的前10加速度值。
atmp = diff (vtmp)。/ diffTime;事情=意味着(atmp (1:10,:));stateparts(过滤器,加速度=事情);
初始化方向滤波器的状态的一部分。得到一个健壮的估计,这个例子中发现的第一个10地面真值。
qinit = meanrot (groundTruthData.Orientation (1:10));stateparts(过滤器,取向=紧凑(qinit));statecovparts(过滤器,取向= 1 e-5);
如果你没有地面实况数据,那么您可以使用helperEstimateOrientation函数来估计初始取向状态的传感器数据。函数估计的初始定位速度估计使用传感器数据,例如,qinit = helperEstimateOrientation(速度)。
由于陀螺仪读数传感器坐标系和状态估计需要在导航框架,将第一个陀螺仪测量从传感器坐标系为导航坐标系和用它来初始化角速度在过滤器的一部分。
avinit = rotatepoint (qinit sensorData.Gyroscope (1:));stateparts(过滤器,AngularVelocity = avinit);statecovparts(过滤器,AngularVelocity = 1 e - 3);
减少之间的估计误差传感器数据融合和地面真理,调优参数insEKF过滤器使用helperTuneInsEKF函数。函数找到最优值的测量噪声和添加剂过程噪声过滤器。有关优化的更多信息insEKF过滤器,看到调优(传感器融合和跟踪工具箱)。
[tunedFilter, tunedMeasureNoise] = helperTuneInsEKF(过滤、sensorData groundTruthData);
代码生成成功。
如果你没有地面实况数据,那么您需要手动调整这些参数最优性能。
既然你有批处理数据,您可以使用平滑。执行批处理传感器数据融合使用estimateStates(传感器融合和跟踪工具箱)函数,它返回平滑状态估计。
(~,sm) = estimateStates (tunedFilter、sensorData tunedMeasureNoise);
提取融合位置、方向和时间信息从平滑状态估计。
fusedPosition = sm.Position;fusedOrientation = sm.Orientation;fusedTime =秒(sm.Time);
想象自我使用车辆位置和航向方向helperPlotHeadingAndPosition函数。
helperPlotHeadingAndPosition (fusedPosition fusedOrientation 30);
生成走鹃场景
生成一个走鹃场景可视化自我车辆轨迹后GPS和IMU传感器数据融合。走鹃要求中的位置和姿态数据East-North-Up参考系(ENU表示)。将融合NED ENU表示参考坐标系的位置和姿态数据使用helperConvertNED2ENU函数。
[fusedPositionENU, fusedOrientationENU] = helperConvertNED2ENU (fusedPosition fusedOrientation);
添加的高度组件引用位置中使用insEKF过滤器的高度分量融合位置和得到的绝对高度融合自我轨迹。
fusedPositionENU (:, 3) = fusedPositionENU (:, 3) + refLoc (3);
走鹃的场景中可以使用CSV文件导入演员轨迹。写一个自我融合轨迹CSV文件使用helperWriteRRCSV函数。
helperWriteRRCSVTrajectory (fusedTime fusedPositionENU fusedOrientationENU,“fusedTrajectory.csv”);
可视化传感器融合的性能改进,从GPS数据提取自我发展轨迹。
gpsData = sensorData.GPS;gpsTime =秒(sensorData.Time);rmRows = isnan (gpsData (: 1));gpsData (rmRows:) = [];gpsTime (rmRows:) = [];
将GPS坐标转换为本地笛卡尔坐标使用latlon2local函数。
[xEast, yNorth, z上]= latlon2local (gpsData (: 1), gpsData (:, 2), gpsData (:, 3), refLoc);gpsPositionENU = [xEast yNorth z上);
得到的绝对高度GPS轨迹。
gpsPositionENU (:, 3) = gpsPositionENU (:, 3) + refLoc (3);
提取的GPS轨迹数据写入一个CSV文件。
helperWriteRRCSVTrajectory (gpsTime gpsPositionENU,“gpsTrajectory.csv”);
使用MATLAB®打开走鹃,指定您的本地RoadRunner安装文件夹的路径。这个代码显示了默认的安装位置的路径在Windows®。
rrAppPath =“C: \ Program Files \ RoadRunner R2023a \ bin \ win64”;
指定的路径走鹃项目。这个代码显示了一个示例项目文件夹的路径在Windows。
rrProjectPath =“C: \ RR \ MyProjects”;
打开走鹃项目使用指定的路径。
rrApp =走鹃(rrProjectPath InstallationFolder = rrAppPath);
这个示例提供了一个走鹃的场景,这是符合Comma2k19这个示例中所使用的数据集。复制场景走鹃项目和开放。
拷贝文件(“commaScene.rrscene”fullfile (rrProjectPath“场景”));openScene (rrApp“commaScene.rrscene”);
创建一个新的走鹃场景。
newScenario (rrApp);
从相应的CSV文件导入GPS轨迹。改变汽车的颜色在视觉上区分从其他车辆。选择场景中的车辆编辑画布。在属性窗格中,单击颜色框,选择红色补丁。
格式=“CSV轨迹”;文件名= fullfile (pwd,“gpsTrajectory.csv”);importScenario (rrApp,文件名格式)
进口的融合轨迹自我车辆从相应的CSV文件。
文件名= fullfile (pwd,“fusedTrajectory.csv”);importScenario (rrApp,文件名格式)
创建一个场景模拟对象当前走鹃场景中使用createSimulation函数。
rrSim = createSimulation (rrApp);
连接状态:1连接到走鹃场景服务器localhost: 63119年,客户机id {21 a352be - 06 - df - 4 - bb6 - 98 - ae - 6 - c4a72b40dd1}
endTime = fusedTime(结束);集(rrSim MaxSimulationTime = endTime);集(rrSim StepSize = 0.1);设置(rrSim日志=“上”);
监控的状态模拟和等待模拟完成。因为走鹃场景不能删除演员退出后时期,由于碰撞场景仿真可以失败。碰撞避免停止场景,消除失败条件使用这些步骤:
1。在逻辑编辑器中,单击条件节点结束时的场景。
2。在属性窗格中,单击删除失败的条件。
设置(rrSim SimulationCommand =“开始”)而比较字符串(get (rrSim,“SimulationStatus”),“运行”)simstatus = (rrSim“SimulationStatus”);暂停(1)结束
查看场景从自我车辆视图或追看,模拟窗格中,在相机节中,设置相机视图要么遵循或前面。然后,设置演员属性来vehicle2,融合后的自我车辆轨迹的场景在这个例子。
这图显示了相机记录数据和生成走鹃场景。红色的车在走鹃场景中遵循原始GPS轨迹,展示了一些不稳定和偏见。白色的车纠正后的不稳定和偏见在它的运动轨迹由GPS和IMU传感器数据融合。
引用
[1]谢弗,哈拉尔德埃德尔桑塔纳,安德鲁•Haden和里卡多。Biasini。“数据:上下班Comma2k19数据集,”2018年。https://doi.org/10.48550/arXiv.1812.05752。
