基于扩展卡尔曼滤波的多传感器融合定位

多传感器融合是一个复杂的问题，本文根据ROS中的robot localization功能包，说明多传感器融合定位的一种实现方式。

卡尔曼滤波

卡尔曼滤波并不复杂，分为预测和更新两个部分：

预测：

−

\hat{X}_{k}^{1} =A\hat{X}_{k-1}+Bu_{k}\\ P_{k}^{1}=AP_{k-1}A^{T}+Q

$\hat{X}_{k}^{1} = A \hat{X}_{k - 1} + B u_{k} P_{k}^{1} = A P_{k - 1} A^{T} + Q$

更新：

(

−

)

(

−

)

K_{k}=\frac{P_{k}^{1}H^{T}}{HP_{k}^{1}H^{T}+R}\\ \hat{X}_{k}=\hat{X}_{k}^{1}+K_{k}(Z_{k}-H \hat{X}_{k}^{1})\\ P_{k}=(I-K_{k}H)P_{k}^{1}

$K_{k} = \frac{P _{k}^{1} H ^{T}}{H P _{k}^{1} H ^{T} + R} \hat{X}_{k} = \hat{X}_{k}^{1} + K_{k} (Z_{k} - H \hat{X}_{k}^{1}) P_{k} = (I - K_{k} H) P_{k}^{1}$

在实际使用卡尔曼滤波时，第一是确定状态变量和控制变量，一般在机器人定位中，8个状态变量

]

[x,y,\theta,v_{x},v_{y},\omega,a_{x},a_{y}]

$[x, y, θ, v_{x}, v_{y}, ω, a_{x}, a_{y}]$

都可以看成是状态变量，也可以把加速度看成是控制变量，得到加速度的方法可以通过上一篇文章讲的动力学控制中得到，也可以通过速度做差分。如果是都看成状态变量，那么加速度的更新是通过更新步的第二个式子进行更新，预测步的第一个式子是无法更新加速度的，只会保留加速度。

第二个问题是，状态变量的选取，一般来说定位是机器人在地图上的定位，使用的是世界坐标系，即

]

[x,y,\theta]

$[x, y, θ]$

是世界坐标系下的变量，而速度、角速度、加速度等都是通过传感器计算得到的，是机器人坐标系下的变量。从机器人坐标系转到世界坐标系需要通过旋转变换，即会引入三角函数，这样状态方程就是非线性的了，因此需要使用扩展卡尔曼滤波，这时公式需要做修改，

A

$A$

矩阵为雅可比矩阵

(

−

)

(

)

∂

∣

∂

−

∣

−

x_{k}^{1}=f(\hat{x}_{k-1},u_{k})\\ z=h(x_{k})\\ H=\frac{\partial h}{\partial x_{k}}|_{\hat{x}_{k}^{1} },A=\frac{\partial f}{\partial x_{k-1}}|_{\hat{x}_{k-1}^{1} }

$x_{k}^{1} = f (\overset{x}{^}_{k - 1}, u_{k}) z = h (x_{k}) H = \frac{\partial h}{\partial x _{k}} ∣_{\overset{x}{^}_{k}^{1}}, A = \frac{\partial f}{\partial x _{k - 1}} ∣_{\overset{x}{^}_{k - 1}^{1}}$

例如：

在这里插入图片描述

第三个问题是

P_{0},Q,R

$P_{0}, Q, R$

矩阵应该如何选取。首先是

R

$R$

矩阵，测量噪声协方差矩阵，一般是传感器的测量噪声，常用的传感器有IMU，激光雷达，摄像头，编码器等，网上都有常用的噪声协方差矩阵，这个不难找。然后是

P_{0}

$P_{0}$

和

Q

$Q$

，初始的先验误差协方差矩阵和过程噪声矩阵，这个可以参考一些论文或开源的ekf包，里边有。本文参考的robot localization中，这两个矩阵为：

在这里插入图片描述

这里的矩阵是15维的：

]

[x,y,z,roll, pitch, yaw, v_{x}, v_{y}, v_{z}, \theta_{roll}, \theta_{pitch}, \theta_{yaw}, a_{x}, a_{y}, a_{z}]

$[x, y, z, r o l l, p i t c h, y a w, v_{x}, v_{y}, v_{z}, θ_{r o l l}, θ_{p i t c h}, θ_{y a w}, a_{x}, a_{y}, a_{z}]$

多传感器融合

多传感器融合是一个复杂的方向，具体可以分为紧耦合和松耦合两种融合方式。紧耦合是将各传感器的数据汇总，然后计算出一个位姿，松耦合是各传感器的数据分别计算出一个位姿，然后再进行汇总得到最终位姿。这么说好像有点不太直观，总之紧耦合的特点是各传感器的数据需要同步，而松耦合的传感器是异步的，不需要同步。robot localization功能包中的EKF是松耦合的融合方式。

多传感器融合的第一个问题是数据处理，由于robot localization包是松耦合的方式，因此对每一个传感器数据单独进行处理。ROS中的协方差矩阵分为三类，位姿、速度和加速度，因此所有传感器的原始数据也会拆分成这三类。举例平面机器人如下：

