5iMX.com 我爱模型玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）

标题: 【DIY】超声波定高功能，解放你的油门，让你飞得很无聊【41楼更新爬楼梯视频】 [打印本页]

作者: auuuux2011 时间: 2011-5-8 00:05
标题: 【DIY】超声波定高功能，解放你的油门，让你飞得很无聊【41楼更新爬楼梯视频】
最近飞四轴让我很是痛苦，因为我用飞固定翼的方式来控制油门，四轴就像兔子一样上窜上跳。等XAircraft 出超声波功能等了很久也没出来，还是自己动手写一个通用的超声波模块吧，估计不会很难。

想了一下需求大概是这样的：

1. 可以由单片机完全控制油门，我只控制四轴的水平运动就行。
2. 可以用控的一个2段开关，来开启和关闭超声波定高，开启后，四轴将悬停在当前的高度。
3. 用另外一个开关，能自动起飞，自动降落。
4. 能兼容大部分的四轴飞控(FS1212，KK，AQ50D之类)，因为接管了接收机的油门通道

在开始之前我设想了一些问题，后来在实际测试中进行了验证和修正。

1.超声波是否会被电机干扰？
实际测试后，超声波并不会被电机发出的声音影响。但很容易被紧贴在旁边的电池反射产生了干扰，所以超声波传感器得找一个周围至少有2-3cm间隔的地方放置。尽管如此，超声波在临界点时返回的数据一定会有噪点，我们得用kalman 过滤一次。

2.pid 如何实时查看output 。
方案一，用电脑上的 processing写一个波形显示程序。方案二，用anroid 写一个手机上的示波器，android sdk2以上可以使用蓝牙SDK，再用一片20元的蓝牙芯片放在飞控上，就可以让飞控和手机通讯了。

3. PID 中比例，微分，积分的整定。
这三个参数需要在外场实际测试中调试，通讯链路是双向的，自订个协议设置参数没多大问题。遇到的困难是，在一些极端参数下，估计会产生巨大的不可控震荡，简单的说四轴会拔地而起，或者重重的高速撞击地球。暂时没想到好的办法防止这些情况，用绳子拴着？

4.pid output 用什么策略映射到 pwm
方案一是直接将输出映射到1000-2000 的脉宽，这样手动的油门将不参与控制
方案二，基于任何时候都不能相信机器人的理由，要将当前的油门值进行数据融合

5.开启pid 的策略
用遥控器的一个2档开关，开关拨到开启时，记录这瞬间的pwm和高度，作为setpoint。

6.超声波的异常处理，距离检测到 0 ，或很大的情况，距离突然变化的情况。
超声波必须要做卡尔曼滤波，在这些特殊情况下应该关闭定高功能，自动缓慢降落。

[ 本帖最后由 auuuux2011 于 2011-5-16 12:54 编辑 ]

作者: auuuux2011 时间: 2011-5-8 00:07
算法早已写完，连续两个星期都没有时间去外场实际验证效果， kp ，ki ，kd 在家基本没法整定。
今天虽然有些小雨，也去了华农体育场，带了手提电脑和无线串口，直接用无线来修正单片机上的参数，大概调整了10来组，就已经很好飞了。我的控是美国手，四轴是x450，最后我的左手完全解放了，只用右手控制四轴前后左右就行了。帮我一起调整参数的朋友cn，从来没有飞过航模，开启锁定航向+超声波定高，让他飞了十来分钟下来说：太没挑战了，太无聊了。话没说完就倾盆暴雨，只好收工回家，改日再战。

(, 下载次数: 49)

(, 下载次数: 39)

经过实际外场测试，整定出PID 参数如下，震荡已经很小了。

#define IMU_SAMPLE_RATE 1.0/16 // 16k Hz
Kalman kalman_ym(IMU_SAMPLE_RATE, 0.002 , 0.003 , 0.001  ); //油门卡尔曼
Kalman kalman_dt(IMU_SAMPLE_RATE, 0.002 , 0.003 , 0.001  ); //高度卡尔曼
int pinCSBin = 10;
int pinCSBout = 11;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, 2  ,  0.5 ,  1 , DIRECT); // 最后整定出来的kp,ki,kd

