视频： 两轴舵机云台
陀螺仪：MPU6050 *1
舵机：MG996R *2
主控芯片：STM32F103C8T6 *1
这部分代码使用TIM2定时器中断提供100Hz的控制频率，并且针对舵机位置控制延迟的问题，对舵机位置做了一个微分比例补偿控制，提高了响应速度。这部分代码在 main.c 里面。
// 全局变量声明
float pitch,roll,yaw; //欧拉角
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//四元数
float hy=0,hp=0; //参考系下云台角
float by,bp,br; //参考系下姿态角（也就是欧拉角）
float bhy=0,bhp=0,bhr=0;//机体系下云台角
float Kd1=10.0; //舵机1补偿系数
float Kd2=10.0; //舵机2补偿系数
/********************************************/
//定时器2中断服务程序 周期：10ms 频率：100Hz
void TIM2_IRQHandler(void) //TIM2中断
{
static int t=0;
u8 button;
float bhylast=0, bhplast=0; //机体系下上一时刻的云台角
float bhyincre=0, bhpincre=0; //机体系下上一时刻的云台角增量
float bhycomp = 0, bhrcomp = 0; //舵机转角度补偿量，与角速度有关
if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) //检查TIM2更新中断发生与否
{ TIM_ClearITPendingBit(TIM2, TIM_IT_Update ); //清除更新中断标志
bhylast=bhy, bhplast=bhp; //记录机体系下上一时刻的云台角
if(mpu_dmp_get_data(&roll,&pitch,&yaw)==0) //更新姿态角(由于安装问题实际roll和pitch互换了)
{
yaw=-yaw, pitch=-pitch, roll=-roll; //由于安装问题实际yaw,roll,pitch均倒向
CalHeadDegree(); //利用欧拉角旋转矩阵计算机体系下的云台角:hby,hbp
bhyincre = bhy - bhylast; //计算云台转动角速度
bhpincre = bhp - bhplast;
bhycomp = Kd1*bhyincre; //计算舵机1补偿量
bhrcomp = Kd2*bhpincre; //计算舵机2补偿量
HeadTurnToXY(bhy+bhycomp ,bhp+bhrcomp); //进行角度补偿输出
t++;
if(t>=10)t=0, LED0=!LED0; //每10*10=100ms闪动
}
}
}
这个函数将云台在参考坐标系内的指向向量旋转到机体系去，并且化为机体系下的云台角，注释掉的部分是用四元数表示的旋转矩阵，此部分放在 servo.c 里面。
/********************************************/
void CalHeadDegree(void) //计算机体系下的云台角
{
const float DEG2RAD = 0.0174533; //度化弧度因子
const float RAD2DEG = 57.29578 ; //弧度化度因子
float xn=cos(hy*DEG2RAD)*cos(hp*DEG2RAD); //参考系下的云台指向向量
float yn=sin(hy*DEG2RAD)*cos(hp*DEG2RAD);
float zn=sin(hp*DEG2RAD);
float xb,yb,zb,normalizer; //机体系下的云台指向向量、模长
float cby,sby,cbp,sbp,cbr,sbr; //姿态角的正余弦值
// printf("xn:%f\tyn:%f\tzn:%f\r\n",xn,yn,zn);
normalizer = invSqrt(xn*xn + yn*yn +zn*zn); //求出向量模长的倒数
xn *= normalizer; //向量单位化
yn *= normalizer;
zn *= normalizer;
bp=pitch,br=roll,by=yaw; //先算出姿态角的三角值方便计算
cby=cos(by*DEG2RAD),sby=sin(by*DEG2RAD);
cbp=cos(bp*DEG2RAD),sbp=sin(bp*DEG2RAD);
cbr=cos(br*DEG2RAD),sbr=sin(br*DEG2RAD);
// printf("yaw:%f\tpitch:%f\troll:%f\r\n",yaw,pitch,roll);
xb = cbp*cby*xn + cbp*sby*yn -sbp*zn;//套用欧拉角旋转矩阵公式
yb = (sbr*sbp*cby - cbr*sby)*xn + (sbr*sbp*sby + cbr*cby)*yn + sbr*cbp*zn;
zb = (cbr*sbp*cby + sbr*sby)*xn + (cbr*sbp*sby - sbr*cby)*yn + cbr*cbp*zn;
// printf("xb:%f\tyb:%f\tzb:%f\t",xb,yb,zb);
// xb = (q0*q0+q1*q1-q2*q2-q3*q3)*xn + 2*(q1*q2-q0*q3)*yn + 2*(q1*q3+q0*q2)*zn;//套用四元数旋转矩阵公式
// yb = 2*(q1*q2+q0*q3)*xn +(q0*q0-q1*q1+q2*q2-q3*q3)*yn + 2*(q2*q3-q0*q1)*zn;
// zb = 2*(q1*q3-q0*q2)*xn + 2*(q2*q3+q0*q1) *yn + (q0*q0-q1*q1-q2*q2+q3*q3)*zn;
// xb=yb, yb=-xb;
normalizer = invSqrt(xb*xb + yb*yb +zb*zb); //求出向量模长的倒数
xb *= normalizer; //向量单位化
yb *= normalizer;
zb *= normalizer;
// printf("normalizer:%f\r\n",normalizer);
//利用几何关系计算机体系下的云台角
bhp = asin(zb)*RAD2DEG;
bhy = acos( xb*invSqrt(xb*xb+yb*yb) )*RAD2DEG;
bhp = -bhp; //加上正负号
if(yb<0) bhy=-bhy;
// printf("bhp:%f\tbhy:%f\r\n",bhp,bhy);
}
以下函数用来控制舵机，使它们指向机体系下的一个方向 (hby,hbp)
水平角hby:-90°~90° 俯仰角hbp: 0°~180° ，放在 servo.c 里面。
/********************************************/
/************************************************/
void Servo1RunToDegree(float degree) //舵机1转到degree角度,degree:-90~90
{
float pwm;
if(degree>=-120 && degree<=120){
pwm = 1.0394*degree + 1848.55;
TIM_SetCompare2(TIM3,pwm); //修改比较值，修改占空比
}
}
void Servo2RunToDegree(float degree) //舵机2转到degree角度,degree:0~180
{
float pwm;
if(degree>=-30 && degree<=210){
pwm = -1.0306*degree + 1939.10;
TIM_SetCompare1(TIM4,pwm); //修改比较值，修改占空比
}
}
/************************************************/
void HeadTurnToXY(float x,float y) //云台转到水平x,俯仰y位置, x:-90~90, y:0~180
{
Servo1RunToDegree(x);
Servo2RunToDegree(y);
}
链接：https://pan.baidu.com/s/1f9QcTQm-TCXQcAJTKidhYg
提取码：rggw
参考资料： 四元数姿态解算 （来源网络，侵权请联系我删除，感谢作者整理）