查看: 37552|回复: 21

[项目] 用Arduino扩展无飞行器续集(飞行器自主避障篇)

[复制链接]

该用户从未签到

发表于 2015-7-17 13:34 | 显示全部楼层 |阅读模式
本帖最后由 Leonardo魏宇科 于 2016-2-27 21:35 编辑

   在上一个帖子中(用Arduino扩展无人飞行器+普及无人机知识,不会算法的人必看 ) 我向大家介绍了多旋翼飞行器的基本知识和用Arduino扩展的方法,这一期我就向大家分享一个Arduino扩展无人机的重要应用:用超声波传感器实现自主臂章的功能。在多次试验中,飞机已经能实现单方向的自主避障。下面我先从硬件开始。


硬件:我采用了QQ飞控和一块Arduino nano作为扩展控制器,添加了一个sr04的超声波传感器,这样就组成了一个前方区域的探测控制闭环系统。 I90.jpg



code:
// uav.ino

#include<Servo.h>;
#define AIL1  5   //AIL fuyi
#define ELE2  6   //ELE shengjiang
#define LED   13
unsigned long INAIL;
unsigned long INELE;

int OUTAIL;
int OUTELE;

int delta = 0;

Servo AIL;
Servo ELE;

const int TrigPin = 2;
const int EchoPin = 3;
float distance;

void setup()
{        
  pinMode(AIL1,0);
  pinMode(ELE2,0);
  pinMode(LED,1);

  AIL.attach(9);
  ELE.attach(10);

  Serial.begin(9600);
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
}

void loop()
{
  sr();

  if(distance<=130)
  {
    delta = 130 - distance;
    delta = delta/4;
  }

  INAIL = pulseIn(AIL1, 1);
  INELE = pulseIn(ELE2, 1);

  OUTAIL = map(INAIL,1000,2000,47,144);
  OUTELE = map(INELE,1000,2000,47,144);

  OUTELE = OUTELE - delta;

  AIL.write(OUTAIL);
  ELE.write(OUTELE);

  Serial.print("AIL=");
  Serial.print(INAIL);
  Serial.print(" ELE=");
  Serial.print(INELE);
  Serial.print("   ");

   Serial.print("outail= ");
  Serial.print(OUTAIL);
  Serial.print(" outele= ");
  Serial.print(OUTELE);
  Serial.print("   ");

  Serial.print("delta= ");
  Serial.print(delta);
  Serial.print("   ");

  digitalWrite(LED,0);
  delay(5);

}

void sr()
{
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(LED,1);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);
  digitalWrite(LED,0);
  distance = pulseIn(EchoPin, HIGH , 8000) / 58.00;
  if(distance>=130 || distance==0)//如果distance大于等于130或等于0,都让distance赋值130
  {
    distance=130;
  }

  Serial.print(distance);
  Serial.print("cm");
  Serial.println();
  delay(50);
}

代码如有疑问可参考上一帖(用Arduino扩展无人飞行器+普及无人机知识,不会算法的人必看)
代码中的控制只选用了AIL 和ELE作为控制通道,独立了油门(THR)和方向通道,目的是若发生意外情况,飞手可操控油门通道让飞机停止工作,保证重要系统工作的独立稳定性。首先在sr()函数里进行了测距工作得出前方距离distance,注意distance为浮点全局变量;后对distance做了一次滤波:如果distance大于等于130或等于0,都让distance赋值130,为什么要这样做,我们来看主函数里的:
if(distance<=130)//如果小于等于130就让130-distance赋值给变量delta,然后再除以4
  {
    delta = 130 - distance;
    delta = delta/4;
  }