编码器里程计可以计算位置和速度，使用

]

[\theta,v_{x},v_{y}]

$[θ, v_{x}, v_{y}]$

数据进行融合，首先先对一个传感器的数据进行分类，

\theta

$θ$

是一组，

v_{x},v_{y}

$v_{x}, v_{y}$

是一组，前者是pose类型的数据，后者是twist类型的数据，处理pose时，将角度值和其协方差放入一个结构体中，作为其中一组测量数据；处理twist时，将两个速度和对应的协方差矩阵放入另一个结构体中，作为另一组测量数据。

数据处理完后进行卡尔曼滤波，预测步没啥好说的，在更新步中，由于第一组测量数据只有角度值，因此进行更新时只更新角度值，即H矩阵只在角度对应的行，斜对角线上的值为1，其余为0。更新完角度后，协方差矩阵P更新，然后不进行预测了，因为都是同一个时间戳的数据，继续更新速度，H矩阵的值改一下，进行第二次更新。

robot localization功能包运行原理

该功能包分为主函数和传感器数据接收回调函数两个线程。

主函数部分：首先是ekf_localization_node.cpp下的main函数：看代码没啥好说的吧，然后进入这个ekf.run()函数。

int main(int argc, char **argv)
{
ros::init(argc, argv, "ekf_navigation_node"); //创建节点

RobotLocalization::RosEkf ekf;//类

ekf.run();//进入这个函数

return EXIT_SUCCESS;
}

ekf.run()这个函数在ros_filter.cpp下1742行（行数可能不一样，因为我的robot localization包原代码被我修改过，但大概就是上下几行）。这个函数中第一个重要的部分就是，这个是读取参数，具体读了什么，可以进去看看，主要是你ekf_params.yaml这里设置的内容。注意这里加载了参数后，回调函数就开始起作用了，具体的内容放后边说。

loadParams();

然后下边就是发布者，tf变换啥的，tf变换是把测量数据从一个坐标系变换到另一个坐标系，还有打印一些东西，主要是让你知道的程序为什么报错的，我们直接忽略，接下来是时间。时间的问题在该包中十分重要，这里是获取当前时间戳，ROS的传感器消息都自带时间戳，储存在header中。

ros::Time curTime;
ros::Time lastDiagTime = ros::Time::now();

以上是初始化的一部分内容，然后就进入死循环进行不断的更新和预测了。接下来先说说回调函数的内容。

回调函数，说的是接收到传感器数据后进入回调处理的过程。具体的处理方式函数在ros_filter.h 105行的类中有详细的说明，这里列一部分比较有用的。

template<class T> class RosFilter
{
    /*控制量回调：这部分就是前文讲的，将加速度看成是控制量还是状态变量，
    如果是看成控制量，那就会根据速度差分的方法计算加速度，同时也会加载
    最大最小加速度限制、加加速度限制的相关参数，该包默认是不使用控制量。*/
    void controlCallback(const geometry_msgs::Twist::ConstPtr &msg);
    void controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);

    /*三种类型数据的回调处理，这里和前文说的差不多，但是除了测量值和协
    方差矩阵外，还加上了很多东西，比如时间戳等等*/
    void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg,
                    const CallbackData &callbackData,
                    const std::string &targetFrame);
    void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
                    const CallbackData &callbackData,
                    const std::string &targetFrame,
                    const bool imuData);
    void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
                    const CallbackData &callbackData,
                    const std::string &targetFrame);
    
    /*IMU数据和里程计数据回调函数*/
    void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, 
                    const std::string &topicName,
                    const CallbackData &poseCallbackData, const CallbackData &twistCallbackData,
                    const CallbackData &accelCallbackData);
    void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName,
                    const CallbackData &poseCallbackData, const CallbackData &twistCallbackData);
}

进入odometryCallback()，

/*根据时间戳检查是否是过去的测量数据，每次进行预测更新后，lastSetPoseTime_
时间戳都会变成使用的测量值的时间戳*/
if (msg->header.stamp <= lastSetPoseTime_)
{
    ···
}

/*数据分类处理了，先处理pose，再处理twist*/
if (poseCallbackData.updateSum_ > 0)
{
    // Grab the pose portion of the message and pass it to the poseCallback
    geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();
    posPtr->header = msg->header;
    posPtr->pose = msg->pose;  // Entire pose object, also copies covariance

    geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
    poseCallback(pptr, poseCallbackData, worldFrameId_, false);
}

if (twistCallbackData.updateSum_ > 0)
{
    // Grab the twist portion of the message and pass it to the twistCallback
    geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
    twistPtr->header = msg->header;
    twistPtr->header.frame_id = msg->child_frame_id;
    twistPtr->twist = msg->twist;  // Entire twist object, also copies covariance

    geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
    twistCallback(tptr, twistCallbackData, baseLinkFrameId_);
}

进入poseCallback()，