(, 下载次数: 42)

这是运行在电脑上，用无线进行调整pid参数，这样就不用重新编译和上传程序到单片机了。
绿色线是目标高度
红色线是当前高度
蓝色是经过pid运算后修正的油门值

[ 本帖最后由 auuuux2011 于 2011-5-8 00:25 编辑 ]

作者: auuuux2011 时间: 2011-5-8 00:15
再上一个高清视频，几乎整个过程都是cpu在推油。

接下来，上制作教程：程序也分享给大家，你可以随意下载烧录。

一。首先你得去taobao 以下器件：
1. arduino uno/nano/mini/pro 都可以。（我一开始用了一片stm32f103RB，但这开发板挺贵而且没有电子基础的人upload都很困难，觉得没必要了，换成最容易买到的avr）
2.超声波传感器（一搜一大把，几十块钱吧，我用的是5米距离的）

二。然后将arduino 的pin 2 接到接收机的 3通道（油门），将pin3 接到接收机的5通道（2段开关）。将arduino 的pin4 （油门输出）接到飞控的1通道（油门输入）

三。下载程序，用usb烧录到芯片里去。（程序到Q群74639945共享里下载）

四。有必要的话调整一下参数，不调就用我设定好的。

然后，然后，打开care free+ 超声波定高，你会觉得飞四轴变得很无聊。

[ 本帖最后由 auuuux2011 于 2011-5-8 09:34 编辑 ]

作者: dstdx72 时间: 2011-5-8 00:17
:em04: 这个强，视频呢？

作者: dstdx72 时间: 2011-5-8 00:19
:em22: 神马CPU？卡尔曼，算得过来不？PID速率？

作者: 兔哥 时间: 2011-5-8 00:35
KK上可以用吗?

作者: ZDB5873 时间: 2011-5-8 03:19
好!

作者: auuuux2011 时间: 2011-5-8 09:26
to: dstdx72
芯片是atmel avr 168 ，16khz 。中断采样，做了2次卡尔曼，1次闭环pid ，从效果看肯定比人脑+眼睛算得快

to：兔哥
理论上兼容所有四轴飞控，包括KK ，因为cpu只是根据高度帮你控制油门，不涉及飞控本身。

作者: zhen_sr 时间: 2011-5-8 10:07
顶了——楼主！
好样的！
能不能出个详细的教程？我们照样学来？

作者: 兔哥 时间: 2011-5-8 10:55

原帖由 auuuux2011 于 2011-5-8 09:26 发表

to：兔哥
理论上兼容所有四轴飞控，包括KK ，因为cpu只是根据高度帮你控制油门，不涉 ...

什么时候出成品或套材吗?

我搞一套

作者: dpcom 时间: 2011-5-8 14:05
能飞个航线来看看吗

作者: Nokia82 时间: 2011-5-8 14:23
:em26: :em15:

作者: code_asf 时间: 2011-5-8 14:32
这个应该加精。

作者: mapgps 时间: 2011-5-8 15:09
> "一个2段开关，来开启和关闭超声波定高"
这样是否更方便:
当油门杆回正后, 就自动触发超声波定高. 省了那个2段开关.
也就是说, 用油门杆位置来作为速度的大小: 这样就能控制: 匀速上升(正速度) / 定高悬停(零速度) / 匀速下降(负速度).

[ 本帖最后由 mapgps 于 2011-5-8 15:15 编辑 ]

作者: yyqop01 时间: 2011-5-8 20:14
楼主厉害，我是那天的美国手200，很敬佩DIYer，:em26: :em26:

作者: 531475118 时间: 2011-5-8 21:33
:em15:

作者: yyqop01 时间: 2011-5-8 21:43

原帖由 dong3x 于 2011-5-8 15:07 发表
让他飞了十来分钟下来说：太没挑战了，太无聊了。话没说完就倾盆暴雨，只好收工回家，改日再战。

你的DIY，人神共愤啊，小心呀。

那天很大雨

作者: longway7721 时间: 2011-5-8 23:41
:em26: 强人，上详细流程吧，造福广大模友，感觉可以用于直机了？