那么我们可以得到这样的结果:如果测出的距离是130或0那么变量delta就为0。如果距离是100,那么delata就是30,所以我的目的是给飞机一个0-130的雷达范围,当距离为130或者大于130时则系统认为是没有障碍物的,当有障碍物进入范围后,这个delta的作用就是测出障碍物进入雷达范围边缘的距离,也可以理解为超边缘距离,而后,将这个delta除以4,作用是给delta做一个比例P,这个比例P的大小为1/4即25%。最后,将delta作用在OUTELE = OUTELE - delta;这样就实现了飞机遇到障碍物后,会根据障碍物的远近做出避让,障碍物越近,那么delta越大,臂章动作越大,实现避开障碍的目的。而刚刚我说到测出的距离是0或大于130都要赋值130,现在你已经知道为什么大于等于130要赋值成130,但可能会问为什么distance等于0的时候也要赋值130,这里要和sr()函数里的
distance = pulseIn(EchoPin, HIGH , 8000) / 58.00;有关系,这里有一个8000微秒的超时时间,这个值确定了当脉冲反馈的时间大于这个值时,那么函数就认为超时,那么就会得出0的值,原因是距离太长后,在8000微秒的检测时间内脉冲还没有反馈回系统,就认为这个脉冲不会回来了,所以等价于距离过长也认为是大于等于130的条件值。8000微秒的超时时间一般能检测130mm-140mm的长度,超过就会是0,而我设置的130则是考虑一些外界干扰因素让判断范围更保守一些,这个道理相当于你用尺子去测一个1米左右的物体,你肯定更愿意选择1米5量程的尺子去。
此文讲解一个探测方向的实现方法和内容,全方位的探测系统道理一样,以此类推,但需要注意声波脉冲的相互干扰误判!


声明:文中的系统经过测试是有效的,但因各无人机控制系统差异(飞控品牌,电调、电机型号、结构等差异),此文的方法可能是无效或逆向的,需要根据自己的系统做一定修改和调试!建议上电之前在无桨的情况下多做测试,确定无误后再试飞。若发生生命安全损失或经济损失,与作者无关!多旋翼无人机危险性高,请爱好者谨慎开发!


分享就到这里,如果有问题欢迎回帖,希望大家可以分享多种方案去实现这一过程,或加入QQ群(67931507)讨论。
打赏作者鼓励一下!

该用户从未签到

