空间姿态解算相关理解

基本知识点的理解

反三角函数的理解

atan2(y,x) (-pi,pi) 根据象限划分

atan(y/x) (-pi/2,pi/2)

矩阵的变换

左乘一个矩阵：相对于基坐标系下进行旋转或者确定新的基的方向

右乘一个矩阵：相对于当前坐标系下进行旋转或者确定新的基的方向

RPW相关理解

Roll（滚转）

绕Z轴旋转（Z轴不动）

Pitch（俯仰）

绕X轴旋转（X轴不动）

Yaw（偏航）

绕Y轴旋转（Y轴不动）

四轴保持稳定的控制方法

根据角度传感器得到滚转、俯仰、偏航角度，然后利用三组PID调节可以使得系统保持稳定（目标值均为0），得到的值是系统的反馈值

滚转

和俯仰类似

俯仰

一组电机速度相同，另一组电机一个速度增加，一个速度减少，即可实现控制

偏航

一组电机速度相同且速度较大，另一组电机速度相同且较小，力矩不同，实现绕Z轴旋转

MMA7361使用

接线

Sleep接3.3V，芯片是3.3V供电，一定要注意模块是否含有电压转换芯片！！

X、Y、Z接AD口，正常读数即可

MPU6050使用

数据获取平台

XS128

基本代码

I2C读取代码

x1=MPU6050GetData(ACCEL_XOUT_H);    //X轴输出值，待转化为角度
y1=MPU6050GetData(ACCEL_YOUT_H);    //
z1=MPU6050GetData(ACCEL_ZOUT_H);    //
gyroX = (MPU6050GetData(GYRO_XOUT_H)) / 131.0;
gyroY = (MPU6050GetData(GYRO_YOUT_H)) / 131.0;
gyroZ = (MPU6050GetData(GYRO_ZOUT_H)) / 131.0;

花费时间

1s内可以执行该段代码54次，即执行周期大约是18.51ms

反三角函数计算代码

gZ=MPU6050GetAngle( x1, y1, z1,0);//把得到的角度扩大十倍
gX=MPU6050GetAngle( x1, y1, z1,1);
gY=MPU6050GetAngle( x1, y1, z1,2);

其中函数定义如下：

double MPU6050GetAngle(float x,float y,float z,uchar dir)
{
float temp;
float res=0;
switch(dir)
{
case 0://与自然Z轴的角度
temp=sqrt((x*x+y*y))/z;
res=atan(temp);
break;
case 1://与自然X轴的角度
temp=x/sqrt((y*y+z*z));
res=atan(temp);
break;
case 2://与自然Y轴的角度
temp=y/sqrt((x*x+z*z));
res=atan(temp);
break;
}
return res*180.0/3.1415926;//把弧度转换成角度
}

花费时间

1s内执行该代码段5078次，相对于I2C读取时间来说几乎可以忽略不计