基于STM32F4的姿态解算系统.doc

基于STM32F4的姿态解算系统
摘要：本实验主要利用的基于STM32F4开发板上MUP6050陀螺仪加速度计来做四元素的更新和四元素的姿态解算系统，开发平台为keil uvision5。主要实现MPU6050自带数字运动处理DMP的四元素与自己编写的三子样旋转矢量算法对比的目的。将MPU6050输出的原始数据和姿态解算数据通过串口上传到四轴上位机进行3D仿真，和用LCD屏进行数据的实时显示，并且用一个LED灯显示系统的运行和一个开关控制数据是否上传上位机。程序是在UCOSIII操作系统中实现，建立了一个定时器，五个任务。定时器实现四元素更新的三子样算法和姿态解算；开始任务负责创建任务；任务一实现开关对数据上传控制；任务二实现上传MPU6050输出数据和自己解算姿态到上位机；任务三实现MPU6050的四元素读取并显示到LCD上；任务四负责LCD数据和字符的显示。
关键字：MPU6050；四元素；姿态解算；UCOSIII
1 引言
现在MEMS陀螺仪和加速的计价格便宜，精度虽不如光纤和激光陀螺，但是它以价格便宜体积重量小等优势占据一定的地位，它应用的领域主要在民用手机导航小型飞行器当中。基于MEMS惯性元件的导航可应用于个人的导航和小型飞行器的导航，研究MEMS陀螺导航具有一定的实用价值。本实验主要利用的基于STM32F4开发板上MUP6050陀螺仪加速度计来做四元素的更新和四元素的姿态解算，开发平台为keil uvision5，将数据通过串口上传到四轴上位机进行3D仿真，并用LCD屏进行数据的实时显示。
2 系统架构
以STM32F4为处理器，通过I2C总线与MEMS陀螺仪加速速度计通信，16位FTFLCD液晶显示屏作为数据的实时显示，通过串口将MPU6050的数据和解算的姿态角上传上位机。姿态解算系统的构架如图2-1。

2-1系统构架图
系统的功能：
（1） 将STM32F4读取的MPU6050的数据上传上位机
（2） 解算姿态角，上传四轴上位机进行仿真，并在LCD上实时显示姿态角。
（3） 读取MPU6050的四元素并在LCD上显示
（4） 解算使用三子样四元素更新算法解算姿态角，并在LCD上显示。

3 软硬件设计
3.1 硬件设计
本实验所要用到的硬件资源如下：
1） 指示灯 DS0
2） KEY0 按键
3） TFTLCD 模块
4） 串口
5） MPU6050
指示灯：
用到的指示灯DS0，位共阳型，连接到STM32F4的PF9引脚。如图3-1所示。

图3-1 SD0与STM32连接原理图
按键：
KEY0按下引脚PE4接上低电平。判断PE4上的电平就可以知道按键的状态。如图3-2所示。

3-2 按键与STM32F4的连接原理图
TFTLCD 模块：
4.3寸，分辨率为800*480，16真彩显示。其与STM32F4的连接如图3-3所示。
引脚含义：
CS： TFTLCD 片选信号。
WR：向 TFTLCD 写入数据。
RD：从 TFTLCD 读取数据。
D[15：0]：16 位双向数据线。
RST：硬复位 TFTLCD。
RS：命令/数据标志（ 0，读写命令； 1，读写数据）。

图3-3 TFTLCD与STM32F4的连接原理
MPU6050：
MPU6050内部整合了3轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（ DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。MPU6050与STM32F4的连接如图3-4所示。

图3-4 MPU6050与STM32F4的连接原理图
3.2 软件设计
3.2.1 底层驱动部分
程序的实现是在UCOSIII实时操作系统上实现，但是在使用操作系统前必须写好指示灯 DS0、KEY0 按键、TFTLCD 模块、串口以及MPU6050的驱动，还有系统的移植。本设计是在已经移植好的UCOSIII操作系统上进行的，所以就对UCOSIII移植到STM32F4上不做说明。
工程文件夹下的文件HARDWARE文件夹存放的是驱动文件，其中有IIC、KEY、LCD、LED、MPU6050文件分别是I2C、按键KEY、LCD、LED和MPU6050的驱动文件，如图4-1所示。其中的UCOSIII文件夹为UCOSIII操作系统文件。

