VINS_FUSIOＮ意義?

?VINS Fusion在VINS Mono的基礎上，添加了GPS等可以獲取全局觀測信息的傳感器，使得VINS可以利用全局信息消除累計誤差，進而減小閉環依賴。此外，全局信息可以使分多次運行的VINS Mono統一到一個坐標系，從而方便協同建圖和定位。

局部傳感器（如ＩＭＵ，相機，雷達等）被廣泛應用與建圖與定位算法。盡管這些傳感器能在沒有ＧＰＳ信息的區域，實現良好的局部定位和建圖效果，但這些傳感器只能提供局部觀測，限制了其應用場景：

１、第一個問題是局部觀測數據缺乏全局約束，當我們每次在不同的位置運行算法時，都會得到不同坐標系下的定位和建圖結果，因而難以將這些測量結果結合起來，形成全局效果。

２、第二個問題時基于局部觀測的估計算法必然存在累積漂移，盡管回環檢測可以在一定程度上消除漂移，但是對于數據量較大的大范圍場景，算法依然難以處理。

相比于局部傳感器，全局傳感器（如ＧＰＳ，氣壓計和磁力計等）可以提供全局觀測，這些傳感器使用全局統一坐標系，并且輸出的觀測數據的方差不隨時間累積而增加，但這些傳感器也存在一些問題，導致無法直接用于精確定位和建圖，以ＧＰＳ為例，ＧＰＳ數據通常不平滑，存在噪聲，且輸出速率低，因此，一個簡單而直觀的想法是將局部傳感器和全局傳感器結合起來，以達到局部精確全局零漂的效果，也即是ＶＩＮＳ　Fusion的核心。

其算法框架如下：

?下圖以因子圖的方式表示觀測和狀態之間的約束：

其中圓形為狀態量(如位姿，速度，偏置等)，黃色正方形為局部觀測的約束，即來自VO/VIO的相對位姿變換；而其他顏色的正方形為全局觀測的約束，比如紫色正方形為來自GPS的約束。

局部約束(殘差)的構建參考vins mono論文，計算的是相鄰兩幀之間的位姿殘差。這里只討論GPS帶來的全局約束。首先當然是將GPS坐標，也就是經緯度轉換為大地坐標系。習慣上選擇右手坐標系，x,y,z軸正方向分別是北東地或東北天方向。接下來就可以計算得到全局約束的殘差：

其中z為GPS測量值，X為狀態預測，h方程為觀測方程。X可以通過上一時刻狀態以及當前時刻與上一時刻的位姿變換計算出來。具體到本文的方法，就是利用VIO得到當前時刻和上一時刻的相對位姿dX，加到上一時刻的位姿上X(i-1)，得到當前時刻的位姿Xi。需要注意的是，這里的X以第一幀為原點。通過觀測方程h，將當前狀態的坐標轉換到GPS坐標系下。這樣就構建了一個全局約束。

之后的優化就交給BA優化器進行迭代優化，vins fusion沿用了ceres作為優化器。

代碼解析

1. GPS和VIO數據輸入

需要明確的一點是，VIO的輸出是相對于第一幀的累計位姿變換，也就是以第一幀為原點。VINS Fusion接收vio輸出的局部位姿變換(相對于第一幀)，以及gps輸出的經緯度坐標，進行融合。接受數據輸入的接口在global_fusion/src/globaOptNode.cpp文件中，接口定義的關鍵代碼如下：

