miniAHRS传感器的四元数单片机采集与欧拉角转换_周永录(1)
6轴 欧拉角 解算 开源 -回复
6轴欧拉角解算开源-回复欧拉角是描述物体在三维空间中旋转的一种方法，是三个方向角的组合。
在机器人学、航天航空等领域，欧拉角的解算是十分重要的技术。
本文将介绍一个开源的六轴欧拉角解算方法，给出解算步骤，并详细解释每一步骤的原理和实现过程。
本文所介绍的六轴欧拉角解算方法是基于四元数的。
四元数是数学上一种特殊的超复数，通常用来描述旋转变换。
在机器人学和姿态控制领域，四元数广泛应用于旋转表示。
步骤一：导入所需的开源库和工具首先，我们需要导入一些常用的数学库和工具，如numpy、scipy等。
这些库和工具可以方便地进行矩阵计算、数值优化等操作。
步骤二：读取传感器数据现代机器人通常配备有惯性测量单元（IMU）等传感器，可以测量物体的角速度和线加速度。
我们需要读取这些传感器的数据，并进行预处理。
步骤三：数据预处理在进行欧拉角解算之前，需要对传感器数据进行预处理，以消除误差和噪声的影响。
通常可以采用滑动窗口滤波器等方法来平滑数据。
步骤四：计算旋转矩阵欧拉角是基于旋转矩阵的表示。
为了解算欧拉角，我们需要先计算旋转矩阵。
通常，可以通过运动学模型或者传感器数据直接计算旋转矩阵。
步骤五：计算旋转矩阵的欧拉角得到旋转矩阵后，我们可以通过数值优化方法，如Levenberg-Marquardt 算法，来解算其对应的欧拉角。
这个优化过程可以通过最小化欧拉角和旋转矩阵之间的误差来实现。
步骤六：验证解算结果在解算完成后，需要对结果进行验证。
可以将解算得到的欧拉角应用到物体上，观察其在三维空间中的旋转效果是否符合预期。
通过以上的步骤，我们可以得到一个开源的六轴欧拉角解算方法。
这个方法可以广泛应用于机器人姿态控制、航天航空等领域。
并且，由于其基于四元数的形式，具有较强的数学理论和计算性能。
总结：本文介绍了一个开源的六轴欧拉角解算方法，通过导入所需的开源库和工具，读取传感器数据，进行数据预处理，计算旋转矩阵，解算旋转矩阵的欧拉角，并验证解算结果，我们可以得到一个完整的欧拉角解算流程。
机器人姿态插补的四元数直接逆解方法
机器人姿态插补的四元数直接逆解方法
成津赛;张秋菊
【期刊名称】《机械科学与技术》
【年(卷),期】2016(035)009
【摘要】针对欧拉角姿态插补方法中存在奇异性以及姿态过渡不平稳的问题,采用四元数表示机器人姿态,对起点和终点采用四元数球面线性插值的方法来进行机器人姿态插补;并给出了四元数直接逆解求姿态角的方法,舍去了姿态插补点须转化为位姿变换矩阵的过程,提高了机器人控制系统软件的运行效率.以FANUC LR Mate 100i机器人为实例进行求解,对比位姿变换矩阵和四元数各自所求的逆解,可得两种方法算出的结果一致,验证了算法的可行性.
【总页数】5页(P1354-1358)
【作者】成津赛;张秋菊
【作者单位】江南大学机械工程学院,江苏无锡214122;江苏省食品先进制造装备技术重点实验室,江苏无锡214122;江南大学机械工程学院,江苏无锡214122;江苏省食品先进制造装备技术重点实验室,江苏无锡214122
【正文语种】中文
【中图分类】TP242.2
【相关文献】
1.倍四元数在6R串联机器人逆解中的应用 [J], 葛小川;郑飂默;吴纯赟;郑国利
2.基于四元数的6R串联机器人运动学逆解应用 [J], 陈爱;冯桑;何春;陆晓
3.基于单位四元数的机器人多姿态可控的插补算法 [J], 徐超;徐翔鸣
4.基于四元数向量的机器人运动学逆解研究 [J], 屈健康;徐娅萍;亢海龙;王涛;王雄
5.基于单位四元数机器人姿态插补算法 [J], 孔民秀;季晨
因版权原因，仅展示原文概要，查看原文内容请购买。
MPU6050 四元数 欧拉角 程序
MPU6050 四元数欧拉角程序#include \#include \ #include \ #include \#include \#include \#include \#include \#include\#include\static signed char gyro_orientation[9] = {-1, 0, 0,0,-1, 0, 0, 0, 1}; #define q30 1073741824.0ffloat q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; char num[50];float Pitch,Roll,Yaw;unsigned long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4];int main(void) {// u16 i;int result; Stm32_Clock_Init(9);//系统时钟设置 delay_init(72); //延时初始化 uart_init(72,9600); //串口1初始化 LCD_Init(); POINT_COLOR=RED; i2cInit();// result = mpu_init();// if(!result) { // PrintChar(\ //mpu_set_sensor //if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); // PrintChar(\// else // PrintChar(\ if(!mpu_configure_fifo(INV_XYZ_GYRO |INV_XYZ_ACCEL)) //mpu_configure_fifo PrintChar(\ else PrintChar(\ if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_ratePrintChar(\ else PrintChar(\ if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare PrintChar(\ else PrintChar(\if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation PrintChar(\ else PrintChar(\if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT |DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature PrintChar(\ else PrintChar(\ if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate PrintChar(\ else PrintChar(\ run_self_test();if(!mpu_set_dmp_state(1)) PrintChar(\ else PrintChar(\ } while(1){dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);/* Gyro and accel data are written to the FIFO by the DMP in chip * frame and hardware units. This behavior is convenient because it * keeps thegyro and accel outputs of dmp_read_fifo and * mpu_read_fifo consistent. *//* if (sensors & INV_XYZ_GYRO )send_packet(PACKET_TYPE_GYRO, gyro); if (sensors & INV_XYZ_ACCEL)send_packet(PACKET_TYPE_ACCEL, accel); *//* Unlike gyro and accel, quaternions are written to the FIFO in *the body frame, q30. The orientation is set by the scalar passed * todmp_set_orientation during initialization. */if (sensors & INV_WXYZ_QUAT ) { q0=quat[0] / q30; q1=quat[1] /q30; q2=quat[2] / q30; q3=quat[3] / q30; Pitch = asin(2 * q1 * q3 -2 * q0* q2)* 57.3; // pitch Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1* q1 - 2 * q2* q2 + 1)* 57.3; // roll Yaw = atan2(2*(q1*q2 +q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; // sprintf(num,\in Calculating quaternion steps..... // sprintf(num,\ sprintf(num,\ PrintChar(num); PrintChar(\ // LCD_ShowString(30,190,200,16,32,num); //LCD_Show2Num(30,150,Pitch,16,8,0); // LCD_Show2Num(30,170,Roll,16,8,0); // LCD_Show2Num(30,190,Yaw,16,8,0); // UART1_ReportIMU(Yaw*10, Pitch*10,Roll*10,0,0,0,100); } LCD_ShowNum(30,80, result,2,16) ;POINT_COLOR=RED; LCD_ShowString(30,50,\// delay_ms(500); //LCD_ShowNum(30,70,sca610(0,5),6,16); // Hanzi(50,110,BLUE,0,4,1); }} /*$License:Copyright (C) 2021-2021 InvenSense Corporation, All Rights Reserved.See included License.txt for License information. $ */ /*** @addtogroup DRIVERS Sensor Driver Layer* @brief Hardware drivers to communicate with sensors via I2C. ** @{* @file inv_mpu_dmp_motion_driver.c * @brief DMP image and interface functions.* @details All functions are preceded by the dmp_ prefix to* differentiate among MPL and general driver function calls. *///#include \#include #include #include #include #include #include \#include \#include \#include \#include \#include \#define MOTION_DRIVER_TARGET_MSP430/* The following functions must be defined for this platform: *i2c_write(unsigned char slave_addr, unsigned char reg_addr, * unsigned char length, unsigned char const *data) * i2c_read(unsigned char slave_addr, unsigned char reg_addr, * unsigned char length, unsigned char *data) * delay_ms(unsigned long num_ms) * get_ms(unsigned long *count) */#if defined MOTION_DRIVER_TARGET_MSP430 //#include \//#include \#define delay_ms delay_ms #define get_ms get_ms#define log_i(...) do {} while (0) #define log_e(...) do {} while (0)#elif defined EMPL_TARGET_MSP430 #include \#include \#include \#define delay_ms msp430_delay_ms #define get_msmsp430_get_clock_ms #define log_i MPL_LOGI #define log_e MPL_LOGE#elif defined EMPL_TARGET_UC3L0/* Instead of using the standard TWI driver from the ASF library, we're using * a TWI driver that follows the slave address + register address convention. */#include \#include \#include \#include \/* delay_ms is a function already defined in ASF. */ #define get_msuc3l0_get_clock_ms #define log_i MPL_LOGI #define log_e MPL_LOGE#else//#error Gyro driver is missing the system layer implementations. #endif/* These defines are copied from dmpDefaultMPU6050.c in the general MPL * releases. These defines may change for each DMP image, so be sure to modify * these values when switching to a new image. */#define CFG_LP_QUAT (2712) #define END_ORIENT_TEMP (1866) #define CFG_27 (2742) #define CFG_20 (2224) #define CFG_23 (2745) #define CFG_FIFO_ON_EVENT (2690)感谢您的阅读，祝您生活愉快。
eigen中四元数、欧拉角、旋转矩阵、旋转向量
eigen中四元数、欧拉⾓、旋转矩阵、旋转向量⼀、旋转向量1.0初始化旋转向量：旋转⾓为alpha，旋转轴为(x,y,z)Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))1.1旋转向量转旋转矩阵Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.matrix();Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.toRotationMatrix();1.2旋转向量转欧拉⾓(Z-Y-X，即RPY)Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);1.3旋转向量转四元数Eigen::Quaterniond quaternion(rotation_vector);Eigen::Quaterniond quaternion;Quaterniond quaternion;Eigen::Quaterniond quaternion;quaternion=rotation_vector;⼆、旋转矩阵2.0初始化旋转矩阵Eigen::Matrix3d rotation_matrix;rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;2.1旋转矩阵转旋转向量Eigen::AngleAxisd rotation_vector(rotation_matrix);Eigen::AngleAxisd rotation_vector;rotation_vector=rotation_matrix;Eigen::AngleAxisd rotation_vector;rotation_vector.fromRotationMatrix(rotation_matrix);2.2旋转矩阵转欧拉⾓(Z-Y-X，即RPY)Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);2.3旋转矩阵转四元数Eigen::Quaterniond quaternion(rotation_matrix);Eigen::Quaterniond quaternion;quaternion=rotation_matrix;三、欧拉⾓3.0初始化欧拉⾓(Z-Y-X，即RPY)Eigen::Vector3d eulerAngle(yaw,pitch,roll);3.1欧拉⾓转旋转向量Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));Eigen::AngleAxisd rotation_vector;rotation_vector=yawAngle*pitchAngle*rollAngle;3.2欧拉⾓转旋转矩阵Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));Eigen::Matrix3d rotation_matrix;rotation_matrix=yawAngle*pitchAngle*rollAngle;3.3欧拉⾓转四元数Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));Eigen::Quaterniond quaternion;quaternion=yawAngle*pitchAngle*rollAngle;四、四元数4.0初始化四元数Eigen::Quaterniond quaternion(w,x,y,z);4.1四元数转旋转向量Eigen::AngleAxisd rotation_vector(quaternion);Eigen::AngleAxisd rotation_vector;rotation_vector=quaternion;4.2四元数转旋转矩阵Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.matrix();Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.toRotationMatrix();4.4四元数转欧拉⾓(Z-Y-X，即RPY)Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);代码⽰例：#include<iostream>#include<Eigen/Eigen>using namespace std;#define pi 3.14159265359int main(){cout << "##-------------------搞清旋转关系-------------------##" << endl;Eigen::Vector3d v1(1, 1, 0);//列向量（1系下）Eigen::AngleAxisd angle_axis1(pi / 4, Eigen::Vector3d(0, 0, 1));//1系绕z轴逆时针旋转45得到2系Eigen::Vector3d rotated_v1 = angle_axis1.matrix().inverse()*v1;cout << "绕z轴逆时针旋转45°(R12):" << endl << angle_axis1.matrix() << endl;cout << "(1, 1, 0)旋转后:" << endl << rotated_v1.transpose() << endl;cout << "------------------------------------------------------" << endl;Eigen::Vector3d v2;v2 << 0, 1, 1;Eigen::AngleAxisd angle_axis2(pi / 4, Eigen::Vector3d(1, 0, 0));//1系绕x轴逆时针旋转45得到2系Eigen::Vector3d rotated_v2 = angle_axis2.matrix().inverse()*v2;cout << "绕x轴逆时针旋转45°(R12):" << endl << angle_axis2.matrix() << endl;cout << "(0, 1, 1)旋转后:" << endl << rotated_v2.transpose() << endl;cout << "------------------------------------------------------" << endl;Eigen::Vector3d v3(0, 0, 0);v3.x() = 1;v3[2] = 1;Eigen::AngleAxisd angle_axis3(pi / 4, Eigen::Vector3d(0, 1, 0));//1系绕y轴逆时针旋转45得到2系Eigen::Vector3d rotated_v3 = angle_axis3.matrix().inverse()*v3;cout << "绕y轴逆时针旋转45°(R12):" << endl << angle_axis3.matrix() << endl;//注意和绕x轴z轴不⼀样 cout << "(1, 0, 1)旋转后:" << endl << rotated_v3.transpose() << endl;cout << "##-------------------常⽤数学运算-------------------##" << endl;Eigen::Vector3d v4(1, 1, 0);cout << "(1, 1, 0)模长：" << v4.norm() << endl;cout << "(1, 1, 0)单位向量：" << v4.normalized().transpose() << endl;Eigen::Vector3d v5(1, 0, 0),v6(0, 1, 0);cout << "(1, 0, 0)点乘(0, 1, 0):" << v5.dot(v6) << endl;cout << "(1, 0, 0)叉乘(0, 1, 0):" << v5.cross(v6).transpose() << endl;cout << "##----------------------使⽤块----------------------##" << endl;Eigen::Matrix<double, 4, 4> T12;T12.Identity();//设为单位阵Eigen::AngleAxisd angle_axis(pi / 4, Eigen::Vector3d(0, 0, 1));Eigen::Matrix3d R12(angle_axis);//⽤⾓轴初始化旋转矩阵Eigen::Vector3d t12;t12.setOnes();//各分量设为1//t.setZero();//各分量设为0T12.block<3, 3>(0, 0) = R12;T12.block<3, 1>(0, 3) = t12;cout << "旋转R12：" << endl << T12.topLeftCorner(3, 3) << endl;cout << "平移t12：" << T12.topRightCorner(3, 1).transpose() << endl;//2系原点在1系下的坐标cout << "##---------------欧式变换矩阵(Isometry)-------------##" << endl;Eigen::Isometry3d T = Eigen::Isometry3d::Identity();//虽然称为3d，实质上是4x4的矩阵(旋转R+平移t)T.linear() = Eigen::Matrix3d::Identity();//旋转部分赋值//T.linear() << 1, 0, 0, 0, 1, 0, 0, 0, 1;//cv::Mat赋值//T.rotate(Eigen::Matrix3d::Identity());//T.rotate(angle_axis);T.translation() = Eigen::Vector3d(1, 1, 1);//平移部分赋值//Eigen::Isometry3d T(quaterniond);//T(0,3) = 1; T(1,3) = 1; T(2,3) = 1;cout << "R_12: " << endl << T.linear().matrix() << endl;//输出旋转部分cout << "t_12:" << T.translation().transpose() << endl;//输出平移部分cout << "T_12: " << endl << T.matrix() << endl;//输出4x4变换矩阵cout << "T_12*(1, 2, 3): " << endl << (T*Eigen::Vector3d(1, 2, 3)).transpose() << endl;//相当于R21*v+t21,隐含齐次坐标(1，2，3，1) //Eigen::Quaterniond q = T.rotation();//Eigen::Quaterniond q(T.linear());//cout << "q: " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl;cout << "##旋转向量（轴⾓）、旋转矩阵、欧拉⾓、四元素相互转换##" << endl;//旋转向量（轴⾓）Eigen::AngleAxisd rotation_vector(pi / 4, Eigen::Vector3d(0, 1, 0));//绕y轴逆时针旋转45度(转yaw)cout << "axi: " << rotation_vector.axis().transpose() << " angle: " << rotation_vector.angle() * 180 / pi << endl;//旋转向量->旋转矩阵Eigen::Matrix3d rotation_matrix3d = rotation_vector.matrix();//Eigen::Matrix3d rotation_matrix3d = rotation_vector.toRotationMatrix();cout << "rotation_matrix:" << endl << rotation_matrix3d << endl;//旋转矩阵->欧拉⾓Eigen::Vector3d euler_angles = rotation_matrix3d.eulerAngles(0,1,2);//(0,1,2)表⽰分别绕XYZ轴顺序，即pitch yaw roll顺序，逆时针为正 cout << "pitch yaw roll = " << euler_angles.transpose() * 180 / pi << endl;//旋转向量->四元素Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);//或⽤旋转矩阵cout << "q: " << q.w() << "" << q.x() << "" << q.y() << "" << q.z() << endl;//旋转矩阵->旋转向量Eigen::AngleAxisd rotation_vector2;rotation_vector2.fromRotationMatrix(rotation_matrix3d);cout << "axi: " << rotation_vector.axis().transpose() << " angle: " << rotation_vector.angle() * 180 / pi << endl;//四元素->旋转矩阵Eigen::Quaterniond q2 = Eigen::Quaterniond(1, 0, 0, 0);//(w,x,y,z)cout << "q2:" << endl << q2.toRotationMatrix() << endl;cout << "##------------------解线性⽅程------------------##" << endl;//AX = 0//(AX)`(AX)//X`(A`A)XEigen::SelfAdjointEigenSolver<Eigen::Matrix4d> self_adjoint_solver;self_adjoint_pute(ATA);cout << "eigenvalues:\n" << self_adjoint_solver.eigenvalues();cout << "eigenvectors:\n" << self_adjoint_solver.eigenvectors();Eigen::Vector4d wxyz = self_adjoint_solver.eigenvectors().col(0);Eigen::EigenSolver<Eigen::Matrix4d> general_solver;general_pute(ATA);cout << "eigenvalues:\n" << general_solver.eigenvalues();cout << "eigenvectors:\n" << general_solver.eigenvectors();//wxyz = general_solver.eigenvectors().col(0);//AX=BEigen::Vector3d t = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);getchar();return0;}参考：。
四元数转欧拉角的雅可比矩阵
四元数转欧拉角的雅可比矩阵在机器人控制和运动规划中，四元数是一种常用的表示机器人位姿的方法。
然而，在某些情况下，需要将四元数转换为欧拉角，以方便人类理解和可视化机器人姿态。
因此，本文将介绍四元数转欧拉角的雅可比矩阵，该矩阵可以用于求解机器人位姿控制中的导数和微分方程。
首先，我们需要定义四元数和欧拉角的概念。
四元数是一种四维复数，通常用一个实部和三个虚部表示。
欧拉角是一种用于描述旋转的坐标系，通常包括绕x轴的旋转角度、绕y轴的旋转角度和绕z轴的旋转角度。
接下来，我们需要考虑如何将四元数转换为欧拉角。
这可以通过以下公式实现：$ psi = atan2(2(q_0q_3+q_1q_2),1-2(q_2^2+q_3^2)) $$ theta = asin(2(q_0q_2-q_1q_3)) $$ phi = atan2(2(q_0q_1+q_2q_3),1-2(q_1^2+q_2^2)) $ 其中，$ q_0, q_1, q_2, q_3 $ 分别表示四元数的实部和虚部。
然而，由于四元数和欧拉角之间存在非线性关系，因此我们需要使用雅可比矩阵来求解导数和微分方程。
具体来说，$ psi, theta, phi $ 分别表示绕z轴、绕y轴和绕x轴的旋转角度，因此我们可以将欧拉角表示为一个向量 $ vec{e} = [psi theta phi]^T $。
则四元数对欧拉角的雅可比矩阵可以表示为：$ J_q(vec{e}) = begin{bmatrix} frac{partial psi}{partialq_0} & frac{partial psi}{partial q_1} & frac{partialpsi}{partial q_2} & frac{partial psi}{partial q_3}frac{partial theta}{partial q_0} & frac{partialtheta}{partial q_1} & frac{partial theta}{partial q_2} &frac{partial theta}{partial q_3} frac{partial phi}{partial q_0} & frac{partial phi}{partial q_1} & frac{partialphi}{partial q_2} & frac{partial phi}{partial q_3} end{bmatrix} $其中，每个元素的求解可以使用链式法则和欧拉角转四元数的公式求解。
基于UKF的四元数载体姿态确定
基于UKF的四元数载体姿态确定
刘海颖;王惠南;刘新文
【期刊名称】《南京航空航天大学学报》
【年(卷),期】2006(038)001
【摘要】对于低精度高噪声的传感器组成的低成本姿态测量系统,本文引入Unscented Kalman filtering(UKF)用于姿态确定,设计了有陀螺测量和四元数差分法的无陀螺测量两种UKF滤波器;应用四元数避免了欧拉角法的奇异问题;用高斯-牛顿误差最小法将六维参考向量转化为四元数,作为观测量的一部分,使九维非线性观测方程转化为七维线性方程进行滤波,减少了计算量;应用仿真数据进行算法验证,成功得到姿态估计;对两种算法在低速和高速状态下进行验证,仿真结果表明了该方法的有效性.
【总页数】6页(P37-42)
【作者】刘海颖;王惠南;刘新文
【作者单位】南京航空航天大学自动化学院,南京,210016;南京航空航天大学自动化学院,南京,210016;南京航空航天大学自动化学院,南京,210016
【正文语种】中文
【中图分类】V448.2
【相关文献】
1.基于四元数的UKF多传感器卫星姿态确定 [J], 陈志明;王惠南;刘海颖
2.基于Gauss-Newton和UKF结合的微小卫星姿态确定算法 [J], 康国华;范凯;周
琼峰;梁尔涛
3.基于强跟踪的平方根 UKF 的卫星姿态确定算法 [J], 王松艳;李敏;张迎春;祝宝龙;李化义
4.基于改进自适应UKF算法的飞行器姿态确定 [J], 郑均辉;张国平
5.基于UKF的恒星相机高精度姿态确定方法 [J], 王昱;蒋唯娇
因版权原因，仅展示原文概要，查看原文内容请购买。
基于四元数的四轴无人机姿态的估计和控制
基于四元数的四轴无人机姿态的估计和控制作者：冀亮钱正洪白茹来源：《现代电子技术》2015年第11期摘要：介绍一种只需少量计算就可实现四轴无人机姿态的估计和控制的新颖方法。
整个算法采用四元数表示，四元数运算基于惯性测量单元（IMU）的加速度和陀螺仪传感器。
姿态估计使用四元数代替三角函数，允许用加速度数据分析推导和优化梯度下降算法来减少由陀螺仪传感器产生的误差。
姿态控制将当前姿态与目标姿态的姿态误差反馈到PD控制器来实现想要的目标姿态。
姿态估计中用到的梯度下降算法通过四元数对应的欧拉角的均方误差验证，静态均方误差小于0.945°。
实际的飞行测试可以实现一个稳定的飞行，同时验证了姿态控制算法。
关键词：四轴无人机；四元数；姿态估计；姿态控制中图分类号： TN967⁃34 文献标识码： A 文章编号： 1004⁃373X（2015）11⁃0112⁃05 Attitude estimation and control of quaternion based quad⁃axis UAVJI Liang， QIAN Zheng⁃hong， BAI Ru（Center for Integrated Spintronic Devices， Hangzhou Dianzi University， Hangzhou 310018， China）Abstract： A novel method with few calculations is presented to realize estimation and control of quad⁃axis UAV. The whole algorithm is expressed with quaternion， and the quaternion calculation is based on the acceleration of inertial measurement units （IMU） and gyroscope sensor. The quaternion is applied in attitude estimation instead of trigonometric function. Gradient descent algorithm is analyzed， deduced and optimized with acceleration data to reduce the error generated by gyroscope sensor. The error of current attitude and target attitude is fed back to PD controller with attitude control， to realize the expectant target attitude. The gradient descent algorithm applied in attitude estimation is verified with RMS error which is obtained from Euler angle corresponded with quaternion. The static RMS error is less than 0.945°. The stable flying is realized by actual flight test， meanwhile the attitude control algorithm is verified.Keywords： quad⁃axis UAV； quaternion； attitude estimation； attitude control0 引言四轴飞行器是众多无人机中的典型代表，它通过四个电机作为引擎来实现空中飞行。
全角度欧拉角与四元数转换的方法
2.2 四元数 在刚体定点转动理论中有一个著名的欧拉定理：刚体绕固定点的任一位移，可由绕通 [5] 过此点的某一轴转过一个角度而得到 。在单位时间间隔 t 内假设刚体角速度为 ，则该 转动轴的方向 n 及绕该轴转过的角度 分别为：， 相应四元数超复数形式为：
四元数转化成欧拉角
四元数转化成欧拉角笔者：奋斗 修改时间：2015.11.23一． 姿态解算（以匿名版程序为例）首先，程序中一般用了两种求解姿态的方法，一种为欧拉角法，一种为四元数法。
由四元数法将MPU6050的值融合成四元，再由欧拉角法把四元转换成欧拉角送入串级PID 控制器。
（1）欧拉角法静止状态，或者总加速度只是稍微大于g 时，由加计算出的值比较准确。
使用欧拉角表示姿态，令Φ,θ和Φ代表ZYX 欧拉角，分别称为偏航角、俯仰角和横滚角。
载体坐标系下的 加 速 度(a x B,a y B ,a z B )和参考坐标系下的加速度(a x N, a y N, a z N)之间的关系可表示为(1)。
其中 c 和 s 分别代表 cos 和 sin 。
axB,ayB,azB 就是mpu 读出来的三个值。
这个矩阵就是三个旋转矩阵相乘得到的，因为矩阵的乘法可以表示旋转。
axB c c c s s axN ayB c s s s c c c s s s s c ayN azB s s c s c s c c s s c c azN θψθψθφψφθψφψφθψφθφψφθψφψφθψφθ-⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥=-++⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥+-+⎣⎦⎣⎦⎣⎦ (1) 飞行器处于静止状态，此时参考系下的加速度等于重力加速度，即00xN yN zN a a g a ⎡⎤⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦（2）把（2）代入（1）可以解得arctg θ=（3）yB zB a arctg a φ⎛⎫= ⎪⎝⎭（4） 即为初始俯仰角和横滚角，通过加速度计得到载体坐标系下的加速度即可将其解出，偏航角可以通过电子罗盘求出。
（2）四元数法（通过处理单位采样时间内的角增量(mpu的陀螺仪得到的就是角增量)，为了避免噪声的微分放大，应该直接用角增量）void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){float norm;// float hx, hy, hz, bx, bz;float vx, vy, vz; // wx, wy, wz;float ex, ey, ez;// 先把这些用得到的值算好float q0q0 = q0*q0;float q0q1 = q0*q1;float q0q2 = q0*q2;// float q0q3 = q0*q3;float q1q1 = q1*q1;// float q1q2 = q1*q2;float q1q3 = q1*q3;float q2q2 = q2*q2;float q2q3 = q2*q3;float q3q3 = q3*q3;if(ax*ay*az==0)return;norm = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化ax = ax /norm;ay = ay / norm;az = az / norm;// estimated direction of gravity and flux (v and w)vx = 2*(q1q3 - q0q2); //四元素中xyzvy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// error is sum of cross product between reference direction of fields and direction measured by sensorsex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差ey = (az*vx - ax*vz) ;ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki; //对误差进行积分eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt; //将误差PI 后补偿到陀螺仪，即补偿零点漂移 gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt; //这里的gz 由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减// integrate quaternion rate and normalise //四元素的微分方程 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;// normalise quaternionnorm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;//Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yawQ_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitchQ_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll}下面对上面的程序逐条解释。
三种坐标转换模型的比较
三种坐标转换模型的比较宋世泽;郑勇;王鼎蔚【摘要】本文分别用罗德里格参数、欧拉角、四元数构造旋转矩阵,进行坐标转换,然后做出比较.文章对三种模型的误差和效率都作出了实验分析,结果表明,在没有较好的初值情况下,罗德里格和四元数模型效率较高,但欧拉角和罗德里格模型的精度较高.因此,在没有初值的情况下,建议优先选取罗德里格模型.【期刊名称】《全球定位系统》【年(卷),期】2016(041)006【总页数】5页(P110-114)【关键词】罗德里格参数;欧拉角;四元数;坐标转换【作者】宋世泽;郑勇;王鼎蔚【作者单位】信息工程大学导航与空天目标工程学院,郑州450001;北斗导航应用技术河南省协同创新中心,郑州450001;信息工程大学导航与空天目标工程学院,郑州450001;北斗导航应用技术河南省协同创新中心,郑州450001;信息工程大学导航与空天目标工程学院,郑州450001;北斗导航应用技术河南省协同创新中心,郑州450001【正文语种】中文【中图分类】P228.4在大地测量、摄影测量、和工程测量等应用中,经常用到坐标转换,这就需要解算坐标转换模型中的未知参数,其中关键的部分是解算构成旋转矩阵的未知参数。
有多种方法可以构造旋转矩阵,常用的三种方法为罗德里格参数、欧拉角和四元数法。
这三种方法都有各自的特点。
罗德里格参数法可以计算出较为精确的初始值,并且最后得到的误差方程形式较为简单,因此,比较适合用在相机检校上,文献[3-6]均利用罗德里格参数克服了相机检校中旋转矩阵的初始值不易获取的问题。
在有些应用领域,由于旋转角直接给出,因此选用欧拉角构造旋转矩阵更为简单,例如在视频全站仪测量中,旋转角由全站仪精密测定,文献[7-12]对此做了详细阐述。
四元数法具有直观的物理意义,它是将空间某个基准坐标系绕一个向量仅仅做一次旋转就可旋转到目标坐标系,与罗德里格矩阵一样,如果知道两个坐标系下的3个以上公共点,就可以求解较准确的旋转矩阵的初始值,文献[15-17]给出了详细阐述。
基于四元数和卡尔曼滤波的姿态角估计算法研究与应用
基于四元数和卡尔曼滤波的姿态角估计算法研究与应用一、本文概述Overview of this article本文旨在探讨基于四元数和卡尔曼滤波的姿态角估计算法的研究与应用。
姿态角，即物体在三维空间中的旋转角度，是众多领域如航空航天、机器人技术、虚拟现实等中的重要参数。
精确而稳定的姿态角估计是这些领域实现精确控制和高效运行的关键。
然而，由于传感器噪声、动态环境和计算误差等因素的影响，姿态角的准确估计仍然是一个具有挑战性的问题。
This article aims to explore the research and application of attitude angle estimation algorithms based on quaternion and Kalman filtering. Attitude angle, which refers to the rotation angle of an object in three-dimensional space, is an important parameter in many fields such as aerospace, robotics, virtual reality, and so on. Accurate and stable attitude angle estimation is the key to achieving precise control and efficient operation in these fields. However, due to factorssuch as sensor noise, dynamic environment, and calculation errors, accurate estimation of attitude angle remains a challenging problem.四元数作为一种高效的旋转表示方法，具有无奇异性和避免万向锁等优点，因此在姿态估计中得到了广泛应用。
对基于四元数的飞机本体运动模型的改进
对基于四元数的飞机本体运动模型的改进
王军安
【期刊名称】《系统仿真学报》
【年(卷),期】2006()z2
【摘要】现有的基于四元数的飞机本体运动方程没有完全消除欧拉角。
本文推导了气流坐标系与机体坐标系的转换关系,尤其考虑了俯仰角为±90°时的情况。
得到了以向量式表示的、完全消除欧拉角的飞机本体运动数学模型。
该模型便于面向对象程序实现,并用于大姿态角飞行仿真。
【总页数】3页(P230-232)
【关键词】欧拉角;四元数;六自由度;向量;动力学;运动学
【作者】王军安
【作者单位】中航一集团第一飞机设计研究院
【正文语种】中文
【中图分类】TP391.9
【相关文献】
1.基于事件本体的CGF飞机实体作战计划模型 [J], 张媛;刘文彪;杜彬彬;彭四喜
2.基于本体的飞机数字化柔性装配工艺模型 [J], 胡玉龙;王仲奇;任聪;李西宁;康永刚
3.基于本体的作战飞机角色交互模型研究 [J], 宦婧;周伟祝;赵媛
4.基于本体的飞机设计知识组织模型 [J], 关煜杰;赵罡;吕炎杰;周雷
5.基于改进四元数阻尼误差模型的SINS初始对准算法 [J], 赵仁杰;李开龙;胡柏青;田佳玉
因版权原因，仅展示原文概要，查看原文内容请购买。
miniAHRS传感器的四元数单片机采集与欧拉角转换_周永录(1)
*
（ 云南大学 云南省电子计算中心， 云南 昆明 650223 ） 摘要： 以遥控无人飞艇航拍平台的云台随动控制系统为实例， 研究采用新型 1T 单片机 STC12C5412 ， 通过 UART 接口与 miniAHRS 传感器相连接， 进行硬件接口电路及软件设计， 解决遥控无人飞艇航拍平台上云台随 动控制系统的三维位姿检测问题 ． 关键词： miniAHRS 传感器； 片机； 三维位姿检测； 四元数； 欧拉角转换 中图分类号： TP 391 文献标识码： A 文章编号： 0258 － 7971 （ 2010 ） S2 － 0042 － 07
［1 ］
行从四元数到欧拉角的转换． 1 miniAHRS 传感器和 STC 1T 单片机简介 1． 1 miniAHRS 位姿传感器简介 miniAHRS 位 姿传感器是美国 Innalabs 公司研发生产的一种微
型高性能捷联惯导系统（ 图 1 ） ， 在陆地、 航海、 航空 以及智能机器人等测控领域中具有较好的应用价 3 轴加速度计和 3 值． 该传感器包括 3 轴陀螺仪、 轴磁通门式磁力计， 其中， 陀螺仪用来测量载体的 绝对角速度， 表征载体角定位的俯仰角 （ Pitch ） 、 横 滚角（ Roll） 、 偏航角（ Yaw ） 通过陀螺仪的输出信号 计算得出； 加速度计用于系统开始的清零和修正载 体因陀螺仪漂移造成的俯仰和横滚的误差 ； 磁力计 用来进行开始的航向清零和修正因陀螺仪漂移造 miniAHRS 还包含了一个温度 成的航向误差． 此外，
miniAHRS 传感器提供了 2 个相同信号的 RS － 485 通信接口， 2 个接口的引脚信号定义如图 3 2 引脚为电源输入的正端和负端， 所示． 1 ， 输入的 4 引脚分别是 电压范围为 5． 1 ～ 5． 7 V （ 80 mA ） ． 3 ， RS － 485 的 A， B 2 个输入输出信号．
四旋翼无人机飞行控制中四元数与欧拉角的转换
四旋翼无人机飞行控制中四元数与欧拉角的转换作者：陈孟臻来源：《科学导报·学术》2019年第45期摘; 要：在四旋翼无人机飞行控制环路中，欧拉角便于终端输出，适合人的直观观察和理解，方便人为调整姿态控制参数，进而调整四旋翼飞行姿态，但是占用内存较大。
而四元数占用内存少，且方便在环路控制中实现插值操作，因此可提高机器运算速度，实现快速参数设定，因此研究四元数与欧拉角之间的转换方式具有一定的研究意义。
关键词：无人机;姿态;四元数;欧拉角1 四元数四元数（Quaternions）是简单的超复数，由爱尔兰数学家哈密顿在1843年提出的数学概念，总所周知复数是由实数加上虚数单位i组成，其中。
相似地，四元数都是由实数加上三个虚数单位i、j、k 组成，而且它们有如下的关系：四元数一般可表示为a+bi+cj+dk，其中a、b、c、d是实数，称为四元数的模。
为了说明是a+bi+cj+dk怎么和三维旋转结合在一起的，使用如下式子来表示一个四元数：q =（x，y，z），w）=（v，w），其中：其中v是向量，w是实数。
使用一个四元数：，来执行一个旋转，即把空间中一个P点绕着单位向量轴u=（x，y，z）表示旋转轴旋转角度θ，当把P点扩展到四元数空间，即p=（P，0），旋转后新的点对应的新坐标为：。
2.从欧拉角到四元数欧拉旋转是最接近自然理解的一种旋转方式，日常生活中常这些轴称为前、后、左、右、上、下、这意味着在旋转之前需要指明一个顺序。
其按照一定的坐标轴顺序（例如先x、再y、最后z）、每個轴旋转一定角度来变换坐标或向量，它实际上是一系列坐标轴旋转的组合。
给定一个旋转顺序（例如z、x、y），以及它们对应的旋转角度（φ，θ，ψ），不同的坐标选择结果不一样。
欧拉旋转的数学实现常见使用3×3矩阵：在四旋翼无人机飞行控制中，规定航空次序欧拉角为：Z轴（yaw，ψ），Y轴（pitch，θ），X轴（roll，φ），其中，pitch是围绕X轴旋转，也叫做俯仰角;yaw是围绕Y轴旋转，也叫偏航角;roll是围绕Z轴旋转，也叫翻滚角。
python欧拉角转四元数
python欧拉角转四元数欧拉角是描述物体在三维空间中姿态的常用方法，但是欧拉角存在万向锁问题，使得在计算机图形学中不太适用。
四元数是一种解决欧拉角问题的方法，能够更加精准地描述物体的旋转。
在Python中，我们可以使用NumPy库来进行欧拉角转换成四元数的计算。
具体步骤如下：1. 首先导入NumPy库：import numpy as np2. 定义欧拉角的三个角度：roll = 0.0 # 绕X轴旋转角度pitch = 0.0 # 绕Y轴旋转角度yaw = 0.0 # 绕Z轴旋转角度3. 将欧拉角转换成弧度：roll_rad = np.radians(roll)pitch_rad = np.radians(pitch)yaw_rad = np.radians(yaw)4. 计算四元数：cy = np.cos(yaw_rad * 0.5)sy = np.sin(yaw_rad * 0.5)cp = np.cos(pitch_rad * 0.5)sp = np.sin(pitch_rad * 0.5)cr = np.cos(roll_rad * 0.5)sr = np.sin(roll_rad * 0.5)w = cy * cp * cr + sy * sp * srx = cy * cp * sr - sy * sp * cry = sy * cp * sr + cy * sp * crz = sy * cp * cr - cy * sp * srquat = np.array([w, x, y, z])5. 最后输出四元数：print(quat)以上就是Python实现欧拉角转换成四元数的步骤。
通过这种方法，我们可以更加精准地描述物体的旋转姿态，在计算机图形学等领域有着广泛的应用。
基于四元数的偏流角跟踪与条带拼接成像研究
基于四元数的偏流角跟踪与条带拼接成像研究杜宁;王世耀;孟其琛【期刊名称】《上海航天》【年(卷),期】2016(033)006【摘要】根据小卫星星载相机存在后视角或安装相机摆镜导致相机实际光轴无法与星体主轴平行的状况,为避免用欧拉角时的姿态解算与转序问题,提出了一种基于四元数的卫星偏流角跟踪与条带拼接成像姿态控制方法.用四元数描述卫星姿态,根据相对轨道系目标四元数,绕相机光轴转动偏流角,以此作为成像模式目标四元数,实现绕空间轴的偏流角跟踪控制.给出了姿态规划算法:固定偏置姿态确定偏流角跟踪后的目标姿态和目标角速度,用迭代法提高偏流角控制精度,并在姿态机动过程开始即进行偏流角跟踪,保证姿态机动到位和高精度偏流角跟踪的同时实现.基于内干扰力矩前馈方法设计了姿态机动控制律.以同轨双条带拼接成像为例,给出了成像控制方法:在对日或对地定向基础上,计算偏置目标姿态和目标角速度,并调用姿态机动控制算法;姿态机动到位后,若需当轨完成多目标姿态机动,则用姿态机动控制算法保持姿态偏置飞行和偏流角跟踪控制.数学仿真结果验证了该算法的有效性和高精度.【总页数】7页(P31-37)【作者】杜宁;王世耀;孟其琛【作者单位】上海航天控制技术研究所,上海201109;上海航天控制技术研究所,上海201109;上海航天控制技术研究所,上海201109【正文语种】中文【中图分类】V448.21【相关文献】1.基于极坐标格式算法的大斜视条带SAR子孔径拼接成像算法 [J], 梁媚蓉;王晨沁;毛新华2.基于条带式SAR与聚束式SAR内在联系的SAR成像研究 [J], 魏青;杨绍全;罗明;饶鲜3.基于单位四元数法的激光点云拼接算法研究 [J], 王英男;戴曙光4.基于多波段匹配的超光谱成像仪图像条带噪声去除方法研究 [J], 高海亮;顾行发;余涛;孙源;汪左;刘李5.基于共视目标的侧扫声纳条带图像拼接方法研究 [J], 曹明;郭军因版权原因，仅展示原文概要，查看原文内容请购买。
基于四元数的人运动雷达回波计算方法
基于四元数的人运动雷达回波计算方法
李飞;黄斌科;冯恩信;汪文秉
【期刊名称】《探测与控制学报》
【年(卷),期】2010(032)002
【摘要】人运动的雷达散射回波的获取是研究人微多普勒特征的关键问题.在计算运动人体各部件位置时,常用的欧拉角方法存在复杂矩阵运算等问题,因此针对一个常用的运动模型特点,提出利用四元数方法进行各部件位置计算.四元数法可以有效地减少矩阵运算.应用该方法仿真获得的人体运动散射回波与国外实测数据对比结果表明了其正确性.对合成回波的时频分析结果表明,人运动的微运动特征可以提供有价值的信息,并有望用于人体目标的识别.
【总页数】5页(P38-41,46)
【作者】李飞;黄斌科;冯恩信;汪文秉
【作者单位】西安交通大学电信学院微波所,陕西,西安,710049;西安交通大学电信学院微波所,陕西,西安,710049;西安交通大学电信学院微波所,陕西,西安,710049;西安交通大学电信学院微波所,陕西,西安,710049
【正文语种】中文
【中图分类】TN95
【相关文献】
1.基于Chirplet的逆合成孔径雷达回波高速运动补偿算法 [J], 金光虎;高勋章;黎湘;陈永光
2.基于高频电磁散射理论的电大复杂目标宽带雷达回波快速计算方法 [J], 王海彬
3.基于微多普勒特征的人体目标雷达回波信号分析 [J], 张翼;朱玉鹏;程永强;黎湘
4.基于四元数描述和EMD的人体运动捕获数据检索 [J], 肖秦琨;李俊芳;肖秦汉
5.基于高阶WVD的雷达运动目标回波信号处理 [J], 成志峰;宋耀良;是湘全
因版权原因，仅展示原文概要，查看原文内容请购买。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
miniAHRS 传感器提供了 2 个相同信号的 RS － 485 通信接口， 2 个接口的引脚信号定义如图 3 2 引脚为电源输入的正端和负端， 所示． 1 ， 输入的 4 引脚分别是 电压范围为 5． 1 ～ 5． 7 V （ 80 mA ） ． 3 ， RS － 485 的 A， B 2 个输入输出信号．
43
传感器， 用于修正温度漂移所造成的陀螺仪误差． miniAHRS 传感器数 据 输 出 采 用 TIA / EIA － 485A 标准， 半双工方式， 波特率为 1 Mb / s． 其具有 2 种数 3 轴加 一种是直接输出 3 轴陀螺仪、 据输出方式， 3 轴磁力计以及温度测量数据的原 （ Raw ） 速度计、 数据输出方式， 还有一种是输出表征载体空间位姿 数据的四元数 （ Quaternion ） 输出方式． 采用四元数 表示方法的优点是可以克服用欧拉角 （ Euler ） 来处 ［4 ］ 理刚体旋转所固有的“万向节锁死 ” 问题 ． 虽然 四元数难以直观反映载体在空间的具体方位 ， 但通 过将四元数转换成欧拉角的方法 ， 能够得到直接表 征载 体 空 间 角 定 位 的 俯 仰 角 （ Pitch ） 、 横滚角 （ Roll） 和偏航角（ Yaw ） ． 1． 2 STC 1T 单片机简介 STC 1T 单片机是由宏
*
（ 云南大学 云南省电子计算中心， 云南 昆明 650223 ） 摘要： 以遥控无人飞艇航拍平台的云台随动控制系统为实例， 研究采用新型 1T 单片机 STC12C5412 ， 通过 UART 接口与 miniAHRS 传感器相连接， 进行硬件接口电路及软件设计， 解决遥控无人飞艇航拍平台上云台随 动控制系统的三维位姿检测问题 ． 关键词： miniAHRS 传感器； 片机； 三维位姿检测； 四元数； 欧拉角转换 中图分类号： TP 391 文献标识码： A 文章编号： 0258 － 7971 （ 2010 ） S2 － 0042 － 07
表 1 miniAHRS 的传输数据包格式 Tab． 1 The transport packet format of miniAHRS 序号 1 2 3 XX XX N +2 N +3 名称 Id Len Data1 DataXX DataXX DataN Cs 数值 0x55AA 8 ～ 65534 0 ～ 65535 0 ～ 65535 0 ～ 65535 0 ～ 65535 0 ～ 65535 包头识别符 包字节计数， 从 Id 至 Cs 数据 数据 数据 数据 校验和， 从 Id 到 DataN 的字累加和 说 明
遥控无人飞艇航拍低空遥感技术具有拍摄精 安全、 经济、 高效、 不受航空管制、 不受起降场 度高、 地和飞行次数限制、 操作便捷灵活等突出的性能和 优点， 为高分辨率遥感影像数据的获取提供了一种 新的控制平台． 遥控无人飞艇航拍平台要获得达到 测绘级要求的正射影像数据， 需要航拍相机镜头始 要求安装相机的云台系统能够根 终垂直于地平面， 据飞艇的位姿对云台进行实时调整与控制 ． miniAHRS 位姿传感器是一种高性能的捷联惯 导系统， 能够在 3D 空间中测量任何载体的全角度 ［1 ］ 方向 ， 这为遥控无人飞艇航拍平台的云台随动 控制提供了一种三维位姿检测传感器较好的选择 ． miniAHRS 传感器的数据输出接口是 RS － 485 ， 通 过转接 电 缆， 可 转 换 成 USB 接 口， 与 PC 机 相 连 ［2 － 3 ］ ， 接 其数据传输波特率达到 1 Mb / s． 然而， 一般 的单片机或微处理器的 UART 接口所支持的最高 波特率往 往 只 有 几 十 到 几 百 Kb / s， 难 于 与 miniAHRS 传感器相连接． 有文献提出了借助 LabVIEW 工具实现 传 感 器 数 据 的 间 接 读 取 方 法 和 采 用 miniAHRS 与 PC 机直接连接的方法［2 － 3］， 但在现 有文献中还未见到有通过单片机采集 miniAHRS 传感器数据方法描述． miniAHRS 传感器的位姿数据输出为四元数格 式， 不能直接表征 3D 空间各轴的方向角， 要实现 遥控无人飞艇航拍平台的云台随动控制 ， 还需要进
图4
软件主流程和 UART 中断流程
Fig． 4 Main flow and UART interrupt flow of software
第 S2 期
周永录等： miniAHRS 传感器的四元数单片机采集与欧拉角转换
45
为使 STC12C5412 的 UART 能 够 达 到 1Mbps 的波特率， 必须正确选择 STC12C5412 外部晶体振 荡器 Y1 的振荡频率． 根据标准 51 单片机 UART 的 ［5 ］ 并结合 STC12C5412 的 1T 高 波特率计算方法 ， 速特 性， 当 选 择 Y1 的 振 荡 频 率 为 16 MHz 时， STC12C5412 的 UART 波特率最高可达到 1 Mbps， STC12C5412 的 而当 Y1 的振荡频率为 32 MHz 时， UART 最高波特率可达到 2 Mbps． 本文所述系统选 择 32 MHz 的晶体振荡器， 通过适当的软件设置， 使 STC12C5412 的 UART 工作于 1 Mbps 波特率状态， 同时， 也使 STC12C5412 的工作频率能够达到 384 MHz 的较高性能．
字（ word） 组成的数据帧进行传输， 每个字包含 2 个 （ byte ） ， 1 字节 第 个字节是低位字节， 第 2 个字节 为高位字节． 传输数据包格式如表 1 所示． miniAHRS 传感器具有原（ Raw ） 数据和四元数 两种数据输出方式， 本文主要涉及四元数数据输出 方式． 当 设 置 miniAHRS 工 作 于 四 元 数 输 出 方 式 时， 表 1 中 Data1 ～ DataN 的数据包格式如表 2 所 示． miniAHRS 传感器 综合表 1 和表 2 可以看出， 工作于四元数输出方式时， 一个数据输出帧由 10 个字 20 个字节组成， 第 1 个字是包头 0x55AA， 第2 个字是整个数据包长度的字节计数， 也就是 20 个 字节， 四元数位于包的第 5 至第 8 个字， 数据类型 最后一个字是数据包前 9 个 是 Q15 定标浮点数， 四元 字的累加校验和． miniAHRS 传感器有复位、 、 、 、 数模式 自动传输 原数据请求 四元数请求共 5 个 输入命令， 每个命令由 2 个命令字组成， 命令格式 如表 3 所示．
第 32 卷
miniAHRS 传感器的通信接口信号是 RS － 485 电 平， 而 STC12C5412 单片机的 UART 接口为 TTL 电 为了将 miniAHRS 传感器的 RS － 485 接口信号 平， 还需要采用 1 片 MAX485 半 接入到 STC12C5412 ， 双工电平转换芯片， 将 RS － 485 电平转换为 TTL 电平． miniAHRS 传感器与 STC12C5412 之间的通 miniAHRS 的 RS － 信硬件接口如图 4 所示． 其中， 485 接口中的差分信号 A， B 分别连接到 MAX485 B 两路差分信号； MAX485 的驱动器输入端 的 A，
图1
miniAHRS 传感器
Fig． 1 miniAHRS sensor
*
收稿日期： 2010 － 09 － 01 作者简介： 周永录（ 1965 － ） ， 男， 云南人， 高级工程师， 主要从事自动控制和嵌入式网络音视频技术方面的研究．
第 S2 期
周永录等： miniAHRS 传感器的四元数单片机采集与欧拉角转换
DI 和 接 收 器 输 出 端 RO 分 别 与 STC12C5412 的 UART 接口发送信号 TxD （ P3． 1 ） 和接收信号 RxD （ P3． 0 ） 相连接； STC12C5412 的通用 IO 端口 P3． 4 与 MAX485 的驱动器输出使能信号 DE 和接收器 输出使能信号 RO 相连接， 作为 MAX485 半双工方 式的 收 发 使 能 控 制 信 号， 当 P3． 4 输 出 高 电 平 时， MAX485 工作于发送方式， 当 P3． 4 输出 低 电 平 时 MAX485 工作于接收方式． 的，
云 南 大 学 学 报 （ 自 然 科 学 版 ） ，2010 ，32 （ S2 ） ： 42 ～ 48 Journal of Yunnan University
CN 53 － 1045 / N
ISSN 0258 － 7971
miniAHRS 传感器的四元数单片机采集与欧拉角转换
周永录，张 熹，杨建国
2
miniAHRS 与 STC 单片机通信硬件接 口设计
晶公司推出的新一代 8051 系列高性能微控制器， 包括 STC10 /11 /12 等系列， 其显著特征是采用增 强型 8051 内核， 以 1 个时钟周期为 1 个机器周期 的高速度运行， 而普通的单片机是 12T 方式， 即1 个机器周期需要 12 个时钟周期， 因而 STC 1T 系列 单片机比普通标准的 8051 单片机快 8 ～ 12 倍． 工 作频率为 0 ～ 35 MHz， 相当于 0 ～ 420 MHz 的普通 STC 1T 系列单片机拥有 1 ～ 62 KB 单片机． 此外， 256 ～ 1 280 B 的内部 RAM 空 的程序 Flash 存储器， 2． 1 ～ 5． 5 V 的宽范围供电， 间， 支持在系统编程 （ ISP） 和在应用编程 （ IAP ） ， 具有高速度、 低功耗、 宽电压、 抗干扰能力强等特点． STC 1T 系列单片机 的内部定时器既可工作在 12T 方式， 也能够工作在 1 T 高速方式 ， 因而当内部定时器作 为 UART 波 特
图2
miniAHRS RS － 485 接口信号
Fig． 2 miniAHRS RS － 485 interface signals
图3
miniAHRS 与 STC12C5412 通信接口