[数据结构与算法]捷联惯导算法与组合导航原理学习——等效旋转矢量和姿态阵转换（一）

等效旋转矢量和姿态阵转换

学习资料参考：

[1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8.

EquRotationVec.h

#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>

Eigen::Matrix3d VecToSkewSymmeticMat(const Eigen::Vector3d &vec);
Eigen::Vector3d SkewSymmeticMatToVec(const Eigen::Matrix3d &mat);

// 一些实验性的代码，Eigen库中有更好的实现
class EquivalentRotationVector
{
public:
    double          mAngle;
    Eigen::Vector3d mVec;

    Eigen::Matrix3d toDCM() const;

    void fromDCM(const Eigen::Matrix3d &dcm);
};

EquRotationVec.cpp

// [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8.

#include "EquRotationVec.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

using namespace Eigen;
using namespace std;

const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;

// [1] 等效旋转矢量 -> 方向余弦阵, 参考公式: 2.2.22
// NOTE! 式中 u 为单位矢量
Eigen::Matrix3d EquivalentRotationVector::toDCM() const
{
    Matrix3d u_mat = VecToSkewSymmeticMat(mVec) / mVec.norm();

    Matrix3d dcm;
    dcm = Matrix3d::Identity() +
          sin(mAngle) * u_mat +
          (1 - cos(mAngle)) * u_mat * u_mat;
    return dcm;
}

// [1] 方向余弦阵 -> 等效旋转矢量，参考公式: 2.2.28
// NOTE! 式中结果 phi 不是单位矢量，这里算出来的是单位矢量
void EquivalentRotationVector::fromDCM(const Eigen::Matrix3d &dcm)
{
    mAngle            = acos((dcm.trace() - 1) / 2.0);
    Matrix3d skew_mat = 1.0 / (2 * sin(mAngle)) * (dcm - dcm.transpose());
    mVec              = SkewSymmeticMatToVec(skew_mat);
}

Eigen::Matrix3d VecToSkewSymmeticMat(const Eigen::Vector3d &vec)
{
    Matrix3d skew_mat;
    // clang-format off
    skew_mat << 0,		-vec[2], vec[1],
				vec[2],  0,		-vec[0],
			   -vec[1],  vec[0], 0;
    // clang-format on
    return skew_mat;
}

Eigen::Vector3d SkewSymmeticMatToVec(const Eigen::Matrix3d &mat)
{
    return Eigen::Vector3d(mat(1, 1), mat(0, 2), mat(1, 0));
}

main.cpp

#include "EquRotationVec.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

using namespace Eigen;
using namespace std;

    int main(int argc, char*argv[])
    {
        cout << "----- 自行实现的算法和 Eigen 中的算法比较 -----" << endl;

        EquRotationVec vec1;
        vec1.mAngle = 30 * DEG_TO_ARC;
        vec1.mVec   = {2, 0, 0};

        auto dcm_mat0 = vec1.toDCM();

        cout << "旋转角度（度）：" << vec1.mAngle * ARC_TO_DEG << "\n转轴方向（单位化）：\n"
             << vec1.mVec / vec1.mVec.norm() << endl;

        EquRotationVec vec2;
        vec2.fromDCM(dcm_mat0);

        cout << "dcm_mat0: \n"
             << dcm_mat0 << endl
             << "angle: " << vec2.mAngle << endl
             << "axis: \n"
             << vec2.mVec << endl;

        AngleAxisd angleAxis(vec1.mAngle, vec1.mVec / vec1.mVec.norm());   // 注意在使用AngleAxisd 时转轴必须单位化
        auto       dcm_mat1 = angleAxis.toRotationMatrix();
        AngleAxisd angleAxis1(dcm_mat1);
        cout << "dcm_mat1: \n"
             << dcm_mat1 << endl
             << "angle: " << angleAxis1.angle() << endl
             << "axis: \n"
             << angleAxis1.axis() << endl;
        return 0;
    }