用Arduino扩展无飞行器续集（飞行器自主避障篇）

Leonardo

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


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



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）讨论。

TheBest

你好楼主，最近我也在做飞行器壁障，看你的代码上面没考虑到飞行器惯性的问题，我想问问你的壁障测试过吗？

丶晨默

大赞，学习下

丶晨默

要是实现一键自主飞行，是不是可以通过arduino直接输出PWM给AIL，ELE，THR，RUD来实现，自主解锁起飞，降落

Leonardo

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

完全可以啊，但是有可能飞走了

丶晨默

Leonardo魏宇科 发表于 2015-7-17 15:27
完全可以啊，但是有可能飞走了

用你的方法怎么实现呢？我们第一次用的analogWrite()，不知道啥原因飞控板烧了，，，，，现在用的延时高电平时间，没按螺旋桨测试了，可以自行解锁起飞。

sievent234

我也来贴一版本吧，注意安全就好。2560的板子。
[mw_shl_code=c,true]#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);
}[/mw_shl_code]

suoma

谢谢分享学习一下

suoma

是否有卡尔曼滤波？nano能使飞行器飞起来吗？

丶晨默

distance = pulseIn(EchoPin, HIGH , 8000) / 58.00;
8000微秒的超时时间一般能检测150mm的长度？
这个怎么算出来的，超声波测得距离不是高电平时间（us）/58么,那么8000/58大约是138吧？