动力学姿态变换

前言

        采取QT与Eigen矩阵库实现反对称矩阵、有限转动张量计算以及常见姿态表示间转换：方向余弦阵、有限转动四元数、欧拉四元数、欧拉角、卡尔丹角。

        各种姿态转变间都通过方向余弦阵作为中介。

        方向余弦阵---四元数：利用到有限转动张量

        方向余弦阵--欧拉角、卡尔丹角：利用三次旋转矩阵传递

---

        反算三角函数值存在多值问题；反算除余弦阵外姿态表示存在奇异问题。

        三角函数值多值问题采取 sin、cos值正负找到角度所在象限，利用自定义DATN2方法实现。

 

代码如下：

getAttitudeCoods() 获得全部姿态结果的结构体
showAttitudeCoods() 控制台显示
isMeetConstraint() 检验结果有效性，余弦阵正交性，四元数归一性

---

#ifndef ATTITUDETRANS_H
#define ATTITUDETRANS_H

#include <Geometry>
#include <Dense>
#include <Eigenvalues>
#include <QDebug>
#include <iostream>
#include <QMatrix4x4>
using namespace Eigen;
using namespace std;

#define PI 3.1415926

struct AttitudeCoors{
    Matrix3d DirectCosMat; //方向余弦矩阵
    Vector4d FiniteRotQuaternions1; //有限转动四元数
    Vector4d FiniteRotQuaternions2; //有限转动四元数
    Vector4d EulerQuaternion; //欧拉四元数
    Vector3d EulerAngle; //欧拉角
    Vector3d CarlDanAngle;//卡尔丹角
};

class AttitudeTrans
{
public:
    //传入 当前模型矩阵  及其 基准的模型矩阵
    AttitudeTrans(QMatrix4x4 model, QMatrix4x4 modelStand);

    Matrix3d SkewSymMatrix(VectorXd P);//反对称矩阵
    Matrix3d tensor_FiniteRotation(VectorXd P, double Rot_theta); //有限转动张量
    double DATN2(double s, double c); //由sin cos 计算出θ

    AttitudeCoors getAttitudeCoods(Matrix3d DirectCosMat);
    void showAttitudeCoods(AttitudeCoors coods);

    void isMeetConstraint(AttitudeCoors coods);//检验约束完备性

    //其他表示 转为 方向余弦 阵
    Matrix3d FiniteRotQuaternionsTo(Vector4d fRotQ);
    Matrix3d EulerQuaternionTo(Vector4d eQ);
    Matrix3d EulerAngleTo(Vector3d eA);
    Matrix3d CarlDanAngleTo(Vector3d cDA);

    //方向余弦阵 转其他的表示
    Vector4d toFiniteRotQuaternions1(Matrix3d directCosMat); //有限转动四元数 算法一
    Vector4d toFiniteRotQuaternions2(Matrix3d directCosMat); //有限转动四元数 算法二
    Vector4d toEulerQuaternion(Matrix3d directCosMat); //欧拉四元数
    Vector3d toEulerAngle(Matrix3d directCosMat); //欧拉角
    Vector3d toCarlDanAngle(Matrix3d directCosMat); //卡尔丹角

public:
    AttitudeCoors Coors;

};

#endif // COORTRANS_H

        

#include "AttitudeTrans.h"

AttitudeTrans::AttitudeTrans(QMatrix4x4 model, QMatrix4x4 modelStand)
{
    Matrix3d DirectCosMat;
    for (int i = 0; i<3; i++) {
        for (int j = 0;j<3;j++) {
            DirectCosMat(i,j) = model(i,j);
        }
    }

    Matrix3d DirectCosMatStand;
    for (int i = 0; i<3; i++) {
        for (int j = 0;j<3;j++) {
            DirectCosMatStand(i,j) = modelStand(i,j);
        }
    }
    std::cout<<DirectCosMat<<std::endl;
    DirectCosMat = DirectCosMat * DirectCosMatStand.inverse();
    std::cout<<DirectCosMat<<std::endl;

    Coors = getAttitudeCoods(DirectCosMat);
    isMeetConstraint(Coors);
    //showAttitudeCoods(Coors);
}

Matrix3d AttitudeTrans::SkewSymMatrix(VectorXd P)
{
    //Skew-symmetric Matrix 反对称矩
    int pSize = P.size();
    Matrix3d SsMat(pSize,pSize);

    SsMat(0,0) = 0;
    SsMat(0,1) = -P(2);
    SsMat(0,2) = P(1);
    SsMat(1,0) = P(2);
    SsMat(1,1) = 0;
    SsMat(1,2) = -P(0);
    SsMat(2,0) = -P(1);
    SsMat(2