作者: 咖啡猪 时间: 2011-5-9 10:29
太强悍了！

作者: auuuux2011 时间: 2011-5-9 10:53

原帖由 mapgps 于 2011-5-8 15:09 发表
> "一个2段开关，来开启和关闭超声波定高"
这样是否更方便:
当油门杆回正后, 就自动触发超声波定高. 省了那个2段开关.
也就是说, 用油门杆位置来作为速度的大小: 这样就能控制: 匀速上升(正速度) / 定高悬停(零速 ...

我也在头疼自动起飞，自动降落，定高怎么做切换，用3段开关都不够。
油门要参与数据融合的，不能拿来做开关。

作者: coconut 时间: 2011-5-9 11:28
LZ动手能力很强！:em26:

作者: 9imx 时间: 2011-5-9 12:00
期待楼主的教程

作者: auuuux2011 时间: 2011-5-9 12:29

原帖由 coconut 于 2011-5-9 11:28 发表
LZ动手能力很强！:em26:

嘿嘿，我动手能力很弱的，基本上装机，零件，结构都是朋友维护的，我只写算法和验证

[ 本帖最后由 auuuux2011 于 2011-5-9 12:30 编辑 ]

作者: auuuux2011 时间: 2011-5-9 12:33

原帖由 9imx 于 2011-5-9 12:00 发表
期待楼主的教程

我感觉我的帖子里已经把要买什么零件，怎么接线，都说得很清楚了呀，如果还有不明白的，提问吧。

作者: 空军8号 时间: 2011-5-9 12:55
这帖子不顶不行啊，肯定有地方能用的上！~！

作者: lxd811 时间: 2011-5-9 13:15
整个完整的教程吧。大家学习学习。

作者: 胶凤 时间: 2011-5-10 23:14
ding

作者: 兔哥 时间: 2011-5-11 08:07

原帖由 auuuux2011 于 2011-5-9 12:33 发表

我感觉我的帖子里已经把要买什么零件，怎么接线，都说得很清楚了呀，如果还有不明白的，提问吧。

能出套材吗?

作者: 敌敌畏2009 时间: 2011-5-11 09:28
强烈支持楼主出套件、造福模友、:em26: :em26:

作者: fujianshu 时间: 2011-5-11 12:45
我画的图，有错的地方还请指正，谢谢

作者: 小飞手 时间: 2011-5-14 23:40
观望高科技哈

作者: mapgps 时间: 2011-5-15 17:02

原帖由 fujianshu 于 2011-5-11 12:45 发表
我画的图

超声波传感器的电源应接Arduino Nano的5V针脚, 而不是VIN针脚 (7~12V).

作者: fujianshu 时间: 2011-5-15 21:04
楼主老大告诉我接vin而不是5v，，，求正解

作者: 兔哥 时间: 2011-5-15 22:18
超声波模块的距离只有5米,是否就是说飞行高度在5米以内才能定高?

有没有可达到100米高度的超声波模块?

我现在用KK,基本的航向稳定度已经可以了,就是高度很难控制,如果这套东西能在100米的高度范围内起作用,就可以用KK四轴来FPV了.

作者: auuuux2011 时间: 2011-5-16 10:06

原帖由 fujianshu 于 2011-5-15 21:04 发表
楼主老大告诉我接vin而不是5v，，，求正解

vin 就是外接电源，没有经过稳压的。
5v 是用板载LDO 将vin 稳压成 5v 的工作电压
本来的确应该按照mapgps 推荐的方法接的。
但由于我们供电给nano 是由电调-》接收机供电的，已经是稳定的5v 了，所以不需要经过板载LDO再稳压一次。而且另外一个考虑是超声波需要的电流可能比较大，再从nano取电可能会影响nano。

作者: fujianshu 时间: 2011-5-16 10:39
嗯嗯，完全理解了

作者: auuuux2011 时间: 2011-5-16 10:47

原帖由 fujianshu 于 2011-5-11 12:45 发表
我画的图，有错的地方还请指正，谢谢

多谢付总的图，很形象，很直观了

作者: 兔哥 时间: 2011-5-16 11:00
将超声波模块换成气压计的话,是否就可以任意高度定高了?

作者: ptzh5730 时间: 2011-5-16 11:06
强帖！

作者: auuuux2011 时间: 2011-5-16 12:35
改进了算法，下面是测试视频

超声波定高爬楼梯，全程油门自动控制。

来个更高难度的，宽两米，很陡的楼梯

作者: mapgps 时间: 2011-5-16 13:21

原帖由 auuuux2011 于 2011-5-16 10:06 发表

vin 就是外接电源，没有经过稳压的。
5v 是用板载LDO 将vin 稳压成 5v 的工作电压
本来的确应该按照mapgps 推荐的方法接的。
但由于我们供电给nano 是由电调-》接收机供电的，已经是稳定的5v 了，所以不需要经过板载LDO再稳压一次。而且另外一个考虑是超声波需要的电流可能比较大，再从nano取电可能会影响nano。

那就要将: 电调-》接收机输出5V 和 超声波供电 都接在Nano的5V针脚上. 而VIN针脚悬空.
Nano有三种供电方式: USB / 5V针脚 / VIN针脚.
VIN针脚供电方式时, 输入电压范围为6~20V, 推荐7~12V. 并在5V针脚产生5V输出.
如果将电调-》接收机输出5V 接VIN针脚, 经LDO后Nano实际得到的电压将低于5V. 你可以测一下5V针脚看看.

作者: auuuux2011 时间: 2011-5-16 14:15
mapgps 说的有道理，电源全接到5v ， vin 悬空，这样最好。

如果全接到vin，貌似LDO后低于5v 一点点，芯片也能run的很好，不过mapgps 的办法是最保险的。单片机和超声波都能得到足够的电压。

作者: longkouzhang 时间: 2011-5-18 09:40
没有文化好可怕啊，看的晕头转向的，没看明白。

作者: fujianshu 时间: 2011-5-18 13:30
我采纳mapgps建议，谢谢

作者: wildkatze 时间: 2011-5-18 14:45
mark一下，kk的福音啊
.
.

作者: gg05 时间: 2011-5-18 17:13
高人在民间啊
为什么那么多厂家没人琢磨出来啊

作者: yensun 时间: 2011-5-22 20:35
請問一下那個參數設置的程式,要怎麼執行?

作者: 拎壶冲 时间: 2011-5-23 12:53
不错:em26:

作者: wjdbj 时间: 2011-5-23 22:55
标题: 没有文化果然很悲剧，硬是看不懂
我只能看看热闹而已:em25:

作者: 兔哥 时间: 2011-5-26 22:16
DDDDDDDDDDDD

作者: yyqop01 时间: 2011-5-26 22:50
定的不错，加油，吧旋转的也做出来

作者: sunliang 时间: 2011-5-27 18:23
好东西，能不能给做出来用在直机上？

作者: microhard567 时间: 2011-5-30 20:29
1.顶LZ，制作的东西漂亮，理论也很正确，算法也不错，值得学习。
2.超声波的有效高度太低了，LZ用的超声头（应该是40KHZ的那种吧）一般最多不超过15米，另外，建议楼主用两组，甚至4组传感器，再采用融合算法得出高度，这样也许能在较低高度内进行姿态控制。
3.LZ高人，是否能制作出一套无线电高度仪，再和气压高度仪配合使用
以上小弟愚见。

作者: hpg_xx 时间: 2011-11-15 07:01
:em26: arduino uno :em26: 楼主

作者: wjdbj 时间: 2011-11-15 08:49
标题: 强大的帖子留名
:em26:

作者: 竹蜻蜓的天空 时间: 2011-11-15 09:31
先顶后看。

作者: 芙蓉姐夫 时间: 2011-11-15 16:18
泥马的都是牛淫不顶对不住党！

作者: neki 时间: 2011-11-15 16:55
LZ，你的群已经满员啦，能将资料公布出来吗？

欢迎光临 5iMX.com 我爱模型玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年） (http://5imx.com/)

5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）

5iMX.com 我爱模型玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）