四元数转换为欧拉角（多解问题）

车辆行驶状态估计（4）中车辆横摆角信息在顺时针转向时存在明显的错误，进行记录输出

• 2023-05-25-aft02.txt

四元数：  -0.0020121
  0.00115721
-0.000596761
    0.999997
欧拉角：3.14039

四元数： -0.00170584
-0.000230032
   -0.499041
    0.866577
欧拉角：2.09661

四元数：-0.00445314
-0.00242893
   -0.87609
    0.48212
欧拉角：1.00616

通过在线转换网站进行转换，检查是否一致

3D Rotation Converter

发现和用 Eigen 进行四元数转欧拉角存在很大的差别

在Eigen库中，可以使用Quaternion类来表示四元数，并使用Matrix类来表示欧拉角。下面是将四元数转换为欧拉角的示例代码

#include <iostream>
#include <Eigen/Dense>

int main() {
    // 定义四元数
    Eigen::Quaterniond quaternion(0.707, 0.0, 0.707, 0.0); // 示例四元数

    // 将四元数转换为旋转矩阵
    Eigen::Matrix3d rotationMatrix = quaternion.toRotationMatrix();

    // 将旋转矩阵转换为欧拉角
    Eigen::Vector3d eulerAngles = rotationMatrix.eulerAngles(2, 1, 0); // ZYX顺序的欧拉角，可根据需求调整顺序

    // 输出欧拉角
    std::cout << "Roll: " << eulerAngles[2] << std::endl;
    std::cout << "Pitch: " << eulerAngles[1] << std::endl;
    std::cout << "Yaw: " << eulerAngles[0] << std::endl;

    return 0;
}

在上述代码中，使用Quaterniond类创建了一个四元数对象，然后使用toRotationMatrix()函数将其转换为旋转矩阵。最后，使用eulerAngles()函数将旋转矩阵转换为欧拉角，其中参数2、1、0表示按照Z轴、Y轴、X轴的顺序进行转换，即ZYX顺序的欧拉角

四元数和欧拉角互相转换

static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
		// roll (x-axis rotation)
		double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
		double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
		roll = atan2(sinr_cosp, cosr_cosp);

		// pitch (y-axis rotation)
		double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
		if (fabs(sinp) >= 1)
			pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
		else
			pitch = asin(sinp);

		// yaw (z-axis rotation)
		double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
		double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
		yaw = atan2(siny_cosp, cosy_cosp);
}

编写测试功能包，angle_transform

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <math.h>

using namespace std;
using namespace Eigen;

void toEulerAngle(const Quaterniond &q, Vector3d& eulerAngles)
{
    // roll (x-axis rotation)
    double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
    eulerAngles[2] = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
    if (fabs(sinp) >= 1)
        eulerAngles[1] = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        eulerAngles[1] = asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
    eulerAngles[0] = atan2(siny_cosp, cosy_cosp);
}

int main()
{
    double w = 0.524;
    double x = -0.0048521;
    double y = -0.00167539;
    double z = -0.851703;
    Quaterniond q(w, x, y, z);

    // 使用 Eigen    
    Matrix3d rotMat = q.toRotationMatrix();
    Vector3d eulerAngles = rotMat.eulerAngles(2, 1, 0);
    cout << "使用Eigen库" << endl;
    cout << "旋转矩阵：" << endl;
    cout << rotMat << endl;
    cout << "Roll: " << eulerAngles[2] << endl;
    cout << "Pitch: " << eulerAngles[1] << endl;
    cout << "Yaw: " << eulerAngles[0] << endl;

    // 自己的方法
    Vector3d eulerAngles_def;
    toEulerAngle(q, eulerAngles_def);
    cout << "自定义方法" << endl;
    cout << "Roll: " << eulerAngles_def[2] << endl;
    cout << "Pitch: " << eulerAngles_def[1] << endl;
    cout << "Yaw: " << eulerAngles_def[0] << endl;

    return 0;
}

修改状态估计代码

#include <iostream>
#include <string.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <realsense_state_estimate/robot_state.h>

using namespace std;
using namespace Eigen;

ros::Publisher pub;

realsense_state_estimate::robot_state imu_state;

inline double rad2deg(const double& rad)
{
    return rad * 180 / M_PI;
}

inline void toEulerAngle(const Quaterniond& q, Vector3d& eulerAngles)
{
    // roll (x-axis rotation)
    double sinr_cosp = 2.0 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
    eulerAngles[2] = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
    if (fabs(sinp) >= 1)
        eulerAngles[1] = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        eulerAngles[1] = asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
    eulerAngles[0] = atan2(siny_cosp, cosy_cosp);
}

void callback(const nav_msgs::Odometry::ConstPtr &msg)
{
    imu_state.X_pos = msg->pose.pose.position.x;
    imu_state.Y_pos = msg->pose.pose.position.y;
    imu_state.x_vel = msg->twist.twist.linear.x;
    imu_state.y_vel = msg->twist.twist.linear.y;
    Quaterniond q(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    Vector3d eulerAngles;
    toEulerAngle(q, eulerAngles);
    imu_state.yaw_angle = eulerAngles[0];
    imu_state.yaw_vel = msg->twist.twist.angular.z;
    cout << imu_state.X_pos << "," << imu_state.Y_pos << "," << imu_state.x_vel << \ 
    "," << imu_state.y_vel << "," << imu_state.yaw_angle << "," << imu_state.yaw_vel << endl;
    pub.publish(imu_state);
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "realsense_state");
    ros::NodeHandle nh;

    string subscribe_topic = "/camera/odom/sample";
    string publishe_topic = "/realsense_state_estimate";
    pub = nh.advertise<realsense_state_estimate::robot_state>(publishe_topic, 10);
    ros::Subscriber sub = nh.subscribe(subscribe_topic, 10, callback);

    ros::spin();

    return 0;
}

实车测试，原地顺时针转向

• 2023-05-25-aft03.csv

车辆位置信息

车辆横摆角信息

车辆横摆角速度信息

车辆横摆角变化符合预期，验证四元数到欧拉角转换方法的正确性

存在的问题是由于IMU并未处于车辆质心位置，因此其估计值并不能直接使用，可能需要经过坐标变换