首页 › Arduino › 关于四轴悬停请教大神们（板块如有冒犯请删帖）

camion 发表于 2013-5-13 14:41:34

关于四轴悬停请教大神们（板块如有冒犯请删帖）

首先检讨一下，该主题本应该发到四轴板块，但考虑到这边人气较多，所以。。。。


我的故事是这样的：刚刚第一个月拿到工资就到淘宝上买了无刷电机、电调、高倍率电池等等做四轴飞行器，现在快递还没到我用matlab做了一个四轴自平衡的控制仿真，用了PID算法（模糊逻辑也写了一个，对倾角自平衡控制也不错），然后出现了我自己技术能力有限的问题，只好来请教这边的神们。

我在以下代码中做了一对浆的自平衡，需要平衡的对象是 1 倾角 2 垂直速度 3 水平速度
控制量自己设的两个 F_basic(t) 和 F_rot(t)， 其中左浆升力大小=F_basic(t)+F_rot(t) ， 右浆升力= F_basic(t) - F_rot(t)

在我的模拟中，倾角 和 垂直速度 都可以在较短的时间内达到要求值（即平衡状态 0）， 但是由于有两个控制量和三个输出量，水平速度一直无法控制为零。

上代码 （scilab写的， 要在matlab中跑只需将 注释 // 改为%）

clear;
//basic para
L=0.5;
m=0.8;
J=2;
g=10;
delt_T=0.01;
N=400;

//consigne
ver_vit_standard = 0;
hori_vit_standard = 0;
angle_standard =0;
//registres
F_basic = zeros(1,N);
F_rot = zeros(1,N);
command_basic= zeros(1,N);
command_rot = zeros(1,N);

angle_acc = zeros(1,N);
angle_vit = zeros(1,N);
angle = zeros(1,N);

hori_acc = zeros(1,N);
hori_vit = zeros(1,N);
hori = zeros(1,N);

vert_acc = zeros(1,N);
vert_vit = zeros(1,N);
vert = zeros(1,N);

error_angle = zeros(1,N);
error_vert_vit = zeros(1,N);

sum_error_angle = zeros(1,N);
sum_error_vert_vit = zeros(1,N);

error_hori_vit = zeros(1,N);
sum_error_hori_vit = zeros(1,N);
//PID paras
P_basic=2;
I_basic=3.5;
D_basic=0.2;

P_rot=16;
I_rot=0.02;
D_rot=13;

// time 1 sample
F_rot(1)= 4.9;
F_basic(1) =0.1;

angle(1) =0.1;
angle_vit(1) = 0.2;
angle_acc(1) = F_rot(1)*L/J;
vert_vit(1) =1.2;
vert_acc(1) = ( 2*F_basic(1)*cos(angle(1)) - m*g )/m ;
error_angle(1) = angle_standard - angle(1);
error_vert_vit(1) = ver_vit_standard- vert_vit(1);
sum_error_angle(1) = error_angle(1)*delt_T;
sum_error_vert_vit(1) = error_vert_vit(1)*delt_T;
hori_acc(1) = ( 2*F_basic(1)*sin(angle(1)) )/m ;
hori_vit(1)=0;

//recurrence
for i=2:N
    F_rot(i)= command_rot(i-1);
    F_basic(i)= command_basic(i-1);
   
    angle_acc(i) = F_rot(i)*L/J;
    angle_vit(i) = angle_vit(i-1) + angle_acc(i-1)*delt_T;
    angle(i) = angle(i-1) + angle_vit(i-1)*delt_T;
    error_angle(i) = angle_standard - angle(i);
    sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
   
   
    vert_acc(i) = ( 2*F_basic(i)*cos(angle(i)) - m*g )/m ;
    vert_vit(i) = vert_vit(i-1) + vert_acc(i-1)* delt_T;
    vert(i) = vert(i-1) + vert_vit(i-1)*delt_T;
    error_vert_vit(i) =   ver_vit_standard - vert_vit(i);
    sum_error_vert_vit(i) = sum_error_vert_vit(i-1) + error_vert_vit(i)*delt_T;
   
    hori_acc(i) =( 2*F_basic(i)*sin(angle(i)) )/m ;
    hori_vit(i) = hori_vit(i-1) + hori_acc(i-1)*delt_T;
    hori(i) = hori(i-1) + hori_vit(i-1)*delt_T;
    error_hori_vit(i) = hori_vit_standard - hori_vit(i);
   
   
    command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
    command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
end

subplot(311);
plot (1:N, angle);
subplot(312);
plot (1:N, vert_vit);
subplot(313);
plot (1:N, hori_vit);


能否给出建议如何解决 ？ 如解决我很快就可以将控制算法写成arduino的c语言，卡尔曼滤波都是现成的

