姿态解算算法模块--C源码

生来就可爱ヽ(ⅴ<●) 提交于 2020-02-16 00:18:30

了解或想开发无人机的朋友肯定绕不过姿态解算这茬,花点时间去了解它们原理并不难,这里提供两个原理链接供大家参考:
四元数表示旋转的理解
四旋翼姿态解算原理
而在代码实现方面,我这里写好了姿态解算算法模块供大家学习和参考。

下载地址:
百度云链接: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;
	}
}
标签
易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!