图4-1 工程文件
LED文件夹中的led.c和led.h主要是对GPIO口和使能对应的GPIO时钟的操作。KEY文件夹中的key.c和key.h主要是先初始化对应的GPIO口和实现一个按键扫描函数KEY_Scan( ) 。
//mode:0,不支持连续按;1,支持连续按;
//返回值1表示,KEY0按下
//注意此函数有响应优先级,KEY0>KEY1>KEY2>WK_UP!!
u8 KEY_Scan(u8 mode)
{
static u8 key_up=1;//按键按松开标志
if(mode)key_up=1; //支持连按
if(key_up&&(KEY0==0||KEY1==0||KEY2==0||WK_UP==1))
{
delay_ms(10);//去抖动
key_up=0;
if(KEY0==0)return 1;
else if(KEY1==0)return 2;
else if(KEY2==0)return 3;
else if(WK_UP==1)return 4;
}else if(KEY0==1&&KEY1==1&&KEY2==1&&WK_UP==0)key_up=1;
return 0;// 无按键按下
}
在IIC文件夹中的myiic.c和myiic.h主要实现I2C读和写一个字节， IIC_Read_Byte ( )和IIC_Send_Byte( )。由于大家对I2C总线比较熟悉就不做多说明。
LCD文件夹中lcd.c和lcd.h主要实现一些对LCD屏的操作的函数。用到的主要有LCD_ShowString/LCD_ShowxNum( )/LCD_ShowNum( )。
//显示数字,高位为0,则不显示
//x,y :起点坐标
//len :数字的位数
//size:字体大小
//color:颜色
//num:数值(0~4294967295);
void LCD_ShowNum(u16 x,u16 y,u32 num,u8 len,u8 size); //显示一个数字
//显示数字,高位为0,还是显示
//x,y:起点坐标
//num:数值(0~999999999);
//len:长度(即要显示的位数)
//size:字体大小
//mode:
//[7]:0,不填充;1,填充0.
//[6:1]:保留
//[0]:0,非叠加显示;1,叠加显示.
void LCD_ShowxNum(u16 x,u16 y,u32 num,u8 len,u8 size,u8 mode); //显示 数字
//显示字符串
//x,y:起点坐标
//width,height:区域大小
//size:字体大小
//*p:字符串起始地址
void LCD_ShowString(u16 x,u16 y,u16 width,u16 height,u8 size,u8 *p); //显示一个字符串,12/16字体
MPU6050中的文件mpu6050.c和mpu6050.h实现mpu6050的驱动。只要对mpu6050的一些基本的设置和初始化以及数据的读取。
u8 MPU_Init(void)
{
u8 res;
IIC_Init();//初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g

MPU_Set_Rate(50); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//器件ID正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);//设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(50); //设置采样率为50Hz
}else return 1;
return 0;
}
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
//得到陀螺仪值(原始值)，16位AD采集的值
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功/其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)，16位AD采集的值
//返回值:0,成功；其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
//获得四元素
u8 MPU_Get_Quaternion(float *quat)
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常； 其他,失败
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)