拾瑞 发表于 2013-5-13 20:11:13

太高深!不过四轴的开源项目很多的,APM和WMC都是开源的,你能直接参考一下啊

darkorigin 发表于 2013-5-13 23:28:02

说实话 这种极端专业的编程，坛子里估计能搞的定的不多
很多人都是半路出家甚至只是COPYER 连基本的数据结构 单片机都没学过。
连编译是个神马过程。机器码是神马都搞不懂的汉子 你指望能调试诸如PID算法神马的。哎~~~~
哎 当年的数据结构 MSC51 Z80 MASM全部还给老师了。惭愧啊~~~~
你的代码我也仅仅只能是看懂基本的。。。

darkorigin 发表于 2013-5-13 23:28:27

建议参考MWC大神的程序推敲一下~~

camion 发表于 2013-5-14 08:21:58

darkorigin 发表于 2013-5-13 23:28 static/image/common/back.gif
说实话 这种极端专业的编程，坛子里估计能搞的定的不多
很多人都是半路出家甚至只是COPYER 连基本的数据结 ...

其实我也是半路出家，一年前转成的程序猿啊
我再试试完全的模糊逻辑控制，PID缺点还有一点是需要调，在matlab里也是试了很多次组合才配到的，现实中配PID太需要经验和直觉了

johnsonzzd 发表于 2013-5-14 14:38:38

command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
    command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
理论上说，因为你的控制量算法里没有水平位置，必然控制不了水平位置。
实际上说，水平匀速运动是一种稳态，不会干扰角度。

camion 发表于 2013-5-14 19:14:33

johnsonzzd 发表于 2013-5-14 14:38 static/image/common/back.gif
command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*su ...

问题就在这里
command_basic（t）单独用来控制垂直速度
command_rot（t）单独控制角度

但是水平速度需要两者结合来控制，但是我无法将error_hori_vit结合到某个控制量去做单独PID，因为前两个控制量已经占用了
而且控制水平速度在某些情况下会要求倾角必须“先远离平衡位置”，降低水平速度后再平衡到零度，而这是实际command_rot的效果背离的

johnsonzzd 发表于 2013-5-15 17:09:14

多输入、多输出，当然用状态反馈。PID就不行。状态X，6维：角度、角速度、垂直位置、垂直速度、水平位置、水平速度。
控制量U，2维：command_basic、command_rot
U=KX

camion 发表于 2013-5-15 18:24:21

johnsonzzd 发表于 2013-5-15 17:09 static/image/common/back.gif
多输入、多输出，当然用状态反馈。PID就不行。状态X，6维：角度、角速度、垂直位置、垂直速度、水平位置、水 ...

那就是说要建立一个矩阵方程然后求微分解了？我现在暂时是一对桨所以维数少一点
还有想了一个变通的方式解决了该问题http://www.geek-workshop.com/thread-4584-1-1.html

johnsonzzd 发表于 2013-6-3 22:33:00

状态变量是测出来的，反馈系数是需要你确定的。直接计算控制量。和解方程无关。

学慧放弃 发表于 2013-11-7 21:03:14

darkorigin 发表于 2013-5-13 23:28 static/image/common/back.gif
说实话 这种极端专业的编程，坛子里估计能搞的定的不多
很多人都是半路出家甚至只是COPYER 连基本的数据结 ...

我就算法不行

darkorigin 发表于 2013-11-13 20:24:58

本帖最后由 darkorigin 于 2013-11-13 20:26 编辑

学慧放弃 发表于 2013-11-7 21:03 static/image/common/back.gif
我就算法不行

算法就是体现解决问题，把问题模型化的方式
算法不好的人也多，计算机专业的很多人也未必能达到算法很强的程度
这个和逻辑思维能力高数 离散数学水平有很大关系

要积累的，所以 多积累就好了，实在不行背一些常见算法 多读点程序，当然 学程序比较枯燥的

学慧放弃 发表于 2013-11-13 20:29:18

darkorigin 发表于 2013-11-13 20:24 static/image/common/back.gif
算法就是体现解决问题，把问题模型化的方式
算法不好的人也多，计算机专业的很多人也未必能达到算法很 ...

是啊，我就是多看数学，理解一些基本算法

学慧放弃 发表于 2013-11-13 20:31:44

弱弱的问句，我的六轴怎么用无刷控制平衡？

darkorigin 发表于 2013-11-15 23:55:10

学慧放弃 发表于 2013-11-13 20:29 static/image/common/back.gif
是啊，我就是多看数学，理解一些基本算法

谭浩强的 C语言版的《数据结构》也可以看看 几乎是计算机专业的必修书
谭教授的书影响着几乎所有科班的程序员。

查看完整版本: 关于四轴悬停请教大神们（板块如有冒犯请删帖）