Question

因此，我有一个模拟汽车的C ++项目。我的程序在2D中工作（仅在XY平面中），它使用rosbag给出的经纬度数据输入，根据世界原点，他在XY平面中的位置。 2D一切都很好。

但是当我处于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将为您提供局部旋转< / strong>。

上述问题是将您的局部旋转自由度限制为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);

所有旋转都将在对象的局部发生。

孩子绕父轴旋转，而不是自己旋转

1 个答案: