查看: 63468|回复: 66

[讨论] Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）

本文多图，流量党当心了。
加载慢的话请耐心等候一下下。
前言： 饶厂长五月初申请试用了curie nano这块板子，扬言要制作一个
智能移动垃圾桶。但是项目太大， 30号前无法完成全部，因此先把目前
的进度做一个汇报分享。

首先介绍一下我希望制作的作品：
功能：能够自动接住空中飞来的垃圾。
实现框图：

图1 智能垃圾桶体统框图

整个项目分为几个部分：

1. kinect视觉定位，分析飞行物品的轨迹与落点。

2. kinect视觉定位，计算移动机器人的空间位置。

3. 小车的运动学解算，速度闭环控制，偏航方向检测。

4. 移动机器人的硬件电路。

由于是一个大项目，平常工作也比较忙，我今天这个文章就先分享一下3、4两部分的内容。

每做完一部分我就多发一张帖子。初步计划分为：底盘篇， kinect飞行轨迹预测篇， kinect环境搭建与小车位置检测篇，

wifi模块开发环境搭建篇，万向轮底盘位置闭环解耦控制篇。今天这一篇就是底盘篇，

也就是上面总框图的以下部分

图2 底盘部分控制框图

------------------------------------------正文分割线--------------------------------------

万向轮，又称Omni Wheel，具备万向轮的机器人底盘无需任何转动，就能向任何方向移动。除了万向轮外，麦克纳姆轮也具备同样的全向移动能力，但是这篇文章只讲解万向轮的相关知识，不会涉及麦克纳姆轮的相关内容。

万向轮虽然能够全向移动，但当创客们这正编程开始控制万向轮底盘时，往往显得手足无措。这是因为，万向轮的移动并不像两轮底盘，很容易理解和控制，万向轮的运动需要一些抽象的数学工具来描述。

在这篇文章中，我将从硬件底层开始讲起，带读者从0开始了解万向轮底盘的设计与控制，给读者提供切实可用可借鉴的知识和技巧，使读者们今后真正要用万向轮底盘制作机器人时，能够快速上手，解决核心问题。

一、底盘部分功能简述
底盘功能如图2所示，功能为：
1. 通过检测电机的速度，完成电机的闭环调速。
2. 通过检测偏航角和运动学反解，完成底盘的偏航角闭环控制。
3. 完底盘的运动学反解。

该功能动图如下所示：
1. 电机速度闭环控制。
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图17

2. 底盘偏航角控制。
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图22

3. 万向轮底盘运动学反解。
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图19

二、本文内容概述本文较长，大家可以根据内容概述挑选感兴趣部分进行阅读。
1. 硬件电路与接口软件设计。将详细描述硬件电路的选型要求、设备清单、连接方式、接口软件代码几部分。
2. 速度检测与速度反馈控制。将详细讲解如何通过Arduino检测电机转速，
简述如何设计速度PI控制器，并用动图直观展示控制器的作用。
3. 万向轮底盘运动学方程讲解。将掠过数学过程，直接引用某文章的结论，
重点阐述如何利用arduino进行解算。
4. 偏航角的检测与控制。重点介绍一个抗尖峰噪声的中值滤波算法，简单
讲解一下控制器部分的设计思路。

三、硬件电路与接口软件设计1. 硬件需求描述：
1.1 移动功能需求
鉴于我的项目需要让底盘带着垃圾桶快速的移动到目标点，那么双轮式底盘
由于必须先转向再前进，速度太慢，无法完成这个需求。鉴于此，我选用
不算太贵的万向轮底盘作为此次的移动平台。此平台最快速度为1.2m/s，
基本能够满足要求。底盘如图：
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图3

图3 底盘淘宝截图图4 电机参数
此外，电机必须选择带编码器的电机，否则是无法完成速度反馈的功能的。
刚好，此底盘配的电机就是自带编码器的电机，刚好解决了这两个问题。

图4描述的电机堵转电流为5.4A, 我在设计电路时有两个方法避免电流过大。
1：防止堵转，增加电流反馈。 2：使用超过5.4A电流的电机驱动。
由于怕麻烦，我选择了后者。
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图5

图5 驱动淘宝截图图6 驱动参数，为7A

这样就能够在电机堵转时也不会造成任何损坏了。

1.2 通信需求分析
curienano使用蓝牙进行通讯， BLE，非透传，这个不是很自由开放啊。而且对于运动控制来说
数据传输频率较高，果断放弃蓝牙，用TCP传输了。因此还要搞一个wifi模块， esp8266，因此在
官网买了一个。这个wifi模块的arduino开发，固件刷写我另一个帖子细讲。

1.3 占用板子资源分析
三路电机的调速需要 6个pwm口， 3个使能数字口，共9个口。
wifi模块通信需要2个数字口。
电机转速检测需要3个中断口， 3个数字口，共6个口。
调试需要串口， 0、1 我就空出来不动了，共2个口。
算下来一共要。。。。。 18个口。

这尼玛！！！！

不太够啊！！！！

好像模拟口也可以当数字口用啊，这样应该加起来够用吧。。。。。
PWM只有四路这不够啊！！

这都难不住我，呵呵，我用mega2560和curienano一起来，接口够了吧！
就这么决定了，mega负责底层控制， curienano负责接收无线信号，检测陀螺仪数据，
然后统一把命令发送给mega，由mega执行。就酱。。。。。
饶厂长是不会倒在困难面前的！

2. 硬件设备清单

万向轮机器人底盘 X1
双路大电流电机驱动 X2
带编码器直流电机 X3
航模电池 X1
大电流开关 X1
curienano DFR0453 X1
Arduino mega2560 X1
wifi模块 X1
洞洞板 X1
杜邦线若干

3. 硬件电路连接原理图：
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图7

图7 硬件电路连接原理图
关于为什么这样接，下面会有详细解释。

完成图：
开关加在电池之后。
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图10

图8 洞洞板制作的电机转接板

Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图9

图9 整体俯视图

图10 curienano 与 wifi模块
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图13

图11 底盘的电机

图12 电路部分总览
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图11

图13 电机驱动接线
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图12

图14 负责底层控制的mega

到这里硬件部分介绍完毕。说做的丑的，额。。。。我会打个板子的，嗯。

四、电机速度检测与速度控制
1. 速度检测
首先，速度的检测依赖于电机自带的编码器。这款电机带的编码器是霍尔的，但检测的方法和光电码盘
完全一样。编码器的输出有两相， A相与B相。当电机转动时，转子带动磁码盘转动，当磁片通过霍尔传感器时，传感器输出高电平，否则输出低电平。这样霍尔传感器就会产生脉冲信号，不断地在低电平和高电平之间转换。 A相和B相实际上就是两个相同的霍尔传感器输出端，但A和B的安装位置很讲究，保证了两个霍尔无法同时高电平。这样就可以通过AB两相谁先高谁后高判断转向了。
同样的，单位时间内产生的脉冲，就是转速了。
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图15

图15 光电码盘原理，图源自百度图片搜索

那么读取脉冲的数量该怎么做呢？方法有：
1. 查询式，不断地读引脚，产生变化后计数。
2. 中断式，产生中断后在中断函数中计数。
（3. 多线程，查询，不讨论）

用第一种方法的话，光测一路的速度还可以满足，现在有三路，还要运行控制代码，根本无法完成。
用第二种方法就比较棒了，刚好mega有多个中断口。我选用20，19，18三个数字口连接到A相输出。

对于B相，假如A高电平触发了中断，此时在函数中查询B相的状态，如果是高电平，则说明是B先触发A后触发，反之则是A先B后。
定义A先B后为正转方向，否则为反向。代码实现如下：

void setup() {
  Serial.begin(115200);
    attachInterrupt(2, counter_motor1, RISING);
  pinMode(A0, INPUT);
  attachInterrupt(3, counter_motor2, RISING);
  pinMode(A1, INPUT);
  attachInterrupt(4, counter_motor3, RISING);
  pinMode(A2, INPUT);
  FlexiTimer2::set(50, control); // 50ms period
  FlexiTimer2::start();

}
复制代码

这部分是setup配置代码。 attachInterrupt(2, counter_motor1, RISING); 这句话的意思就是打开2号外部中断，注意不是2号口哦，这是21号口，中断号和引脚对应如下。
0->2
1->3
2->21
3->20
4->19
5->18
counter_motor1这是中断进入的函数，详细见下文。 RISING是一个参数，表示从低电平到高电平才会触发中断。FlexiTimer2::set(50, control);FlexiTimer2::start();这两句的目的是打开一个定时中断，严格的每50ms运行一次control函数，这样能够保证系统的运行周期，保证控制的稳定性。

int counter1 = 0;
void counter_motor1() {
  // Serial.println("test");
  if (digitalRead(A0) == LOW) {
    counter1++;
  } else {
    counter1--;
  }
}
int counter2 = 0;
void counter_motor2() {
  // Serial.println("test");
  if (digitalRead(A1) == LOW) {
    counter2++;
  } else {
    counter2--;
  }
}

int counter3 = 0;
void counter_motor3() {
  // Serial.println("test");
  if (digitalRead(A2) == LOW) {
    counter3++;
  } else {
    counter3--;
  }
}
复制代码

这部分是中断函数。在函数里检测B相状态，满足正转条件则计数加，否则减。

 void control(){
Serial.print(counter1);Serial.print(' ');
Serial.print(counter2);
Serial.print(' ');
Serial.println(counter3);
 // Serial.println (theta);
  counter1 = 0;
  counter2 = 0;
  counter3 = 0;

}
复制代码

在control函数里打印计数，并清零计数，完成速度检测部分内容。

下面看一下效果：
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图16

2. 速度控制
有了速度信息我们就可以开始进行电机的PI控制了。关于控制理论和PID参数调节, 我是真的不想讲了。。。。。。
以后搞个专门控制的帖子详细描写系统辨识，参数选取，控制器设计等等，这次就不讲那么细了。
我就直接上代码了

int speed_loop1(int set, int state, float p, float i) {
  int e, u;
  static int sum_e;
  if (set >= 95) {
    set = 95;
  }
  if (set <= -95) {
    set = -95;
  }
  if (abs(state) < 10) {
    p = p * 2;
    //i = i*2;

  }
  e = set - state;
  sum_e += e;
  u = e * p + sum_e * i;

  return u;

}

int speed_loop2(int set, int state, float p, float i) {
  int e, u;
  static int sum_e;
  if (set >= 95) {
    set = 95;
  }
  if (set <= -95) {
    set = -95;
  }
  if (abs(state) < 10) {
    p = p * 2;
    //i = i*2;

  }
  e = set - state;
  sum_e += e;
  u = e * p + sum_e * i;

  return u;

}


int speed_loop3(int set, int state, float p, float i) {
  int e, u;
  static int sum_e;
  if (set >= 95) {
    set = 95;
  }
  if (set <= -95) {
    set = -95;
  }
  if (abs(state) < 10) {
    p = p * 2;
    //i = i*2;

  }
  e = set - state;
  sum_e += e;
  u = e * p + sum_e * i;

  return u;

}
复制代码

这些是三个轮子分别对应的控制器函数，是不是很简单。
control函数对应如下：

 run_motor1(speed_loop1(desire_speed1, counter1, 1, 0.9));
 run_motor2(speed_loop2(desire_speed2, counter2, 1, 0.9));
  run_motor3(speed_loop3(desire_speed3, counter3, 1, 0.9));
复制代码

下面看一下控制效果：

Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图25

蓝色线为期望速度，红色线为检测速度，看起来效果还不错吧。

五、运动学方程与反解
光是能够控制每个轮子的转速还不够啊。我是要让底盘横着走，竖着走，旋转的。

假设小车移动的地面是xy坐标系，直观的理解为x方向速度， y方向速度，小车自旋转速度w三个量，
因此我们需要一个映射函数：
[v1 v2 v3] = f([vx,vy,w]);
v1 v2 v3分别是三个轮子的转速， vx，vy，w分别为x方向速度， y方向速度，自转速度。
跳过数学推导，引用一个文章结论：
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图18

图16 非线性映射关系引用自《三轮全向移动机器人运动控制研究》 - 作者谢志诚

其中那个“fai”角就是电机之间的夹角，120度。
ok，带入进去，解算的函数代码实现如下：

float get_wheel_speed1(float vx, float vy, float w) {
  return vx * 0.8660 + vy * 0.5 + w * 0.15;
}

float get_wheel_speed2(float vx, float vy, float w) {
  return -vx * 0.8660 + vy * 0.5 + w * 0.15;
}

