一种关于四元数旋转的方法

在做apriltags项目的过程中，发现ros中常常以四元数来表示一个位姿（包含位置和姿态）的姿态，但有时候我们会遇到想要的姿态与预定的姿态出现偏差，想通过旋转来实现姿态的变换，这时直接在四元数进行变换时，常常会遇到看不懂四元数表示的实际姿态，更不要谈去在四元数上进行操作，我们一般会想到把四元数转化成欧拉角或者旋转矩阵进行旋转操作，其实这是可行的，而且依附于ros中TF树的强大，常常我们只需要调用相关的语句就可以实现三者的变换。

但其实我们可以直接在四元数上进行旋转变换，下面我就分析一下我的思路。
首先要学习下面几个四元数的理论：
一,下面是如何把具体的四元数与旋转轴和旋转角度对应起来。
1.指出旋转轴和旋转角度，如何转化为四元素。参考网址
假定旋转轴是：RAxis = Y轴，换算成三维空间单位向量就是RAxis = [0 1 0],旋转180度（顺时针为负，逆时针为正）注意：这里的旋转是在在前一次旋转前坐标上进行旋转变换
那么转化成四元数就是（第一个180是旋转角度）
q.w=cos((180/2)pi/180) = 0
q.x=RAix.xsin((180/2)pi/180) = 00.5=0
q.y=RAix.ysin((180/2)pi/180) = 00.5=1
q.z=RAix.zsin((180/2)pi/180) = 10.5=0
例子验证：我们知道，绕y轴旋转180的欧拉角是rpy=（0,180,0）借助在线软件，我们输入后发现四元数便是wxyz=（0,0,1,0）。如下：

2.指出四元数，怎么知道旋转轴和旋转角度呢。
假定q=[0,0,1,0] (其实这个是上面的反例子而已)
q.w=cos((a/2)pi/180) = 0
q.x=RAix.xsin((a/2)pi/180) = 0
q.y=RAix.ysin((a/2)pi/180) = 1
q.z=RAix.zsin((a/2)*pi/180) = 0
从上面可以得到:RAix.x=RAix.z=0
由sin((a/2)*pi/180) =1，得到
a = 180
因此a = 180度(四元数的旋转角度一般是在0-360之间，之后就是多一圈的问题。
于是可得RAix.y = 1,因此其他q=[0,0,1,0]意味着旋转轴是RAxis =[0 1 0]，旋转角度是180度，其他的类似可以计算

二，四元数与旋转
四元数（Quaternions）为数学家Hamilton于1843年所创造的，您可能学过的是复数，例如：a + b i 这样的数，其中i * i = -1，Hamilton创造了三维的复数，其形式为 w + x i + y j + z k，其中i、j、k的关系如下

i * j = k = -j * i （叉乘）
j * k = i = -k * j
k * i = j = -i * k

关于四元数的其它理论，参照这篇网址
我们要明确后面两个四元数相乘q" = Rot(q) * q’的矩阵表示法如下所示：
q = [w, x, y, z]

综合上面两个理论我们便可以在原有四元数表示姿态的基础上，进行旋转变换。
比如我们想在原有四元数基础上，绕y轴循转180度，则四元数为wxyz=（0,0,1,0）这时我们便可以左乘一个矩阵

---

工程背景及应用：
做apriltags项目时，需要机械臂末端对准tag，但实际tf树出来的位姿是机械臂末端与tag的位姿相同，所以需要绕y轴旋转180度，然后实现机械臂末端对准tag的效果。

理论实现：

也就是

程序实现部分：

#include 
#include 
#include 

#include 

class ScanNPlan
{
public:
  ScanNPlan(ros::NodeHandle& nh)
  {
    vision_client_ = nh.serviceClient("localize_part");
  }

  void start(const std::string& base_frame)
  {
    ROS_INFO("Attempting to localize part");
    // Localize the part
    apriltags2_node::LocalizePart srv;

    srv.request.base_frame = base_frame;
    ROS_INFO_STREAM("Requesting pose in base frame: " << base_frame);
    //如果服务回调不成功
    if(!vision_client_.call(srv))
    {
      ROS_ERROR("Could not localize part");
      return ;
    }

    ROS_INFO_STREAM("part localized: " << srv.response);
   //四元数旋转变换
    move_target = srv.response.pose;
    move_target1 = srv.response.pose;
    move_target.orientation.x=move_target1.orientation.z;
    move_target.orientation.y=move_target1.orientation.w;
    move_target.orientation.z=-1*move_target1.orientation.x;
    move_target.orientation.w=-1*move_target1.orientation.y;
 
    //ROS_INFO_STREAM("part localized: " << move_target.orientation);
    //创建一个对象，move_group（）构造函数定义计划组名称
    moveit::planning_interface::MoveGroupInterface move_group("manipulator");

    moveit::planning_interface::MoveItErrorCode success;
    moveit::planning_interface::MoveGroupInterface::Plan plan;

     //设置初始位置
    move_group.setStartState(*move_group.getCurrentState());
    
    move_group.setPoseReferenceFrame(base_frame);
    move_group.setPoseTarget(move_target); 
    move_group.setPlanningTime(10);
    //进行运动规划
    success = move_group.plan(plan);   //运动规划输出
    ROS_INFO("Visualizing plan (stateCatch pose) %s",success == moveit_msgs::MoveItErrorCodes::SUCCESS ? "SUCCESS" : "FAILED");
    if (success  == moveit_msgs::MoveItErrorCodes::SUCCESS)  
    move_group.execute(plan);
    move_group.setStartState(*move_group.getCurrentState());

  }
  
private:
  // Planning components
  ros::ServiceClient vision_client_;
  geometry_msgs::Pose move_target;
  geometry_msgs::Pose move_target1;
  std::string end_effector_link;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "apriltags2_node");
  ros::NodeHandle nh;
  ros::NodeHandle private_node_handle ("~");
  ros::AsyncSpinner async_spinner(1);
  async_spinner.start();

  ros::Rate r(10); // 10 hz,它的功能就是先设定一个频率，然后通过睡眠度过一个循环中剩下的时间，来达到该设定频率。

  while(ros::ok())
  {
    ROS_INFO("ScanNPlan node has been initialized");
    std::string base_frame;
    private_node_handle.param("base_frame", base_frame, "world"); // world parameter name, string object reference, default value

    ScanNPlan app(nh);
    ros::Duration(.5).sleep();  // wait for the class to initialize
    app.start(base_frame);
    //ros::waitForShutdown();
  }
  return 0;
}

其它参考文献：https://wenku.baidu.com/view/8bba76d371fe910ef02df821.html