/******************************************************** Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology* * This file is part of VINS.* * Licensed under the GNU General Public License v3.0;* you may not use this file except in compliance with the License.** Author: Qin Tong (qintonguav@gmail.com)GPS和VIO數據輸入*******************************************************/#include "ros/ros.h" #include "globalOpt.h" #include <sensor_msgs/NavSatFix.h> #include <nav_msgs/Odometry.h> #include <nav_msgs/Path.h> #include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Geometry> #include <iostream> #include <stdio.h> #include <visualization_msgs/Marker.h> #include <visualization_msgs/MarkerArray.h> #include <fstream> #include <queue> #include <mutex>GlobalOptimization globalEstimator; ros::Publisher pub_global_odometry, pub_global_path, pub_car; nav_msgs::Path *global_path; double last_vio_t = -1; std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue; std::mutex m_buf;void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car) {visualization_msgs::MarkerArray markerArray_msg;visualization_msgs::Marker car_mesh;car_mesh.header.stamp = ros::Time(t);car_mesh.header.frame_id = "world";car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;car_mesh.action = visualization_msgs::Marker::ADD;car_mesh.id = 0;car_mesh.mesh_resource = "package://global_fusion/models/car.dae";Eigen::Matrix3d rot;rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;Eigen::Quaterniond Q;Q = q_w_car * rot; car_mesh.pose.position.x = t_w_car.x();car_mesh.pose.position.y = t_w_car.y();car_mesh.pose.position.z = t_w_car.z();car_mesh.pose.orientation.w = Q.w();car_mesh.pose.orientation.x = Q.x();car_mesh.pose.orientation.y = Q.y();car_mesh.pose.orientation.z = Q.z();car_mesh.color.a = 1.0;car_mesh.color.r = 1.0;car_mesh.color.g = 0.0;car_mesh.color.b = 0.0;float major_scale = 2.0;car_mesh.scale.x = major_scale;car_mesh.scale.y = major_scale;car_mesh.scale.z = major_scale;markerArray_msg.markers.push_back(car_mesh);pub_car.publish(markerArray_msg); }void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg) {//printf("gps_callback! \n");m_buf.lock();gpsQueue.push(GPS_msg); //每次接收到的gps消息添加到gpsqueue隊列中m_buf.unlock(); }void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {//printf("vio_callback! \n");double t = pose_msg->header.stamp.toSec();last_vio_t = t;Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);//平移矩陣Eigen::Quaterniond vio_q; //旋轉四元陣vio_q.w() = pose_msg->pose.pose.orientation.w;vio_q.x() = pose_msg->pose.pose.orientation.x;vio_q.y() = pose_msg->pose.pose.orientation.y;vio_q.z() = pose_msg->pose.pose.orientation.z;globalEstimator.inputOdom(t, vio_t, vio_q);//將時間、平移，四元數作為預測添加到globalEstimatorm_buf.lock();while(!gpsQueue.empty()){sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();double gps_t = GPS_msg->header.stamp.toSec();printf("vio t: %f, gps t: %f \n", t, gps_t);// 10ms sync tolerance 找到vio里程數據相差在10ms之內的數據，作為匹配數據if(gps_t >= t - 0.01 && gps_t <= t + 0.01){//printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());double latitude = GPS_msg->latitude;double longitude = GPS_msg->longitude;double altitude = GPS_msg->altitude;//int numSats = GPS_msg->status.service;double pos_accuracy = GPS_msg->position_covariance[0];if(pos_accuracy <= 0)pos_accuracy = 1;//printf("receive covariance %lf \n", pos_accuracy);//if(GPS_msg->status.status > 8)globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);//關鍵，將gps數據作為觀測輸入到globalEstimator中gpsQueue.pop(); //滿足條件的gps信息彈出break;}else if(gps_t < t - 0.01)gpsQueue.pop();//過時的gps消息彈出else if(gps_t > t + 0.01)break;//說明gps消息是后來的，就不要改gps隊列，退出}m_buf.unlock();Eigen::Vector3d global_t;Eigen:: Quaterniond global_q;globalEstimator.getGlobalOdom(global_t, global_q);nav_msgs::Odometry odometry;odometry.header = pose_msg->header;odometry.header.frame_id = "world";odometry.child_frame_id = "world";odometry.pose.pose.position.x = global_t.x();odometry.pose.pose.position.y = global_t.y();odometry.pose.pose.position.z = global_t.z();odometry.pose.pose.orientation.x = global_q.x();odometry.pose.pose.orientation.y = global_q.y();odometry.pose.pose.orientation.z = global_q.z();odometry.pose.pose.orientation.w = global_q.w();pub_global_odometry.publish(odometry);pub_global_path.publish(*global_path);publish_car_model(t, global_t, global_q);// write result to filestd::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);foutC.setf(ios::fixed, ios::floatfield);foutC.precision(0);foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";foutC.precision(5);foutC << global_t.x() << ","<< global_t.y() << ","<< global_t.z() << ","<< global_q.w() << ","<< global_q.x() << ","<< global_q.y() << ","<< global_q.z() << endl;foutC.close(); }int main(int argc, char **argv) {ros::init(argc, argv, "globalEstimator");ros::NodeHandle n("~");global_path = &globalEstimator.global_path;ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);ros::spin();return 0; }

2. GPS和VIO融合

VINS Fusion融合GPS和VIO數據的代碼在global_fusion/src/globalOpt.cpp文件中，下面進行詳細介紹。

a. 接收GPS數據，接收VIO數據并轉到GPS坐標系

// 接收上面輸入的vio數據 void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ){vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};localPoseMap[t] = localPose;// 利用vio的局部坐標進行坐標變換，得到當前幀的全局位姿Eigen::Quaterniond globalQ;globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};globalPoseMap[t] = globalPose; }// 接收上面輸入的gps數據 void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy) {double xyz[3];GPS2XYZ(latitude, longitude, altitude, xyz); // 將GPS的經緯度轉到地面笛卡爾坐標系vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};GPSPositionMap[t] = tmp;newGPS = true; }

上面出現了VIO數據的局部坐標轉到GPS坐標的計算過程，其公式如下：

這個公式中的GPS2VIO出現在后面的優化過程中，計算方法為：

b. 融合優化

這里是融合的關鍵代碼，可以看出其流程如下：

構建t_array和q_array，用來存入平移和旋轉變量，方便輸入優化方程，以及在優化后取出。

利用RelativeRTError::Create()構建VIO兩幀之間的約束，輸入優化方程

利用TError::Create()構建GPS構成的全局約束，輸入優化方程

取出優化后的數據

void GlobalOptimization::optimize() {while(true){if(newGPS){newGPS = false;// ceres定義部分略去// add parammPoseMap.lock();int length = localPoseMap.size();// ********************************************************// *** 1. 構建t_array, q_array用來存優化變量，等優化后取出 ***// ********************************************************double t_array[length][3];double q_array[length][4];map<double, vector<double>>::iterator iter;iter = globalPoseMap.begin();for (int i = 0; i < length; i++, iter++){// 取出數據部分省略// 添加了parameterblockproblem.AddParameterBlock(q_array[i], 4, local_parameterization);problem.AddParameterBlock(t_array[i], 3);}map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;int i = 0;for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++){// ********************************************************// ********************* 2. VIO約束 *******************// ********************************************************iterVIONext = iterVIO;iterVIONext++;if(iterVIONext != localPoseMap.end()){Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity(); // 第i幀的變換矩陣Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity(); // 第j幀的變換矩陣// 取出數據部分省略Eigen::Matrix4d iTj = wTi.inverse() * wTj; // 第j幀到第i幀的變換矩陣Eigen::Quaterniond iQj; // 第j幀到第i幀的旋轉iQj = iTj.block<3, 3>(0, 0);Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3); // 第j幀到第i幀的平移ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),iQj.w(), iQj.x(), iQj.y(), iQj.z(),0.1, 0.01);problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);}// ********************************************************// ********************* 3. GPS約束 *******************// ********************************************************double t = iterVIO->first;iterGPS = GPSPositionMap.find(t);if (iterGPS != GPSPositionMap.end()){ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],iterGPS->second[2], iterGPS->second[3]);problem.AddResidualBlock(gps_function, loss_function, t_array[i]);}}ceres::Solve(options, &problem, &summary);// ********************************************************// ******************* 4. 取出優化結果 *****************// ********************************************************iter = globalPoseMap.begin();for (int i = 0; i < length; i++, iter++){// 取出優化結果vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};iter->second = globalPose;if(i == length - 1){Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();// 剩余的存入部分省略，得到WGPS_T_WVIOWGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();}}updateGlobalPath();mPoseMap.unlock();}std::chrono::milliseconds dura(2000);std::this_thread::sleep_for(dura);}return; }

上述代碼中出現了一個關鍵的部分，即WGPS_T_WVIO的計算。從之前的代碼中知道，這個4*4矩陣是用來做VIO到GPS坐標系的變換的。按道理講，這個矩陣應當是不需要反復計算的，畢竟第一幀和GPS的坐標變換是確定的。但是運行結果來看，這個矩陣的值是在變化的，而且有時變化比較大。這里不太懂，知乎劉知說是為了避免VIO漂移過大。

3. 總結(摘自知乎劉知)

使用場景

根據上文中分析的優化策略，global fusion的應用場景應該是GPS頻率較低，VIO頻率較高的系統。fusion 默認發布頻率位10hz，而現在的GPS可以達到20hz，如果在這種系統上使用，你可能還需要修改下VIO或者GPS頻率。另外，使用的GPS是常見的誤差較大的GPS，并不是RTK-GPS。

? ? 2. GPS與VIO時間不同步

上文提到了，在多傳感器融合系統中，傳感器往往需要做時鐘同步，那么global Fusion需要么？GPS分為為很多種，我們常見的GPS模塊精度較低，十幾米甚至幾十米的誤差，這種情況下，同不同步沒那么重要了，因為GPS方差太大。另外一種比較常見的是RTK-GPS ，在無遮擋的情況下，室外精度可以達到 2cm之內，輸出頻率可以達到20hz，這種情況下，不同步時間戳會對系統產生影響，如果VIO要和RTK做松耦合，這點還需要注意。

三、前后端詳解

總結

以上是生活随笔為你收集整理的VINS_FUSION的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。