引言

之前一直用的vins-fusion来做无人机的视觉定位，但是由于记载电脑低算力（800块的电脑，cpu为intelN150、无gpu），以及飞行条件比较极端（需要移动平台上初始化vins并起飞），vins-fusion的定位经常不稳定：

1.初始化失败

2.无人机降落或者撞地导致vins定位漂移严重

3.长时间定位imu误差积累严重

而最近新出的这个SchurVins据说能够吊打一切其他vins，而且对电脑cpu算力的要求很低

由于笔者课业繁忙，将持续少量更新

相关配置

• 硬件配置，d435i+px4 2.0.4.8 飞控（imu为mpu6000）
• 环境配置，ubuntu20.04+ros+noetic

部署步骤

SchurVins的部署

代码网址：https://github.com/bytedance/SchurVINS

参考教程：自己部署运行SchurVINS-CSDN博客

SchurVins配置

使用双目相机的红外图像话题

我使用的是realsense相机d435i，可以参考fast-drone-250的配置，驱动相机后可以获得/camera/infra1/image_rect_raw,/camera/infra2/image_rect_raw话题，在SchurVins的euroc_vio_stereo.launch文件内的相机输入修改为这两个话题即可

<!-- Launch File for running svo with ceres backend from bag-file -->
<launch>
  <arg name="calib_file" default="$(find svo_ros)/param/calib/euroc_stereo.yaml"/>

  <!-- SVO node -->
  <node pkg="svo_ros" type="svo_node" name="svo" clear_params="true" output="screen" >

    <!-- Camera topic to subscribe to -->
    <param name="cam0_topic" value="/camera/infra1/image_rect_raw" type="str" />
    <param name="cam1_topic" value="/camera/infra2/image_rect_raw" type="str" />
    <param name="remote_key_topic" value="/remote_key" type="str" />
	
    <!-- Imu topic to subscribe to -->
    <param name="imu_topic" value="/mavros/imu/data_raw" type="str" />

    <!-- Camera calibration file -->
    <param name="calib_file" value="$(arg calib_file)" />

    <!--Parameters-->
    <rosparam file="$(find svo_ros)/param/vio_stereo.yaml" />
    <param name='runlc' value='false' />

  </node>

  <!-- RVIZ -->
  <!-- <node name="vis" pkg="rviz" type="rviz" args=" -d $(find svo_ros)/rviz_config_vio.rviz" /> -->

</launch>

修改euroc_stereo.yaml内相机的内参

修改vio_stereo.yaml内SchurVins的各种参数，主要是要增加特征点的数目，过少的特征点会导致定位很容易飘，如用手遮挡相机、近处过多物体遮挡相机时会导致定位迅速丢失，是十分严重的问题，可以尝试打开边缘检测。

解决schurVins的报错

主要是会导致程序进程终止的问题，实际测试发现其他一些红字的报错影响不大（除了初始化失败的报错），特征点数目过少的问题不大

• imu接收频率低导致的报错终止程序

SchurVINS::Prediction(double _dt, const Eigen::Vector3d& _acc, const Eigen::Vector3d& _gyr) {
    CHECK(_dt >= 0 && _dt < 0.1) << "dt: " << _dt;

• 没有特征点时导致的计算报错

svo_node: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:425: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index) [with Derived = Eigen::Matrix<int, -1, 1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = int; Eigen::Index = long int]: Assertion index >= 0 && index < size()' failed.
*** Aborted at 1739344134 (unix time) try "date -d @1739344134" if you are using GNU date ***
PC: @     0x7f9239ea900b gsignal
*** SIGABRT (@0x3e800001060) received by PID 4192 (TID 0x7f921a2f3700) from PID 4192; stack trace: ***

Eigen 库的数组越界错误，具体来说是 Eigen::DenseCoeffsBase 在访问矩阵元素时出现了索引超出范围的情况，主要是以下三个函数出现的报错，检查一下它们的数组大小，当数组大小为0时跳过运行即可。

@ 0x7f9239595c87 svo::Frame::getFeatureWrapper()
@ 0x7f9239782f79 svo::depth_filter_utils::updateSeed()
@ 0x7f923978ae4c svo::DepthFilter::updateSeedsLoop()

SchurVins发布频率

SchurVins的定位/svo/pos_imu发布频率和相机的帧率一致，我使用的相机帧率是60hz，因此需要将发布频率提高至能够让无人机使用的频率（如px4ctrl代码，需要200hz），由于这个定位发布频率和相机的帧率高度绑定，建议在外部再写一个项目订阅来/svo/pos_imu，再将其使用线性插值简单提高频率就好了。

补充发布速度信息

由于SchurVins只发布位置信息、方向四元数，而无人机的定位需要速度信息，因此需要使用另外的项目辅助速度的计算。我们对这个鲁棒性要求不高，于是随便用的一个无人机跟踪项目的速度预测功能包进行速度的计算。
Fast-tracker项目：GitHub - ZJU-FAST-Lab/Fast-tracker

功能包：bezier_predict

如果对定位要求不是很高的话可以试试，使用方法如下：
 

Bezierpredict svopredict;
static std::vector<Eigen::Matrix<double,6,1>> svo_predict_list;
        if(!svo_initialize){//速度预测的类初始化，当足够选取到_MAX_SEG=30个点后开始拟合
            svo_list.push_back(state);        
            if(svo_list.size()>=_MAX_SEG){
                svo_initialize=1;
            }            
        }
        else{
            svo_list.erase(svo_list.begin());
            svo_list.push_back(state);  

            int bezier_flag = svopredict.TrackingGeneration(5,5,svo_list);
            if(bezier_flag==0){
                svo_predict_list = svopredict.getStateListFromBezier(_PREDICT_SEG);
                g_predictedTrajectory = svo_predict_list;
                last_svo_p = svo_p;
                svo_p.x() = svo_predict_list[0](0);
                svo_p.y() = svo_predict_list[0](1);
                svo_p.z() = svo_predict_list[0](2);
                svo_v.x() = svo_predict_list[0](3);//速度
                svo_v.y() = svo_predict_list[0](4);
                svo_v.z() = svo_predict_list[0](5);
                svo_q.w() = msg->pose.pose.orientation.w;//msg是订阅的/svo/pose_imu
                svo_q.x() = msg->pose.pose.orientation.x;
                svo_q.y() = msg->pose.pose.orientation.y;
                svo_q.z() = msg->pose.pose.orientation.z;
            }      

实机效果

除了在镜头前遮挡一定时间（识别特征点过少）会导致定位炸掉外，在低算力的机载电脑上是相当友好的

在实际任务飞行中，定位漂移很少，相比于使用vins-fusion经常飘到高度几十，而且稳定性好，即使无人机撞地上，SchurVins的定位也没有飘