两轴舵机云台的一点实践

效果演示
硬件使用情况
定时器中断代码
计算机体系下的云台角代码
舵机控制代码
工程源码下载

简介：
为了学习姿态解算相关知识，最近做了一个作品，模仿炮塔跟随系统，用陀螺仪使两个舵机指向空间中的某一特定方向，实际上用欧拉角旋转矩阵法只完成了功能，然后针对舵机延迟问题做了一个微分控制。但由于欧拉角不能解算全姿态，实际能稳定的角度并不大， 将旋转矩阵换成四元数来表示之后解算结果不对，不知道是小弟哪里弄错了？东西很多纯属瞎扯，望大佬们轻喷。

效果演示

视频：两轴舵机云台

硬件使用情况

陀螺仪：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,hbpbhyincre = 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

参考资料：四元数姿态解算（来源网络，侵权请联系我删除，感谢作者整理）