四轴飞行器姿态解算超声波定高光流定点

只看该作者 · 2015-8-3 23:38

本帖最后由 xiongcaifei 于 2015-8-4 12:35 编辑

和大家分享下我做四轴的经验，参加电子竞赛时候接触到四轴飞行器，目前还在继续做。先上最新进展视频

四轴飞行器的前后、左右、顺时针和逆时针运动是通过姿态角（角度）来控制的，遥控器是用来设定目标姿态角的，只要测得的姿态与设定的目标姿态进行串级PID控制就可以使四轴飞行器稳定飞行了。所以做四轴最重要、最关键的就是姿态角测量和串级PID控制。

姿态结算：[size=13.63636302947998px]测量姿态的传感器主要是三轴加速度计、三轴陀螺仪和三轴磁力计。整个姿态描述的原理是：通过解算地理坐标系和机体坐标系的角度位置关系来得到姿态，由于传感器的测量误差（主要是陀螺仪的积分误差和振动引起的加速度计误差），导致测得的机体坐标系不准，得到的姿态也就会不准确。因为地理坐标系中的四轴飞行器所受的重力和磁场是个常量，所以将地理坐标系中的重力向量和磁场向量转换到机体坐标系中，此时转换到机体坐标系的重力向量和磁场向量与机体坐标系中测出来的重力向量和磁场向量会有误差，只要消除此误差，就可以校正机体坐标系，进而得到准确的姿态。消除误差的方法有卡尔曼滤波法、互补滤波法、姿态插值法等。考虑到计算能力和现有的资料，本设计采用互补滤波法，达到的效果也比较好。

[size=13.63636302947998px]姿态解算算法步骤：

[size=13.63636302947998px]姿态解算涉及到坐标的变换，还有互补滤波算法部分。该算法的实现步骤为：

[size=16.363636016845703px]

姿态解算C语言算法具体实现：

norm =Q_rsqrt(ax*ax + ay*ay + az*az); //加速度计数据归一化

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+ VariableParameter(ex) * ex * Ki;//对误差积分

eyInt = eyInt+ VariableParameter(ey) * ey * Ki;

ezInt = ezInt+ VariableParameter(ez) * ez * Ki;

gx = gx + Kp* VariableParameter(ex) * ex + exInt; //补偿陀螺仪

gy = gy + Kp* VariableParameter(ey) * ey + eyInt;

gz = gz + Kp* VariableParameter(ez) * ez + ezInt;

q0 = q0 +(-q1*gx - q2*gy - q3*gz)*halfT; //四元数的微分方程

q1 = q1 +(q0*gx + q2*gz - q3*gy)*halfT;

q2 = q2 +(q0*gy - q1*gz + q3*gx)*halfT;

q3 = q3 +(q0*gz + q1*gy - q2*gx)*halfT;

norm =Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3); //四元数归一化

q0 = q0 *norm;

q1 = q1 *norm;

q2 = q2 *norm;

q3 = q3 *norm;

angle.pitch =asin(-2*q1q3 + 2*q0q2); //四元数转换成欧拉角

angle.roll =atan2(2*q2q3 + 2*q0q1, -2*q1q1 - 2*q2q2 + 1);

串级PID控制：姿态角PID+姿态角速度PID

姿态角速度就是陀螺仪测量的数据，为了使四轴飞行器稳定飞行，应该使飞行器姿态稳定的时候，姿态速度也应为零，故采用角度和角速度串级PID控制器。由于姿态速度响应的速度比姿态响应的速度快，四轴飞行器飞行的时候引起的姿态速度变化也比姿态速度变化大，故姿态速度控制作为内环，姿态控制作为外环，也就是角度外环和角速度内环组成的串级PID控制器。

串级PID具体C语言代码：

if(ctrl.ctrlRate >= 2) //内环进行2次控制、外环进行1次控制、内环控制频率为外环2倍