if (msg->header.stamp >= lastMessageTimes_[topicName])
    {
      // 创建向量和矩阵，向量用来存放测量值，矩阵存放协方差
      Eigen::VectorXd measurement(STATE_SIZE);
      Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
      measurement.setZero();
      measurementCovariance.setZero();

    ···

    //这个if就是进行处理了，具体处理些个啥，有兴趣可以点进去看，
      if (preparePose(msg,
                      topicName,
                      targetFrame,
                      callbackData.differential_,
                      callbackData.relative_,
                      imuData,
                      updateVectorCorrected,
                      measurement,
                      measurementCovariance))
      {
    /*这个函数就是把处理好的数据，measurement这个放入测量数据队列中
    这样每一个传感器数据就被分解成pose、twist、acc等三类数据存到队列中*/
        enqueueMeasurement(topicName,
                           measurement,
                           measurementCovariance,
                           updateVectorCorrected,
                           callbackData.rejectionThreshold_,
                           msg->header.stamp);

      }
      ···
  }

这样我们的传感器数据经过这样的回调处理后，最后得到一个队列：[imu_pose,imu_twist,odom1_pose,odom1_twist,odom2_pose…]

说回主函数，进入到死循环后，主要有以下几个函数起作用，

 while (ros::ok())
{
    // 获取当前时间戳
    curTime = ros::Time::now();

    // 将队列里的传感器数据取出，ekf就运行在这个函数中
    integrateMeasurements(curTime);

    // Get latest state and publish it
    nav_msgs::Odometry filteredPosition;
    ···
    //后面就是一些尾巴的处理，什么坐标系变换啊，发布话题啊，数据重新跟新啊啥的。
}

进入integrateMeasurements(curTime);

while (!measurementQueue_.empty() && ros::ok())
    {
        //检查队列是否为空，检查队头的数据时间是不是过去的，
        MeasurementPtr measurement = measurementQueue_.top();
        if (measurement->time_ > currentTime.toSec())
        {
          break;
        }
        measurementQueue_.pop();
        if (useControl_ && restoredMeasurementCount > 0)
        {
          filter_.setControl(measurement->latestControl_, measurement->latestControlTime_);
          restoredMeasurementCount--;
        }

        // EKF的预测和更新步
        filter_.processMeasurement(*(measurement.get()));

        ···
    }

进入filter_.processMeasurement();

if (initialized_)
    {
     // 时间差🔺t
      delta = measurement.time_ - lastMeasurementTime_;

      if (delta > 0)
      {
        validateDelta(delta);

        // 预测步
        predict(measurement.time_, delta);

        // Return this to the user
        predictedState_ = state_;
      }

      // 更新步
      correct(measurement);
    }

predict(measurement.time_, delta);在ekf.cpp中

···//前边都是雅可比矩阵的计算
// (1) Apply control terms, which are actually accelerations，controlAcceleration_都是0，因为无控制量
    state_(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta;
    state_(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta;
    state_(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta;

//这个加速度也不变，和上一次状态变量的值一样，第(1)步几乎没用
    state_(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ?
      controlAcceleration_(ControlMemberVx) : state_(StateMemberAx));
    state_(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ?
      controlAcceleration_(ControlMemberVy) : state_(StateMemberAy));
    state_(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ?
      controlAcceleration_(ControlMemberVz) : state_(StateMemberAz));

// (2) Project the state forward: x = Ax + Bu (really, x = f(x, u))，预测步第一个式子
    state_ = transferFunction_ * state_;
    // Handle wrapping
    wrapStateAngles();
// (3) Project the error forward: P = J * P * J' + Q，第二个式子
    estimateErrorCovariance_ = (transferFunctionJacobian_ *
                                estimateErrorCovariance_ *
                                transferFunctionJacobian_.transpose());
    estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);
  }

correct();也在ekf.cpp中

···
// 各个矩阵
Eigen::VectorXd stateSubset(updateSize);                              // x (in most literature)
Eigen::VectorXd measurementSubset(updateSize);                        // z
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize);  // R
Eigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows());  // H
Eigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize);          // K
Eigen::VectorXd innovationSubset(updateSize);                         // z - Hx

stateSubset.setZero();
measurementSubset.setZero();
measurementCovarianceSubset.setZero();
stateToMeasurementSubset.setZero();
kalmanGainSubset.setZero();
innovationSubset.setZero();

for (size_t i = 0; i < updateSize; ++i)
{
    // 这里就是改变测量矩阵H的值了，只在测量数据部分有值。
    measurementSubset(i) = measurement.measurement_(updateIndices[i]);
    ···
}
···
//后面更新就是套公式了

放两张更新时的图，分别是odometry的pose和twist数据进行更新的，state就是目前的状态，11是

\dot{\theta}_{z}

$\dot{θ}_{z}$

数据所在的位置，其它部分是0，是因为这是平面机器人。6是

v_{x}

$v_{x}$

测量数据所在的位置，机器人不是全向的，所有7处

v_{y}

$v_{y}$

总为0。

在这里插入图片描述

在这里插入图片描述

原文链接：https://blog.csdn.net/qq_45443570/article/details/125372008

基于扩展卡尔曼滤波的多传感器融合定位

卡尔曼滤波

多传感器融合

robot localization功能包运行原理

你可能也喜欢