


下载本文档
版权说明:本文档由用户提供并上传,收益归属内容提供方,若内容存在侵权,请进行举报或认领
文档简介
飞控的算法代码一般包括下面三个部分:滤波,姿态,PID1,滤波可以用互补滤波来实现,互补滤波的资料很多,大家随便就能找到。基本公式是:
2,滤波完就是四元数拉。直接用老外Madgwick的IMU就可以。超级简单unsignedcharBS004_IMU_Update(floatax,floatay,floataz,floatgx,floatgy,floatgz)
{
floatnorm;
floatvx,vy,vz;
floatex,ey,ez;
//
//圆点博士:四元数乘法运算
floatq0q0=q0*q0;
floatq0q1=q0*q1;
floatq0q2=q0*q2;
floatq1q1=q1*q1;
floatq1q3=q1*q3;
floatq2q2=q2*q2;
floatq2q3=q2*q3;
floatq3q3=q3*q3;
//
//圆点博士:归一化处理
norm=sqrt(ax*ax+ay*ay+az*az);
if(norm==0)return0;
ax=ax/norm;
ay=ay/norm;
az=az/norm;
//
//圆点博士:建立小四轴坐标系
vx=2*(q1q3-q0q2);
vy=2*(q0q1+q2q3);
vz=q0q0-q1q1-q2q2+q3q3;
//
//圆点博士:坐标系和重力叉积运算
ex=(ay*vz-az*vy);
ey=(az*vx-ax*vz);
ez=(ax*vy-ay*vx);
//
//圆点博士:比例运算
exInt=exInt+ex*bs004_quad_Ki;
eyInt=eyInt+ey*bs004_quad_Ki;
ezInt=ezInt+ez*bs004_quad_Ki;
//
//圆点博士:陀螺仪融合
gx=gx+bs004_quad_Kp*ex+exInt;
gy=gy+bs004_quad_Kp*ey+eyInt;
gz=gz+bs004_quad_Kp*ez+ezInt;
//
//圆点博士:整合四元数率
q0=q0+(-q1*gx-q2*gy-q3*gz)*bs004_quad_halfT;
q1=q1+(q0*gx+q2*gz-q3*gy)*bs004_quad_halfT;
q2=q2+(q0*gy-q1*gz+q3*gx)*bs004_quad_halfT;
q3=q3+(q0*gz+q1*gy-q2*gx)*bs004_quad_halfT;
//
//圆点博士:归一化处理
norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
if(norm==0)return0;
q0=q0/norm;
q1=q1/norm;
q2=q2/norm;
q3=q3/norm;
//
//圆点博士:欧拉角转换
bs004_imu_roll=asin(-2*q1q3+2*q0q2)*57.30f;
bs004_imu_pitch=atan2(2*q2q3+2*q0q1,-2*q1q1-2*q2q2+1)*57.30f;
bs004_imu_yaw=bs004_imu_yaw-gz*bs004_mpu6050_gyro_scale;
//
return1;
}3,PID的代码其实也很简单,主要是要了解其中的原理,才能更好地调整参数。为了方便新手们理解,楼主建立了一个数学模型来让大家了解。(只针对新手,老手就算了)========圆点博士小四轴之PID控制模式分析=======PID控制的P是Proportional的缩写,是比例的意思,I是Integral的缩写,是积分的意思,D是Derivative的缩写,是微分的意思。所以,PID就是我们常说的比例,积分,微分控制。我们首先来看一个PID控制模型曲线图:该图包含了比例控制,比例+积分控制,比较+积分+微分控制的电机响应图的对比。
下面我们对曲线进行具体分析:PID中的比例控制是最容易理解的,比例控制就是把角度的误差乘以一个常数作为输出驱动。假定我们有一个理想模型的电机,1V电压的变化会带来小四轴1度的角度改变。假定现在电机控制电压是5V,小四轴在某一轴上的偏角是5度,目标角度是100度。我们把当前的电压量定义为Vin,把输出控制量定义为Vout。假定P等于0.2,那么比例控制的结果就是:第一次:Vout=Vin+(100-5)*P=5V+19V=24V,得到电机电压是24V,对应的小四轴角度是24度,距离目标角度的误差是100-24=76度。第二次:Vout=Vin+(100-24)*P=24V+15V=39V,从而引起的角度是39度。我们看到,在这么的一个比例控制系统下,小四轴角度在慢慢地向目标角度靠近。PID中的积分控制就是把把所有角度误差相加起来,然后乘上一个常数作为输出驱动。在上述例子中,假定I=0.2,我们来看看比例和积分控制同时起作用下的系统反应。第一次:Vout=Vin+(100-5)*P+(100-5)*I=5V+19V+19V=43V,这时候小四轴角度为43度。由于第一次控制前的误差是100-5=95,第二次控制前的误差是100-43=57,所以积分结果是152。第二次:Vout=Vin+(100-43)*P+((100-5)+(100-43))*I=43V+11V+30V=84V,这时候小四轴角度变为84度。第三次:Vout=Vin+(100-84)*P+((100-5)+(100-43)+(100-84))*I=84+3V+33V=120V。这时小四轴角度变为120度。我们看到,在增加了积分控制后,小四轴角度在快速向目标角度靠近。PID中的微分控制就是把角度的变化乘上一个常数来作为电机驱动输出。在上述例子中,假定D=0.2,我们来看看比例,积分和微分共同控制下的系统反应。假定第一次前,电机转速保持5转,那么第一次前的角度变化为0。第一次:Vout=Vin+(100-5)*P+(100-5)*I-(5-5)*D=5V+19V+19V-0V=43V,这时候小四轴角度为43度。和上一次相比,角度从5度变化到了43度,所以小四周角度变化是43-5=38度。第二次:Vout=Vin+(100-43)*P+((100-5)+(100-43))*I-(43-5)*D=43V+11V+30V-7V=77V,这时候小四周角度77度。把上述的计算结果列出来,我们看到:
从上面的数据,我们可以看到:1,单独比例控制的时候,数据慢慢接近目标(图表中的红色线)2,加入积分控制之后,数据快速接近目标(图表中的蓝色线)3,微分控制起到抑制变化的作用。(图表中的绿色线)有了这些理论基础,就可以写PID控制代码拉。========圆点博士小四轴之PID控制代码分析=======在圆点博士小四轴2014版代码里,我们只使用到PD参数。首先我们来看PID中的比例控制。跟上一节模型提到的一样,比例是针对误差的控制。首先我们获取小四轴当前角度。bs004_angle_cur_pitch=bs004_imu_pitch;bs004_angle_cur_roll=bs004_imu_roll;把当前角度和目标角度相减,就可以得到角度偏差。bs004_angle_err_pitch=bs004_angle_cur_pitch-bs004_angle_target_pitch;bs004_angle_err_roll=bs004_angle_cur_roll-bs004_angle_target_roll;然后进行比例控制:bs004_fly_m1=bs004_fly_m1+bs004_pitch_p*bs004_angle_err_pitch-bs004_roll_p*bs004_angle_err_roll-bs004_yaw_p*bs004_angle_err_yaw;bs004_fly_m2=bs004_fly_m2-bs004_pitch_p*bs004_angle_err_pitch-bs004_roll_p*bs004_angle_err_roll+bs004_yaw_p*bs004_angle_err_yaw;bs004_fly_m3=bs004_fly_m3-bs004_pitch_p*bs004_angle_err_pitch+bs004_roll_p*bs004_angle_err_roll-bs004_yaw_p*bs004_angle_err_yaw;bs004_fly_m4=bs004_fly_m4+bs004_pitch_p*bs004_angle_err_pitch+bs004_roll_p*bs004_angle_err_roll+bs004_yaw_p*bs004_angle_err_yaw;在上一节模型中,我们提到PID中的微分控制针对的是角度变化而进行的控制。所以我们首先要得到当前角度和上一次角度的差异。bs004_angle_dif_pitch=bs004_angle_cur_pitch-bs004_angle_last_pitch;bs004_angle_dif_roll=bs004_angle_cur_roll-bs004_angle_last_roll;bs004_angle_dif_yaw
=bs004_angle_last_yaw-bs004_angle_cur_yaw;然后进行微分控制:bs004_fly_m1=bs004_fly_m1+bs004_pitch_d*bs004_angle_dif_pitch-bs004_roll_d*bs004_angle_dif_roll-bs004_yaw_d*bs004_angle_dif_yaw;bs004_fly_m2=bs004_fly_m2-bs004_pitch_d*bs004_angle_dif_pitch-bs004_roll_d*bs004_angle_dif_roll+bs004_yaw_d*bs004_angle_dif_yaw;bs004_fly_m3=bs004_fl
温馨提示
- 1. 本站所有资源如无特殊说明,都需要本地电脑安装OFFICE2007和PDF阅读器。图纸软件为CAD,CAXA,PROE,UG,SolidWorks等.压缩文件请下载最新的WinRAR软件解压。
- 2. 本站的文档不包含任何第三方提供的附件图纸等,如果需要附件,请联系上传者。文件的所有权益归上传用户所有。
- 3. 本站RAR压缩包中若带图纸,网页内容里面会有图纸预览,若没有图纸预览就没有图纸。
- 4. 未经权益所有人同意不得将文件中的内容挪作商业或盈利用途。
- 5. 人人文库网仅提供信息存储空间,仅对用户上传内容的表现方式做保护处理,对用户上传分享的文档内容本身不做任何修改或编辑,并不能对任何下载内容负责。
- 6. 下载文件中如有侵权或不适当内容,请与我们联系,我们立即纠正。
- 7. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。
最新文档
- 内蒙古鄂尔多斯市东胜区九年级化学下册 8.4 常见的盐(2)教学实录 (新版)粤教版
- 小学信息技术第一册 我的图册-用看图软件创建图册教学实录 河大版
- 22《 为中华之崛起而读书》(第二课时)(教学设计)2024-2025学年统编版语文四年级上册
- ma信号转dc信号并电阻
- 2025年阿片类中毒解毒药项目发展计划
- 电脑的整流电路
- 2025年烧伤整形科手术器械项目发展计划
- 2023六年级数学上册 四 解决问题的策略第3课时 解决问题的策略(练习课)教学实录 苏教版
- 2024年秋九年级历史上册 第4课《古代罗马》教学实录 中华书局版
- 制定数字营销转型策略计划
- 2025年沈阳职业技术学院单招职业技能测试题库附答案
- 建筑施工安全风险分级管控和隐患排查 治理双重预防机制实施细则
- 乡村规划理论与方法智慧树知到期末考试答案章节答案2024年长安大学
- 2022年湖南(土建)二级造价师考试题库汇总(含基础和实务)
- 人教版新课标六年级数学下册(4~6单元)重点知识归纳
- (完整版)Brownbear绘本
- 校园安全小品剧本-中学生安全小品剧本
- 高支模工程专项施工方案(专家论证)(完整版)
- 标准电极电势表(非常全)
- 驾驶员技师论文
- 室外道路及管网工程施工组织设计方案.doc
评论
0/150
提交评论