{

//*****************外环PID**************************//

//俯仰计算//

pit=pit- (Rc_Data.PITCH - Rc_Data.pitch_offset)/30;

ctrl.pitch.shell.increment+= pit; //俯仰方向误差积分

//积分限幅

if(ctrl.pitch.shell.increment> ctrl.pitch.shell.increment_max)

ctrl.pitch.shell.increment= ctrl.pitch.shell.increment_max;

elseif(ctrl.pitch.shell.increment < -ctrl.pitch.shell.increment_max)

ctrl.pitch.shell.increment= -ctrl.pitch.shell.increment_max;

ctrl.pitch.shell.pid_out= ctrl.pitch.shell.kp * pit + ctrl.pitch.shell.ki * ctrl.pitch.shell.increment+ ctrl.pitch.shell.kd * (pit - pitch_old);

pitch_old= pit; //储存俯仰偏差

//横滚计算//

rol=rol- (Rc_Data.ROLL - Rc_Data.roll_offset)/30;

ctrl.roll.shell.increment+= rol; //横滚方向误差积分

//积分限幅

if(ctrl.roll.shell.increment> ctrl.roll.shell.increment_max)

ctrl.roll.shell.increment= ctrl.roll.shell.increment_max;

elseif(ctrl.roll.shell.increment < -ctrl.roll.shell.increment_max)

ctrl.roll.shell.increment= -ctrl.roll.shell.increment_max;

ctrl.roll.shell.pid_out = ctrl.roll.shell.kp * rol +ctrl.roll.shell.ki * ctrl.roll.shell.increment + ctrl.roll.shell.kd * (rol -roll_old);

roll_old= rol; //储存横滚偏差

//航向计算////////////

ctrl.yaw.shell.pid_out = ctrl.yaw.shell.kp * (Rc_Data.YAW -Rc_Data.yaw_offset)/10 + ctrl.yaw.shell.kd * sensor.gyro.origin.z;

yaw_old=yaw;

ctrl.ctrlRate = 0;

}

ctrl.ctrlRate++;

//*************内环(角速度环)PD****************************//

ctrl.roll.core.kp_out= ctrl.roll.core.kp * (ctrl.roll.shell.pid_out + sensor.gyro.radian.y *RtA);

ctrl.roll.core.kd_out= ctrl.roll.core.kd * (sensor.gyro.origin.y - sensor.gyro.histor.y);

ctrl.pitch.core.kp_out= ctrl.pitch.core.kp * (ctrl.pitch.shell.pid_out + sensor.gyro.radian.x * RtA);

ctrl.pitch.core.kd_out= ctrl.pitch.core.kd * (sensor.gyro.origin.x - sensor.gyro.histor.x);

ctrl.yaw.core.kp_out= ctrl.yaw.core.kp * (ctrl.yaw.shell.pid_out + sensor.gyro.radian.z * RtA);

ctrl.yaw.core.kd_out= ctrl.yaw.core.kd * (sensor.gyro.origin.z - sensor.gyro.histor.z);

ctrl.roll.core.pid_out= ctrl.roll.core.kp_out + ctrl.roll.core.kd_out;

ctrl.pitch.core.pid_out= ctrl.pitch.core.kp_out + ctrl.pitch.core.kd_out;

ctrl.yaw.core.pid_out= ctrl.yaw.core.kp_out + ctrl.yaw.core.kd_out;

ctrl.roll.core.pid_out、ctrl.pitch.core.pid_out和ctrl.yaw.core.pid_out为电机输出值。

超声波定高：

超声波定高就是测量高度数据与设定数据进行PID控制，在此I需要限幅，高度控制输出值代替油门输出即可。定高1米，变化范围不超过5厘米

光流定点：先上视频下次帖子将为大家介绍请尽情期待。

[size=16.363636016845703px][size=13.63636302947998px]四轴飞行器交流群200718960 希望大家互相交流互相提高更多资料去菜鸟飞行器

[size=16.363636016845703px][size=13.63636302947998px]分享一下最好的一篇四轴飞行器论文

只看该作者 · 2015-8-5 11:53

本帖最后由 xiongcaifei 于 2015-8-5 12:16 编辑

cuiweimy 发表于 2015-8-4 22:22
mark

大家一起交流共享经验资料吗，希望可以多多支持菜鸟飞行器:D

只看该作者 · 2015-8-4 12:37

程序和文件视频无法弄上去，更多资料去。视频在优酷搜菜鸟飞行器。程序在群200718960

只看该作者 · 2015-8-4 14:36

大神膜拜一下

只看该作者 · 2015-8-4 22:22

mark

只看该作者 · 2015-8-5 11:53

本帖最后由 xiongcaifei 于 2015-8-5 12:14 编辑

风清云淡发表于 2015-8-4 14:36
大神膜拜一下

大家一起学习提高嘛:D

只看该作者 · 2015-8-12 13:31

看看。

只看该作者 · 2015-9-8 15:26

技术贴！要顶！

只看该作者 · 2015-11-9 22:40

不错的东西

1万 · 2015-11-12 09:55

xiongcaifei 发表于 2015-8-4 12:37
程序和文件视频无法弄上去，更多资料去。视频在优酷搜菜鸟飞行器。程序在群200718960 ...

积分提高到50分就可以发件了.

只看该作者 · 2016-1-4 12:52

加油。

只看该作者 · 2016-1-4 21:05

谢谢楼主分享

只看该作者 · 2016-1-5 17:14

好东西一定要好好消化消化

只看该作者 · 2018-4-12 15:49

好东西一定要好好消化消化

四轴飞行器 姿态解算 超声波定高 光流定点

相关下载

相关帖子

技术新星奖章

技术奇才奖章

以坛为家

四轴飞行器姿态解算超声波定高光流定点