3.2.2 基于UCOSIII操作系统部分
程序是在UCOSIII操作系统中实现，建立了一个定时器，五个任务。定时器实现四元素更新的三子样算法和姿态解算，其优先级为2，堆栈大小为256；开始任务负责创建定时器和其他四个任务，其优先级为3，堆栈大小为128；任务一实现开关对数据上传控制，优先级为4，堆栈大小为256；任务二实现上传MPU6050输出数据和自己解算姿态到上位机，其优先级为5，堆栈大小为256；任务三实现MPU6050的四元素读取并显示到LCD上，其优先级为6，堆栈大小为128；任务四负责LCD数据和字符的显示，其优先级为7，堆栈大小为128。
Main函数中主要是初始化硬件资源和UCOSIII操作系统以及创建开始任务和开始UCOSIII操作系统。
int main(void)
{
OS_ERR err;
CPU_SR_ALLOC();

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
delay_init(168); //初始化延时函数
uart_init(500000); //初始化串口波特率为500000
LED_Init(); //初始化LED
KEY_Init(); //初始化按键
LCD_Init(); //LCD初始化
MPU_Init(); //初始化MPU6050
Show_Main_UI(); //在LCD上显式界面

OSInit(&err); //初始化UCOSIII
OS_CRITICAL_ENTER(); //进入临界区
//创建开始任务
OSTaskCreate((OS_TCB * )&StartTaskTCB, //任务控制块
(CPU_CHAR * )"start task", //任务名字
(OS_TASK_PTR )start_task, //任务函数
(void * )0, //传递给任务函数的参数
(OS_PRIO )START_TASK_PRIO, //任务优先级
(CPU_STK * )&START_TASK_STK[0], //任务堆栈基地址
(CPU_STK_SIZE)START_STK_SIZE/10, //任务堆栈深度限位
(CPU_STK_SIZE)START_STK_SIZE, //任务堆栈大小
(OS_MSG_QTY )0, //任务内部消息队列能够接收的最大消息数目,为0时禁止接收消息
(OS_TICK )0, //当使能时间片轮转时的时间片长度，为0时为默认长度，
(void * )0, //用户补充的存储区
(OS_OPT )OS_OPT_TASK_STK_CHK|OS_OPT_TASK_STK_CLR, //任务选项
(OS_ERR * )&err); //存放该函数错误时的返回值
OS_CRITICAL_EXIT(); //退出临界区
OSStart(&err); //开启UCOSIII
}
开始任务start_task( ),在创建了一个时钟和四个任务后将自己删除。
用一个全局的变量report来表示数据上传串口开关的状态，当report=1时表示开启上传，report=0是表示停止上传。开始默认是开启上传的，即report=1，当按键KEY0按下时翻转report的值，即可达到开启和关闭数据上传串口的目的。
if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);//用自定义帧发送加速度和陀螺仪原始数据
if(report)usart1_report_imu(aacx,aacyaacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
任务一的函数如下每隔100ms进行扫描一次。
void task1_task(void *p_arg)
{
OS_ERR err;
u8 key;
LED1=0;
while(1)
{
key=KEY_Scan(0);

if(key==KEY0_PRES)
{
report=!report;
if(report)
{
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
LED1=0;
}
else
{
LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
LED1=1;
}
}
OSTimeDlyHMSM(0,0,0,100,OS_OPT_TIME_HMSM_STRICT,&err); //延时100ms
}
}
对于在LCD上姿态角的显示，结果只精确到小数点后的一位。可以先定义一个短整型的temp，将要显示的姿态角负值给，它然后将其扩大10倍，分为整数部分和小数部分分别显示。任务二的服务函数如下。
void task2_task(void *p_arg)
{
OS_ERR err;
float pitch,roll,yaw; //欧拉角
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
short temp; //温度
u8 t=0;
while(1)
{
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);//用自定义帧发送加速度和陀螺仪原始数据
if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
if((t%10)==0)
{
if(temp<0)
{
LCD_ShowChar(30+48,200,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,200,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,200,temp/100,3,16); //显示整数部分
LCD_ShowNum(30+48+40,200,temp%10,1,16); //显示小数部分
temp=pitch*10;
if(temp<0)
{
LCD_ShowChar(30+48,220,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,220,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,220,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,220,temp%10,1,16); //显示小数部分
temp=roll*10;
if(temp<0)
{
LCD_ShowChar(30+48,240,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,240,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,240,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,240,temp%10,1,16); //显示小数部分
temp=yaw*10;
iftemp<0)
{
LCD_ShowChar(30+48,260,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,260,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,260,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,260,temp%10,1,16); //显示小数部分
t=0;
LED0=!LED0;//LED闪烁
}

}
t++;
OSTimeDlyHMSM(0,0,0,10,OS_OPT_TIME_HMSM_STRICT,&err); //延时10ms
}
}
由于四元素的四个数都是大于0小于1，所以借鉴前面的经验，只需要显示小数部分。将q扩大100000000倍后变成整数显示在LCD上。任务3的函数如下：
void task3_task(void *p_arg)
{
float quat[4]={0,0,0,0};
OS_ERR err;
CPU_SR_ALLOC();
OS_CRITICAL_ENTER();
LCD_ShowString(30,300,200,16,16,"Orinal Quaternion:");
POINT_COLOR=BLACK;//设置字体为黑色
//POINT_COLOR=BLUE;//设置字体为蓝色
LCD_ShowString(30,320,200,16,16,"q0=0.");
LCD_ShowString(30+100,320,200,16,16,"q1=0.");
LCD_ShowString(30+100*2,320,200,16,16,"q2=0.");
LCD_ShowString(30+100*3,320,200,16,16,"q3=0.");
OS_CRITICAL_EXIT();
while(1)
{
if(MPU_Get_Quaternion(quat)==0); //读取MPU6050 DMP处理后的数据
{
printf("四元素为q0=%f q1=%f q2=%f q3=%f\r\n",*quat,*(quat+1),*(quat+2),*(quat+3));
LCD_ShowxNum(70,320,quat[0]*10000000,7,16,0x80); //显示小数部分
LCD_ShowxNum(70+100,320,quat[1]*10000000,7,160x80); //显示小数部分
LCD_ShowxNum(70+100*2,320,quat[2]*10000000,7,16,0x80); //显示小数部分
LCD_ShowxNum(70+100*3,320,quat[3]*10000000,7,16,0x80); //显示小数部分
OSTimeDlyHMSM(0,0,0,10,OS_OPT_TIME_HMSM_STRICT,&err); //延时10ms
}
}
}
任务四负责LCD上显示一些字符，只执行一次。任务四的服务函数如下：
void task4_task(void p_arg)
{
POINT_COLOR=RED;//设置字体为红色
LCD_ShowString(30,340,200,16,16,"Clculate Quaternion:");
POINT_COLOR=BLACK;//设置字体为黑色
LCD_ShowString(30,360,200,16,16,"q0= .");
LCD_ShowString(30+100,360,200,16,16,"q1= .");
LCD_ShowString(30+100*2,360,200,16,16,"q2= .");
LCD_ShowString(30100*3,360,200,16,16,"q3= .");

LCD_ShowString(30,380,200,16,16,"pitch= . C");
LCD_ShowString(30,400,200,16,16," roll= . C");
LCD_ShowString(30,420,200,16,16," yaw= . C");
}
定时器定时进行50ms的周期定时，其优先级为2，是这几个任务中最高的，主要负责三子样旋转矢量算法。
三子样四元素更新算法的步骤为：
（1） 分别计算三个时间段的角增量，；
（2） 计算旋转矢量；
（3） 由旋转矢量构造增量四元素；
其中， 。
（4） 利用计算更新后的四元素；
（5） 利用更新后的四元素得到姿态变换矩阵。

为了实现上面的功能本实验做了如下的一些设计。MPU6050的陀螺仪输出是角速度，所以需要先积分得到角度，再用角度计算角增量。选取，计算角增量按下图4-2所示。

图4-2 角增量计算方法
由于定时器回调函数有点多，这里就不给出程序，具体程序可以见工程文件包中的main.c文件。
4实验
正对开机时刻的MPU6050的状态为初始状态。实验结果见下图4-1到4-5。

图4-1 开机后的状态

图4-2 LED灯的情况

图4-3 与四轴上位机连接

图4-4 与四轴上位机连接
有图4-1可见MPU6050的DMP计算得到的欧拉角还是比较有精度的。我的三子样旋转矢量算法的结果对比之还是有很多的不足，在静态情况还好，动态情况下就很糟糕。
5 总结
使用MPU6050自带数字运动处理DMP能很好的解算得到运动物体的姿态角。而使用三子样旋转矢量算法得到的运动物体的姿态角并不是很理想，原因是三子样旋转矢量算法使用的是角增量进行四元素的更新，而MPU6050的陀螺仪输出的是角速度，通过一个中值积分后误差就变大，结果变得不可控。虽然三子样旋转矢量法对抗圆锥运动有很好，但是三子样旋转矢量算法不适合输出为角速度的陀螺，而比较适合输出为角增量的陀螺。为解决这个问题后面应该采用方向余弦法或欧拉角法。