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. 电机速度闭环控制。
2. 底盘偏航角控制。
3. 万向轮底盘运动学反解。
二、本文内容概述本文较长， 大家可以根据内容概述挑选感兴趣部分进行阅读。
1. 硬件电路与接口软件设计。 将详细描述硬件电路的选型要求、 设备清单、连接方式、 接口软件代码几部分。
2. 速度检测与速度反馈控制。 将详细讲解如何通过Arduino检测电机转速，
简述如何设计速度PI控制器， 并用动图直观展示控制器的作用。
3. 万向轮底盘运动学方程讲解。 将掠过数学过程， 直接引用某文章的结论，
重点阐述如何利用arduino进行解算。
4. 偏航角的检测与控制。 重点介绍一个抗尖峰噪声的中值滤波算法， 简单
讲解一下控制器部分的设计思路。
三、 硬件电路与接口软件设计1. 硬件需求描述：
1.1 移动功能需求
鉴于我的项目需要让底盘带着垃圾桶快速的移动到目标点， 那么双轮式底盘
由于必须先转向再前进， 速度太慢， 无法完成这个需求。 鉴于此， 我选用
不算太贵的万向轮底盘作为此次的移动平台。 此平台最快速度为1.2m/s，
基本能够满足要求。 底盘如图：
图3底盘淘宝截图 图4电机参数
此外， 电机必须选择带编码器的电机， 否则是无法完成速度反馈的功能的。
刚好， 此底盘配的电机就是自带编码器的电机， 刚好解决了这两个问题。
图4描述的电机堵转电流为5.4A,我在设计电路时有两个方法避免电流过大。
1： 防止堵转， 增加电流反馈。 2： 使用超过5.4A电流的电机驱动。
由于怕麻烦， 我选择了后者。
图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个口。
这尼玛！！！！
{:5_163:}
不太够啊！！！！
{:5_174:}
好像模拟口也可以当数字口用啊， 这样应该加起来够用吧。。。。。
PWM只有四路这不够啊！！
{:5_199:}
这都难不住我， 呵呵， 我用mega2560和curienano一起来， 接口够了吧！
就这么决定了，mega负责底层控制， curienano负责接收无线信号， 检测陀螺仪数据，
然后统一把命令发送给mega， 由mega执行。 就酱。。。。。
饶厂长是不会倒在困难面前的！
{:5_196:}
2. 硬件设备清单
万向轮机器人底盘 X1
双路大电流电机驱动 X2
带编码器直流电机 X3
航模电池 X1
大电流开关 X1
curienano DFR0453 X1
Arduino mega2560 X1
wifi模块 X1
洞洞板 X1
杜邦线若干
3. 硬件电路连接原理图：
图7 硬件电路连接原理图
关于为什么这样接， 下面会有详细解释。
完成图：
开关加在电池之后。
图8 洞洞板制作的电机转接板
图9 整体俯视图
图10 curienano 与 wifi模块
图11 底盘的电机
图12 电路部分总览
图13 电机驱动接线
图14 负责底层控制的mega
到这里硬件部分介绍完毕。 说做的丑的， 额。。。。我会打个板子的， 嗯。
四、 电机速度检测与速度控制
1. 速度检测
首先， 速度的检测依赖于电机自带的编码器。 这款电机带的编码器是霍尔的， 但检测的方法和光电码盘
完全一样。 编码器的输出有两相， A相与B相。 当电机转动时， 转子带动磁码盘转动， 当磁片通过霍尔传感器时， 传感器输出高电平， 否则输出低电平。 这样霍尔传感器就会产生脉冲信号， 不断地在低电平和高电平之间转换。 A相和B相实际上就是两个相同的霍尔传感器输出端， 但A和B的安装位置很讲究， 保证了两个霍尔无法同时高电平。 这样就可以通过AB两相谁先高谁后高判断转向了。
同样的， 单位时间内产生的脉冲， 就是转速了。
图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函数里打印计数， 并清零计数， 完成速度检测部分内容。
下面看一下效果：
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));
下面看一下控制效果：
蓝色线为期望速度， 红色线为检测速度， 看起来效果还不错吧。
五、 运动学方程与反解
光是能够控制每个轮子的转速还不够啊。 我是要让底盘横着走， 竖着走， 旋转的。{:5_185:}{:5_168:}{:5_179:}
假设小车移动的地面是xy坐标系， 直观的理解为x方向速度， y方向速度， 小车自旋转速度w三个量，
因此我们需要一个映射函数：
= f();
v1 v2 v3分别是三个轮子的转速， vx，vy，w分别为x方向速度， y方向速度， 自转速度。
跳过数学推导， 引用一个文章结论：
图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;
}
以上为画圆形的控制代码。
下面看一下实现的效果：
吓得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;
// 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, MA02110-1301USA
*/
用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 = 0;
//Serial.println (buf);
theta = String(buf).toFloat();
window = window;
window = window;
window = theta;
//theta = midien(window,window,window);
pos = 0;
process_it = false;
}// end of flag set
}
通信部分我放到了loop里面， 查询执行。有同学问为什么不直接串口发送。
这个。。。。。。 我用串口调了一天都不行， 不知道为啥。。。
{:5_162:}
然后还IIC, 发现arduino101的库不支持从机模式。。。。
{:5_127:}
一怒之下换SPI了。
{:5_137:}
然后成功了。
看一下传过来的角度值波形：
图17 尖峰噪声
如图， 时不时的有尖峰噪声出现。 主要来自于传输丢个小数点什么的。 这个学过图像处理或信号处理的同学都知道， 搞一个
实时滑动窗， 对滑动窗内做中值滤波， 就能有效滤去尖峰噪声。 函数代码如下：
float window = {0.0, 0.0, 0.0};
float midien(float x1, float x2, float x3) {
float temp;
float tem;
int i,j;
temp=x1;
temp=x2;
temp=x3;
for(i=0;i<3;i++){
for(j=0;j<3-i-1;j++){
if(temp<temp){
tem=temp;
temp=temp;
temp=tem;
}
}
}
return temp;
}
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 = 0;
//Serial.println (buf);
theta = String(buf).toFloat();
window = window;
window = window;
window = theta;
theta = midien(window,window,window);
pos = 0;
process_it = false;
}// end of flag set
}
滤波过后效果如下， 完全去除了尖峰噪声。
图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));解决问题。
效果如下：
七、下期预告
下期我将分享一下如何配置vs2010+kinect+opencv开发环境， 然后如何实时监测底盘的空间坐标，
以及如何设计观测器融合底盘编码器的位置估测与视屏估测。 下期基本是纯数学内容了， 嗯， 这期就到这里， 欢迎回复！
期待回复！
期待贴个精华给我！
期待点赞！
饶厂长渴望回帖与交流！
快留言和我探讨！
luna 发表于 2017-5-31 13:56
66666 可以作为范文帖了！流比~~~~~~~
还有四期呢， 我会保持质量嗒{:5_128:} DFHJEmMoogH 发表于 2019-7-10 17:10
厂长您好，我想问两个问题，偏航检测是用于运动学解算吗，还有kinect用1.0可以吗 ...
偏航是平面机器人的重要状态，如果要做机器人的全状态控制，这个测量是必不可少的。几乎所有的机器人都有偏航角检测，否则定位、控制、规划相关算法都无法进行。kinect1.0还是2.0其实无所谓啦，但是我现在其实更推荐用双目做，这样可以避免kinect深度信息的噪声。 厂长您能帮看看如果直接买底盘电控还需要些什么
清单如下
兼容arduino mega 2560
转结板
a4950驱动模块*2
稳压模块
mpu6050模块
电量测量模块
oled显示屏
3s 3500mah航模电池
b3充电器
USB程序下载线
p2s手柄和接收器
。。。。。。。。
我是不是还需要另买curienano dfr0453和wifi模块 大赞!好详细啊!!!{:5_168:} nicho 发表于 2017-5-29 14:48
大赞!好详细啊!!!
上个月复刻了你一个体感猫耳， 大受好评 饶厂长 发表于 2017-5-29 14:54
上个月复刻了你一个体感猫耳， 大受好评
{:5_190:}开心~ 楼主技术大神，收藏了:handshake 厉害厉害厉害，学习了 沧海笑 发表于 2017-5-29 19:25
楼主技术大神，收藏了
:handshake 过奖啦， 共同努力。 hnyzcj 发表于 2017-5-29 19:45
厉害厉害厉害，学习了
太客气了。 您也是搞创客教育的吧， 看了您的一些中学生项目的帖子， 我到时候希望大大推广一下， 提前征求一下您的同意{:5_169:} nicho 发表于 2017-5-29 14:55
开心~
猫耳果然是大爱 66666 可以作为范文帖了！流比~~~~~~~{:5_151:} 赞赞赞 围观一下 这个帖子的水平很高啊！拜服。 已收藏:D 看看 本帖最后由 yahoocz 于 2017-6-1 16:36 编辑
代码编译成功，非常棒
好帖果断收藏，楼主流弊{:5_168:} 有意思，收藏。好帖 顶礼膜拜，楼主V5 膜拜大神！ 太赞了，而且这么详细，敬佩楼主！