float get_wheel_speed3(float vx, float vy, float w) {
  return 0 + vy * -1 + w * 0.15;
}
复制代码

以上为函数映射的arduino实现。

int desire_speed = 80;
int desire_speed_y = 0;
void control() {
  static long int distance_motor1 = 0;
  int desire_speed1, desire_speed2, desire_speed3;
  int x_speed, y_speed, w;
  static float t = 0;
  t = t+0.2;
  desire_speed = 40*sin(t);
  desire_speed_y = 40*cos(t);
  distance_motor1 = distance_motor1 + counter1;
  w = 10*(0-theta);
  desire_speed1 = get_wheel_speed1(desire_speed, desire_speed_y, 0);
  desire_speed2 = get_wheel_speed2(desire_speed, desire_speed_y, 0);
  desire_speed3 = get_wheel_speed3(desire_speed, desire_speed_y, 0);

 run_motor1(speed_loop1(desire_speed1, counter1, 1, 0.9));
 run_motor2(speed_loop2(desire_speed2, counter2, 1, 0.9));
 run_motor3(speed_loop3(desire_speed3, counter3, 1, 0.9));
 
  // Serial.print( );
  //Serial.print(' ');
  //if(Serial2.available()){
  //  Serial.println(Serial2.read());
  // }
 // Serial.println(counter1);
   
 // Serial.print(counter2);
 //  Serial.print(' ');
 // Serial.println(counter3);
 // Serial.println (theta);
  counter1 = 0;
  counter2 = 0;
  counter3 = 0;

}
复制代码

以上为画圆形的控制代码。

下面看一下实现的效果：
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图23

吓得Jocker飞奔而逃。
在引入视觉位置检测反馈之前，这个xy方向移动还没什么用处。

六、偏航角的检测与反馈控制
ok，终于本期要写完了，写的我即将崩溃了。下面看一下怎么让CurieNano检测偏航角并发送给mega。

可能有朋友要问了：为什么不用罗盘？因为我不需要他工作很久，最多两分钟。因此不用罗盘问题不大。
下面的代码是CurieNano部分代码。

#include <SPI.h>

/*
 * Copyright (c) 2016 Intel Corporation.  All rights reserved.
 * See the bottom of this file for the license terms.
 */

/*
   This sketch example demonstrates how the BMI160 on the
   Intel(R) Curie(TM) module can be used to read gyroscope data
*/

#include "CurieIMU.h"

void setup() {
  //pinMode(13,OUTPUT);
  pinMode(2,INPUT);
  digitalWrite(2,HIGH);
  //digitalWrite(13,LOW);
  digitalWrite(SS, HIGH);
  SPI.begin ();              // PI通讯开始
  Serial.begin(9600); // initialize Serial communication
 // mySerial.begin(9600);
  //while (!Serial);    // wait for the serial port to open

  // initialize device
  Serial.println("Initializing IMU device...");
  CurieIMU.begin();

  // Set the accelerometer range to 250 degrees/second
  CurieIMU.setGyroRange(250);
}

void loop() {
  float gx, gy, gz; //scaled Gyro values
  static float z=0;
  char c;
  char buf[20];
  // read gyro measurements from device, scaled to the configured range
  CurieIMU.readGyroScaled(gx, gy, gz);
  gz = gz-0.525;
  // display tab-separated gyro x/y/z values
  z = z+gz*0.03;
  
  digitalWrite(SS, LOW);    // SS - pin 10
  dtostrf(z,3,2,buf);
  
  
  // 发送字串
  //digitalWrite(13,HIGH);
  for (const char * p = buf; c = *p; p++) {
    SPI.transfer (c);
    Serial.print(c);
    //buf = "";
  }
    c = '\n';
    SPI.transfer (c);
    Serial.print(c);
  // 取消从机
  digitalWrite(SS, HIGH);
  
  delay (30);
  //digitalWrite(13,LOW);
  
}

/*
   Copyright (c) 2016 Intel Corporation.  All rights reserved.

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.

   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

*/
复制代码

用SPI的方式连接mega， mega的spi引脚为50到53，对应连接为10->53, 11->51, 12->50,13->52。
mega这边的接受代码为：

