c++ - child 绕父轴而不是他自己的轴旋转

Question

所以，我有一个模拟汽车的 C++ 项目。 我的程序在 2D 中工作(仅在 XY 平面中)，它使用 rosbag 提供的里程计数据，根据世界原点为他提供他在 XY 平面中的位置。二维的一切都很好。

但是当我处于 3D 模式时，这意味着我可以围绕多个轴旋转我的汽车，而不仅仅是 Z。 我意识到我的汽车正在围绕“世界”轴的轴旋转，而我希望它们围绕我的车辆轴转动。

为了测试它，我做了一个虚拟代码，我的车辆假设在 Z 轴上旋转 90 度，然后在 Y 轴上旋转 90 度。

我可以做些什么让我的车辆绕着他自己的轴旋转吗？ 数学会怎样？

下面的两个旋转是围绕世界轴完成的。

这是我的一段代码来说明:

void NodeImpl::callback_odometry(motion_common_msg::Odometry input_msg)
{
  //Getting Direction3D

//---
    frame_id = input_msg.header.frame_id;
    stamp = input_msg.header.stamp;

    transform_from_odometry.setOrigin(new_position_3D);

    tf::Quaternion q;
    q.setRPY(0.0, 0.0, 0.0);
    transform_from_odometry.setRotation(q);

    if(input_msg.pos_x >= 5.0)
    {
        double cos90 = 0;
        double sin90 = 1;

        tf::Matrix3x3 Rz90;
        tf::Matrix3x3 Ry90;

        Rz90.setValue(cos90, - sin90, 0, sin90, cos90, 0, 0, 0, 1);
        Ry90.setValue(cos90, 0, sin90, 0, 1, 0, -sin90, 0, cos90);

        tf::Quaternion qz90;
        tf::Quaternion qy90;

        Rz90.getRotation(qz90);
        Ry90.getRotation(qy90);

        qz90.normalize();
        qy90.normalize();

        tf::Transform tf_new_rot;

        tf_new_rot.setRotation(qz90);

        transform_from_odometry.setRotation(qy90);

        transform_from_odometry.mult (transform_from_odometry, tf_new_rot);
    }

    broadcast();
}

void NodeImpl::broadcast()
{
     static tf::TransformBroadcaster br;
     br.sendTransform(tf::StampedTransform(transform_from_odometry, stamp, frame_id, "ego_vehicle_rear_axis"));
}

Answer 1

我不确定您使用的是哪个库，因此我将尝试就此提供一些通用建议。

全局与局部旋转只是矩阵乘法顺序的问题。让 R 成为最终的旋转矩阵。当您使用以下顺序 R=X*Y*Z 乘以 X、Y 和 Z 矩阵时，这将为您提供全局旋转，而 R= Z*Y*X 将为您提供局部旋转。

上面的问题是它限制了你局部旋转的自由度到特定的 Z-Y-X 顺序。例如，如果要旋转，首先在 x 轴上，然后在 y 轴上，然后在 z 轴上轴，上面的方法可以正常工作。其他任何事情都不会给您想要的结果。您将不得不更改矩阵乘法的顺序。

如果您想要围绕某个轴旋转，比如说对象的本地 y 轴，那么您需要知道该轴的位置。您需要在每次转换后保留当前轴的引用，然后使用 Rotation matrix from axis and angle围绕您当前的本地 y 轴 旋转。

/* from the wiki link above */    
Mat3 Axis_Matrix(float angle_, const Vec3 &axis_)
    {
        return Mat3{ cos(angle_)+pow(axis_.x,2)*(1.0-cos(angle_))              , (axis_.x*axis_.y*(1.0-cos(angle_)))-(axis_.z*sin(angle_)) , (axis_.x*axis_.z*(1.0-cos(angle_)))+(axis_.y*sin(angle_)),
                     (axis_.y*axis_.x*(1.0-cos(angle_)))+(axis_.z*sin(angle_)) , cos(angle_)+pow(axis_.y,2)*(1.0 - cos(angle_))            , (axis_.y*axis_.z*(1.0-cos(angle_)))-(axis_.x*sin(angle_)),
                     (axis_.z*axis_.x*(1.0-cos(angle_)))-(axis_.y*sin(angle_)) , (axis_.z*axis_.y*(1.0-cos(angle_)))+(axis_.x*sin(angle_)) , cos(angle_)+pow(axis_.z,2)*(1.0 - cos(angle_)) };
    }

您基本上可以创建自己的结构来执行所有这些操作:

struct CoordinateSystem
{
    Vec3 current_x_axis;
    Vec3 current_y_axis;
    Vec3 current_z_axis;

    Mat3 local;

    void setRotationX(float angle_)
    {
        local *= Axis_Matrix(angle_, current_x_axis);
        update();
    }

    void setRotationY(float angle_)
    {
        local *= Axis_Matrix(angle_, current_y_axis);
        update();
    }

    void setRotationZ(float angle_)
    {
        local *= Axis_Matrix(angle_, current_z_axis);
        update();
    }

    void update()
    {
        current_x_axis = {-1.f, 0.f, 0.f} * local;
        current_y_axis = { 0.f, 1.f, 0.f} * local;
        current_z_axis = { 0.f, 0.f,-1.f} * local;
    }
};

然后你可以说:

setRotationX(45);
setRotationZ(10);
setRotationY(62);
setRotationX(34);
setRotationY(81);

并且所有旋转都将针对您的对象。