了解或想开发无人机的朋友肯定绕不过姿态解算这茬,花点时间去了解它们原理并不难,这里提供两个原理链接供大家参考:
四元数表示旋转的理解
四旋翼姿态解算原理
而在代码实现方面,我这里写好了姿态解算算法模块供大家学习和参考。
下载地址:
百度云链接:https://pan.baidu.com/s/1kFeqP_WYllgXdjU6Ejwejg 提取码:yawh
Github链接:https://github.com/diceTZ/Pose.git
模块的输入:
1、加速度3轴数据
2、陀螺仪3轴数据
3、磁力计3轴数据(可选)
模块的输出:
1、姿态角(pit,roll, yaw)
2、旋转矩阵
3、世界坐标系下的加速度
4、矫正后的加速度(水平加速度和垂直加速度),陀螺仪
注:模块坐标为右手坐标系,机头方向为x轴正方向,天空为z轴正方向。
加速度方向说明:
向机头方向加速,acc_x为正;
向机头左侧方向加速,acc_y为正;
向天空方向加速度,acc_z大于-980 cm/s^2 例如:-970 cm/s^2;
陀螺仪方向说明:
拇指指向机头方向,右手四指旋向为gyro_x正方向;
其他同理
pose.h(pose.c在下载地址中)
//@作者 :tou_zi
//@编写时间 :2019年4月6日
//@修改时间 :2019年4月6日
//@文件名 :pose.h
//@描述 :姿态解算模块
#ifndef _POSE_H
#define _POSE_H
#ifndef u8
#define u8 unsigned char
#endif
#ifndef s8
#define s8 char
#endif
#ifndef XYZ_Data
#define XYZ_Data
typedef struct XYZ_Data_f
{
float x;
float y;
float z;
}XYZ_Data_f;
typedef struct XYZ_Data_s32
{
long x;
long y;
long z;
}XYZ_Data_s32;
typedef struct XYZ_Data_s16
{
short x;
short y;
short z;
}XYZ_Data_s16;
typedef struct XYZ_Data_s8
{
s8 x;
s8 y;
s8 z;
}XYZ_Data_s8;
#endif
typedef struct Pose_Flag
{
u8 run;
u8 use_mag;
}Pose_Flag;
typedef struct Pose_DInterface
{
float *a_x;
float *a_y;
float *a_z;
float *g_x;
float *g_y;
float *g_z;
float *m_x;
float *m_y;
float *m_z;
}Pose_DInterface;
typedef struct Pose_Interface
{
Pose_DInterface data;
}Pose_Interface;
typedef struct Pose_Data
{
float yaw;
float rol;
float pit;
float rotate_matrix[3][3]; //旋转矩阵
XYZ_Data_f acc_world; //世界坐标系下的加速度
XYZ_Data_f mag_world; //世界坐标系下的磁场强度 -- 只与无人机位置有关的量
XYZ_Data_f acc_correct; //机体坐标系下的加速度 -- 矫正了俯仰翻滚后的加速度
XYZ_Data_f mag_correct; //机体坐标系下的磁场强度 -- 矫正了俯仰翻滚后的磁场强度
XYZ_Data_f gyro_correct; //融合加速度和磁力计数据,矫正后的陀螺仪值
}Pose_Data;
typedef struct Pose_Process
{
float mag_yaw; //磁力计的偏航角
float mag_yaw_bias; //磁力计矫正的偏航角偏差
float quaternion[4]; //四元数
XYZ_Data_f error; //由加速度计与等效重力的偏差
XYZ_Data_f error_integral; //误差积分
}Pose_Process;
typedef struct Pose_Parameter
{
float correct_kp; //矫正速度
float error_kp; //矫正速度
float error_ki;
}Pose_Parameter;
typedef struct Pose_Module
{
Pose_Flag flag;
Pose_Interface interface;
Pose_Process process;
Pose_Data data;
Pose_Parameter parameter;
}Pose_Module;
//初始化结构体
void initPose_Module(Pose_Module *pose);
//计算姿态
void calculatePose_Module(Pose_Module *pose, float cycle);
例程代码:
#include "stdlib.h"
#include "pose.h"
Pose_Module pose;
int main(void)
{
float acc_x = 0, acc_y = 0, acc_z = -980.0f;
float gyro_x = 0, gyro_y = 0, gyro_z = 0;
float pit, rol, yaw;
//初始化结构体
initPose_Module(&pose);
//连接接口
pose.interface.data.a_x = &acc_x;
pose.interface.data.a_y = &acc_y;
pose.interface.data.a_z = &acc_z;
pose.interface.data.g_x = &gyro_x;
pose.interface.data.g_y = &gyro_y;
pose.interface.data.g_z = &gyro_z;
while(1)
{
//运算姿态解算算法模块
calculatePose_Module(&pose, 0.01f);
//获取数据
pit = pose.data.pit;
rol = pose.data.rol;
yaw = pose.data.yaw;
}
}
来源:CSDN
作者:tou_zi
链接:https://blog.csdn.net/weixin_39159599/article/details/104334166