void loop() {
  // By default, the last intialized port is listening.
  // when you want to listen on a port, explicitly select it:
  // desire_speed = desire_speed * -1;

  // Serial.println();
  
  
  if (process_it)
  {
    buf [pos] = 0;
    //Serial.println (buf);
    theta = String(buf).toFloat();
    window[2] = window[1];
    window[1] = window[0];
    window[0] = theta;
    //theta = midien(window[0],window[1],window[2]);
    
    
    pos = 0;
    process_it = false;
    
  }  // end of flag set


}
复制代码

通信部分我放到了loop里面，查询执行。有同学问为什么不直接串口发送。
这个。。。。。。我用串口调了一天都不行，不知道为啥。。。

然后还IIC, 发现arduino101的库不支持从机模式。。。。

一怒之下换SPI了。

然后成功了。

看一下传过来的角度值波形：
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图20

图17 尖峰噪声

如图，时不时的有尖峰噪声出现。主要来自于传输丢个小数点什么的。这个学过图像处理或信号处理的同学都知道，搞一个
实时滑动窗，对滑动窗内做中值滤波，就能有效滤去尖峰噪声。函数代码如下：

float window[3] = {0.0, 0.0, 0.0};

float midien(float x1, float x2, float x3) {
  float temp[3];
  float tem;
  int i,j;
  temp[0]=x1;
  temp[1]=x2;
  temp[2]=x3;
  for(i=0;i<3;i++){
      for(j=0;j<3-i-1;j++){
          if(temp[j+1]<temp[j]){
              tem=temp[j+1];
              temp[j+1]=temp[j];
              temp[j]=tem;
            }
        }
    }
   return temp[1];




}

void loop() {
  // By default, the last intialized port is listening.
  // when you want to listen on a port, explicitly select it:
  // desire_speed = desire_speed * -1;

  // Serial.println();
  
  
  if (process_it)
  {
    buf [pos] = 0;
    //Serial.println (buf);
    theta = String(buf).toFloat();
    window[2] = window[1];
    window[1] = window[0];
    window[0] = theta;
    theta = midien(window[0],window[1],window[2]);
    
    
    pos = 0;
    process_it = false;
    
  }  // end of flag set


}
复制代码

滤波过后效果如下，完全去除了尖峰噪声。
Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图21

图18 中值滤波后去除噪声。

最后的内容就是：用这个角度反馈控制角度！
没有别的，这样一个一阶系统，直接P控制，不解释，代码如下：

 w = 10*(0-theta);
desire_speed1 = get_wheel_speed1(desire_speed, desire_speed_y, w);
  desire_speed2 = get_wheel_speed2(desire_speed, desire_speed_y, w);
  desire_speed3 = get_wheel_speed3(desire_speed, desire_speed_y, w);

 run_motor1(speed_loop1(desire_speed1, counter1, 1, 0.9));
 run_motor2(speed_loop2(desire_speed2, counter2, 1, 0.9));
 run_motor3(speed_loop3(desire_speed3, counter3, 1, 0.9));
复制代码

解决问题。

效果如下：

Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）图24

七、下期预告
下期我将分享一下如何配置vs2010+kinect+opencv开发环境，然后如何实时监测底盘的空间坐标，
以及如何设计观测器融合底盘编码器的位置估测与视屏估测。下期基本是纯数学内容了，嗯，这期就到这里，欢迎回复！

期待回复！
期待贴个精华给我！
期待点赞！
饶厂长渴望回帖与交流！
快留言和我探讨！

trash_bin.rar

1.88 KB, 下载次数: 564

底层部分代码

[讨论]Intel curienano 硬件试用 之 智能垃圾桶： 第一期（底盘篇） 精华

[讨论] Intel curienano 硬件试用 之 智能垃圾桶： 第一期（底盘篇）

饶厂长 初级技匠 楼主|

饶厂长 初级技匠 楼主|

回帖奖励 +1 创造力

饶厂长 初级技匠 楼主|

饶厂长 初级技匠 楼主|

饶厂长 初级技匠 楼主|

硬件清单

楼主的其它帖子

[讨论]Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）精华

[讨论] Intel curienano 硬件试用之智能垃圾桶：第一期（底盘篇）

饶厂长初级技匠
楼主|

饶厂长初级技匠
楼主|

饶厂长初级技匠
楼主|

饶厂长初级技匠
楼主|

饶厂长初级技匠
楼主|