发表于 2015-10-6 10:13 | 显示全部楼层
你好楼主,最近我也在做飞行器壁障,看你的代码上面没考虑到飞行器惯性的问题,我想问问你的壁障测试过吗?
  • TA的每日心情
    奋斗
    2015-8-13 02:13
  • 签到天数: 13 天

    [LV.3]偶尔看看II

    发表于 2015-7-17 14:41 | 显示全部楼层
    大赞,学习下
  • TA的每日心情
    奋斗
    2015-8-13 02:13
  • 签到天数: 13 天

    [LV.3]偶尔看看II

    发表于 2015-7-17 15:18 | 显示全部楼层
    要是实现一键自主飞行,是不是可以通过arduino直接输出PWM给AIL,ELE,THR,RUD来实现,自主解锁起飞,降落

    该用户从未签到

     楼主| 发表于 2015-7-17 15:27 | 显示全部楼层
    丶晨默 发表于 2015-7-17 15:18
    要是实现一键自主飞行,是不是可以通过arduino直接输出PWM给AIL,ELE,THR,RUD来实现,自主解锁起飞,降落 ...

    完全可以啊,但是有可能飞走了
    打赏作者鼓励一下!
  • TA的每日心情
    奋斗
    2015-8-13 02:13
  • 签到天数: 13 天

    [LV.3]偶尔看看II

    发表于 2015-7-17 16:35 | 显示全部楼层
    Leonardo魏宇科 发表于 2015-7-17 15:27
    完全可以啊,但是有可能飞走了

    用你的方法怎么实现呢?我们第一次用的analogWrite(),不知道啥原因飞控板烧了,,,,,现在用的延时高电平时间,没按螺旋桨测试了,可以自行解锁起飞。
  • TA的每日心情
    郁闷
    2015-4-12 13:21
  • 签到天数: 2 天

    [LV.1]初来乍到

    发表于 2015-7-17 18:53 | 显示全部楼层
    我也来贴一版本吧,注意安全就好。2560的板子。

    kittenblock中小学创客名师推荐的图形化编程软件

    #include<Servo.h>//The Arduino Mega has an additional four: numbers 2 (pin 21), 3 (pin 20), 4 (pin 19), and 5 (pin 18).
    int ppm1 = 2;
    int ppm2 = 3;
    unsigned long rc1_PulseStartTicks, rc2_PulseStartTicks;
    volatile int rc1_val, rc2_val;
    int OUTAIL, OUTELE; //输出
    ////////////////////////////超声波值///////////////////
    int distance1 = 300, distance2 = 300, distance3 = 300, distance4 = 300;
    //////////////////////////超声波针脚定义///////////////
    int input1 = 22, input2 = 24, input3 = 30, input4 = 32;
    int output1 = 23, output2 = 25, output3 = 31, output4 = 33;
    Servo AIL;
    Servo ELE;
    void setup()
    {
      // 电平变化即触发中断
      attachInterrupt(0, rc1, CHANGE);
      attachInterrupt(1, rc2, CHANGE);
      ////////////////////////////////
      AIL.attach(8);
      ELE.attach(9);
      pinMode(ppm1, INPUT);
      pinMode(ppm2, INPUT);
      pinMode(input1, INPUT);
      pinMode(output1, OUTPUT);
      pinMode(input2, INPUT);
      pinMode(output2, OUTPUT);
      pinMode(input3, INPUT);
      pinMode(output3, OUTPUT);
      pinMode(input4, INPUT);
      pinMode(output4, OUTPUT);
        Serial.begin(9600);
    }
    void rc1()
    { // did the pin change to high or low?
      if (digitalRead( ppm1 ) == HIGH)
        rc1_PulseStartTicks = micros();    // store the current micros() value
      else
        rc1_val = micros() - rc1_PulseStartTicks;
    }
    void rc2()
    {
      // did the pin change to high or low?
      if (digitalRead( ppm2 ) == HIGH)
        rc2_PulseStartTicks = micros();
      else
        rc2_val = micros() - rc2_PulseStartTicks;
    }
    void loop() {
    
      Sonar();//超声波函数
      Avoidance (); //避障函数
      OUTAIL = map(rc1_val, 1010, 2007, 47, 144);
      OUTELE = map(rc2_val, 1010, 2007, 47, 144);
      AIL.write(OUTAIL);
      ELE.write(OUTELE);
      Print();//串口输出
    }
    void Sonar ()
    {
      //第一个超声波/前
      digitalWrite(output1, LOW);
      delayMicroseconds(2);
      digitalWrite(output1, HIGH);
      delayMicroseconds(10);
      distance1 = pulseIn(input1, HIGH);
      distance1 = distance1 / 58;
      distance1 = constrain(distance1, 10, 300);//限制量程
      delay(2);
      //第二个超声波/后
      digitalWrite(output2, LOW);
      delayMicroseconds(2);
      digitalWrite(output2, HIGH);
      delayMicroseconds(10);
      distance2 = pulseIn(input2, HIGH);
      distance2 = distance2 / 58;
      distance2 = constrain(distance2, 10, 300);
      delay(2);
      //第三个超声波/左
      digitalWrite(output3, LOW);
      delayMicroseconds(2);
      digitalWrite(output3, HIGH);
      delayMicroseconds(10);
      distance3 = pulseIn(input3, HIGH);
      distance3 = distance3 / 58;
      distance3 = constrain(distance3, 10, 300);
      delay(2);
      //第四个超声波/右
      digitalWrite(output4, LOW);
      delayMicroseconds(2);
      digitalWrite(output4, HIGH);
      delayMicroseconds(10);
      distance4 = pulseIn(input4, HIGH);
      distance4 = distance4 / 58;
      distance4 = constrain(distance4, 10, 300);
      delay(2);
    }
    void Avoidance ()
    {
      if (distance2 <= 80)
      { //前进,后有障碍物
        if ((OUTELE <= 97) && (OUTELE >= 93))
        {
          ELE.write(102);
        }
        else
        { int PTELE1; int NOW_ELE1;
          PTELE1 = 95.5 - OUTELE;
          abs (PTELE1);
          NOW_ELE1 = OUTELE + PTELE1;
          ELE.write(NOW_ELE1);
        }
      }
      //////////////////////////////////////////////////////////////////
      if (distance1 <= 80)
      { //后退,前有障碍物
        if ((OUTELE <= 97) && (OUTELE >= 93))
        {
          ELE.write(87);
        }
        else
        {
          int PTELE2;
          int NOW_ELE2;
          PTELE2 = 95.5 - OUTELE;
          abs (PTELE2);
          NOW_ELE2 = OUTELE - PTELE2;
          ELE.write(NOW_ELE2);
        }
      }
      //////////////////////////////////////////////////////////////////
      if (distance4 <= 80)
      { //左飞,右有障碍物
        if ((OUTAIL <= 97) && (OUTAIL >= 93))
        {
          AIL.write(102);
        }
        else
        { int PTAIL3;
          int NOW_AIL3;
          PTAIL3 = 95.5 - OUTAIL;
          abs (PTAIL3);
          NOW_AIL3 = OUTAIL + PTAIL3;
          AIL.write(NOW_AIL3);
        }
      }
      ////////////////////////////////////////////////////////////////////
      if (distance3 <= 80)
      { //右飞,左有障碍物
        if ((OUTAIL <= 97) && (OUTAIL >= 93))
        {
          AIL.write(87);
        }
        else
        {
          int PTAIL4;
          int NOW_AIL4;
          PTAIL4 = 95.5 - OUTAIL;
          abs (PTAIL4);
          NOW_AIL4 = OUTAIL - PTAIL4;
          AIL.write(NOW_AIL4);
        }
      }
    }
    void Print()
    {
      Serial.print("INAIL=");
      Serial.print(rc1_val);
      Serial.print(" INELE=");
      Serial.print(rc2_val);
      Serial.print(" OUTAIL=");
      Serial.print(OUTAIL);
      Serial.print(" OUTELE=");
      Serial.print(OUTELE);
      Serial.print(" distance1=");
      Serial.print(distance1);
      Serial.print(" distance2=");
      Serial.print(distance2);
      Serial.print(" distance3=");
      Serial.print(distance3);
      Serial.print(" distance4=");
      Serial.print(distance4);
      Serial.println();
      delay(5);
    }
  • TA的每日心情
    开心
    2018-6-16 16:25
  • 签到天数: 777 天

    [LV.10]以坛为家III

    发表于 2015-7-18 19:21 | 显示全部楼层
    谢谢分享学习一下
  • TA的每日心情
    开心
    2018-6-16 16:25
  • 签到天数: 777 天

    [LV.10]以坛为家III

    发表于 2015-7-18 19:23 | 显示全部楼层
    是否有卡尔曼滤波?nano能使飞行器飞起来吗?
  • TA的每日心情
    奋斗
    2015-8-13 02:13
  • 签到天数: 13 天

    [LV.3]偶尔看看II

    发表于 2015-7-19 15:22 | 显示全部楼层
    distance = pulseIn(EchoPin, HIGH , 8000) / 58.00;
    8000微秒的超时时间一般能检测150mm的长度?
    这个怎么算出来的,超声波测得距离不是高电平时间(us)/58么,那么8000/58大约是138吧?
    您需要登录后才可以回帖 登录 | 立即注册  

    本版积分规则

    热门推荐

    KittenBot杯第六届开源硬件开发大赛启动啦
    KittenBot杯第六届开源硬
    大赛简介: 第六届开源硬件开发大赛由Arduino中文社区发起 由KittenBot冠名赞助
    Blinker 模拟传感器+数字传感器例程(DHT11 温湿度+气体传感)
    Blinker 模拟传感器+数字
    才接触1星期,发现Blinker自带了很多例程,已经成功试验了DHT的例程序,并完善了体
    一文教你选择Arduino开发板,小白进
    一文教你选择Arduino开发
    笔者在2016年接触了Arduino,跳了万条坑,行了千里弯,到今天也算是Arduino的一
    [发布帖]RPG游戏 无人世界
    [发布帖]RPG游戏 无人世界
    视频攻略:https://www.bilibili.com/video/av43790997/ (b站正在审核) 剧情:主角是
    【人脸识别】ESP-EYE 最新的板子到货了
    【人脸识别】ESP-EYE 最新
    1.我不会编程,最多抄几个一步一步的网络教程。 2.不知道为啥就是超级喜欢电子元件。
    Copyright   ©2015-2016  Arduino中文社区  Powered by©Discuz!   ( 蜀ICP备14017632号-3 )
    快速回复 返回顶部 返回列表