lesson 3: 代码实现—整体架构

一、整体说明

Diego 3dof的代码是移植的开源3D打印机项目Marlin的源代码,读者感兴趣可以访问Marlin项目github获取其源代码,Marlin项目由于要考虑适配不同的控制器,所以代码比较复杂,这里对其做了大幅的修改删减,已符合Diego 3dof的控制要求,修改后的文件结构如下图:

修改后一共剩余12个文件,且可以在Arudion UNO上运行,资源占用情况如下所示:

资源占用情况比较理想,如果资源占用情况比较高的情况下Arduino IDE会预警,可能在运行中有不可预知的不稳定现象。

同时由于diego3轴码垛和3D打印机的逆解不同,也做了相应的修改,后面会详细说明。

二、文件功能说明

1. Configuration.h 和Configuration_adv.h

这两个头文件中主要是针对运动学逆解,及马达控制的一些默认配置。

2. pin.h

这个头文件中,定义了Arduino的pin功能的对应关系

3. diego.h

这个头文件中定义了通用的函数及宏定义

4. planner.h 和 planner.cpp

这两个文件中主要是针对速度规划的函数、数据结构及运动学逆解运算。

5. stepper.h 和 stepper.cpp

这两个文件中主要是针对马达的控制算法

6.motion_control.h 和 motion_control.cpp

这两个文件中定义了运动规划的相关函数及数据结构

7. speed_lookupdate.h

这个文件中定义了在算法中用到的速度查询数组

8. diego_3dof.ino

arduino项目的主文件,功能包含,对串口命令的接收、g代码的解析、代码的初始化和主循环。

Marlin的代码在网上有非常多的资源可以参考,在Diego-3dof中主要修改了其配置文件中Pin的定义,及码垛的逆解算法,下一章中将介绍逆解算法的实现。
源代码请见https://github.com/diegorobot/diego-3dof

 

 

Lesson 4: 3轴码垛逆解模型及实现

对于3轴码垛机械臂控制最基本的是对其建立运动学模型,而对于3轴码垛类型机械臂来说运动学模型,其本质就是给定空间3D坐标,求解3个轴的旋转角度。

 

如上图所示,左侧为实物坐标,右侧图为抽象到坐标系的几何表示,逆解过程就是知道末端坐标,而求解各个轴的旋转角度,进而转换为步进电机的步进数。上图中涉及到5个点:

O点:坐标原点,也就是机械臂底盘原点

A点:是两个电机安装位置轴心点,此点的Z轴坐标为已知的固定量

B点:两个活动臂的连接点

C点:臂末端点

D点:末端工具安装点

E点:末端工具的工作点,逆解就是知道这点的坐标,求3个马达的选择角度

下面我们利用立体几何,和解析几何知识来进行你运算分析。

一,假设条件

  • 坐标系采用右手坐标系,如上图所示
  • 机械臂的底座位于右手坐标系的XY平面
  • 底座旋转轴的位置,即为坐标系的原点位置,即上图做所视的O点
  • 我们把AB线所表示臂称之为大臂,BC线所表示的臂称之为小臂
  • 点O,A,B,C,D,E始终处于同一平面,此平面与Y轴的夹角为α
  • OA线与AB延长线之间的夹角为β
  • AB线与CB线延长线之间的夹角为γ
  • 工具安装位置CD始终与XY平面平行
  • 工具安装点与末端工作点的直线DE始终垂直于XY平面
  • 机械臂的初始位置为A,B,C三点的Y轴坐标为O,B,C点收回到距离Z轴最近的位置
  • 初始位置α=0
  • 已知条件:OA,AB,BC,CD,DE长度
  • OA与XY平面之间的夹角θ为固定值

二、数学求解

已知E点的坐标(x,y,z)基, 于以上假设条件,求解α,β,γ。

1.求解α

c点坐标为(cx,cy,cz),与E点的坐标是简单的直角坐标系对应关系

α为整个机械臂投影到XY轴平面的投影线与X轴的角度,可以直接根据E点的坐标使用正切函数计算出α

tan(α)=y/x

α=atan(y/x)

由于在机械臂的运动范围内x坐标可能为0,所以需要对x=0的坐标点做特殊的处理,详见代码

2.求解γ

γ是三角形ABC,角ABC沿AB延长线的补角,所以只要求出角ABC,即可求出γ。角ABC只需要采用三角函数即可以求得,所以求解步骤为:

2.1 求A点的坐标(ax,ay,az)

ax=OA*cos(θ)*sin(α)

ay=OA*cos(θ)*cos(α)

az=OA*sin(θ)

2.2求解三角形AC边长

AC=sqrt((cx-ax)(cx-ax)+(cy-ay)(cy-ay)+(cz-az)(cz-az))

2.3求解γ

γ=180-acos((AB*AB+ BC*BC- AC*AC) / (2 * AB* BC))

3.求解β

三角形ABC的的BAC角是可以根据三角函数求出来,γ在OA的斜率大于AC的斜率时γ等于角BAC减去直线OA和AC的夹角

3.1.求解角ABC

ABC=acos((AB*AB+ AC*AC- BC*BC) / (2 * AB* AC))

3.2.求解OA的斜率

Koa=sin(θ) / cos(θ);

3.3.求解AC的斜率

Kac= (c_y – a_y) / (c_x – a_x);

3.4.求解OA和AC的夹角

OA_AC=atan(|Koa-Kac|/(1-Koa*Kac))

3.5.求解β

如果Koa>Kac ,β=ABC-OA_AC

如果Koa<Kac ,β=ABC+OA_AC

 

4.代码实现

逆解部分的都在planner.cpp文件的plan_buffer_line函数中

 ///////////////diego_3dof kinemitacs///////////////////////
  ///alpha  oa_map_xy to y
  float alpha = 0;
  if(y==0){///////////in the x line
    if(x>0){
      alpha=PI/2;
      c_x=x-DEFAULT_CD_LENGTH_MM;
    }else if(x<0){ alpha=-PI/2; c_x=x+DEFAULT_CD_LENGTH_MM; } }else{ float tan_alpha=x/y; if(tan_alpha==0){///in the y line if(y>0){
        alpha=0;
        c_y = y - DEFAULT_CD_LENGTH_MM;
      }else{
        alpha=PI;
        c_y = y + DEFAULT_CD_LENGTH_MM;
      }
    }else if(tan_alpha>0){
      if(y>0){
        alpha=atan(tan_alpha);
      }else{
        alpha=atan(tan_alpha)-PI;
      }
      c_y=y-DEFAULT_CD_LENGTH_MM*cos(alpha);
      c_x=x-DEFAULT_CD_LENGTH_MM*sin(alpha);
    }else{
      if(y>0){
        alpha=atan(tan_alpha);
      }else{
        alpha=PI/2-atan(tan_alpha);
      }
      c_y=y-DEFAULT_CD_LENGTH_MM*cos(alpha);
      c_x=x-DEFAULT_CD_LENGTH_MM*sin(alpha);
    }
  }

  Serial.print("------------alpha\r\n");
  Serial.print(alpha*180/PI);
  Serial.print("\r\n");
  //gamma 180- ab to bc
  float gamma = 0;
  float default_oa = DEFAULT_OA_LENGTH_MM;
  float default_ab = DEFAULT_AB_LENGTH_MM;
  float default_bc = DEFAULT_BC_LENGTH_MM;
  float default_theta = DEFAULT_THETA_DEGREE;

  float a_x = default_oa * cos(default_theta) * sin(alpha);
  float a_y = default_oa * cos(default_theta) * cos(alpha);
  float a_z = default_oa * sin(default_theta);

  float ac = sqrt(pow(c_x - a_x, 2) + pow(c_y - a_y, 2) + pow(c_z - a_z, 2));

  float cos_gamma = (pow(default_ab, 2) + pow(default_bc, 2) - pow(ac, 2)) / (2 * default_ab * default_bc);
//  Serial.print("-------------cos_gamma");
//  Serial.print(cos_gamma);

  
  //gamma = PI - acos(cos_gamma);
  gamma = acos(cos_gamma);

  Serial.print("-------------gamma\r\n");
  Serial.print(gamma*180/PI);
  Serial.print("\r\n");

  ///beta; map to the axis between oa to ab
  float beta = 0;
  float k_map_oa = sin(default_theta) / cos(default_theta);

  float a_map_x = default_oa * cos(default_theta);  ////62.47
  float a_map_y = default_oa * sin(default_theta);  ////101.7

  float c_map_x = sqrt(pow(c_x, 2) + pow(c_y, 2)); ////130
  float c_map_y = c_z; ////102
  float k_map_ac = (c_map_y - a_map_y) / (c_map_x - a_map_x);
//  Serial.print("-------------c_map_y\r\n");
//  Serial.print(c_map_y);
//  Serial.print("\r\n");
//  Serial.print("-------------a_map_y\r\n");
//  Serial.print(a_map_y);
//  Serial.print("\r\n");
//  Serial.print("-------------c_map_x\r\n");
//  Serial.print(c_map_x);
//  Serial.print("\r\n");
//  Serial.print("-------------a_map_x\r\n");
//  Serial.print(a_map_x);
//  Serial.print("\r\n");
//  Serial.print("-------------k_map_oa\r\n");
//  Serial.print(k_map_oa);
//  Serial.print("\r\n");
//  Serial.print("-------------k_map_ac\r\n");
//  Serial.print(k_map_ac);
//  Serial.print("\r\n");

  float tan_oa_ac = abs(k_map_oa - k_map_ac) / abs(1 - k_map_oa * k_map_ac);
  float cos_ab_ac = (pow(default_ab, 2) + pow(ac, 2) - pow(default_bc, 2)) / (2 * default_ab*ac);
  float ab_ac = acos(cos_ab_ac);
//  Serial.print("-------------ab_ac\r\n");
//  Serial.print(cos_ab_ac);
//  Serial.print("\r\n");
//  Serial.print(ab_ac);
//  Serial.print("\r\n");
  float ac_oa = atan(tan_oa_ac);
//  Serial.print("-------------ac_oa\r\n");
//  Serial.print(ac_oa);
//  Serial.print("\r\n");
  
  if (ac_oa < ab_ac) { if (k_map_ac > k_map_oa) {
      beta = 0 - ab_ac - atan(tan_oa_ac);
    } else {
      beta = 0 - ab_ac + atan(tan_oa_ac);
    }
  } else {
    float oc = sqrt(pow(c_map_x, 2) + pow(c_map_y, 2));
    float cos_oa_ac = (pow(default_oa, 2) + pow(ac, 2) - pow(oc, 2)) / (2 * default_oa * ac);
    float oa_ac = acos(cos_oa_ac);
    beta = PI - oa_ac - ab_ac;
  }

  Serial.print("-------------beta\r\n");
  Serial.print(beta*180/PI);
  Serial.print("\r\n");

  float degree_per_step_axis1 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_1 * DEFAULT_REDUCTION_RATE_AXIS_1 * DEFAULT_MICROSTEPS_AXIS_1); //steps per cycle
  float degree_per_step_axis2 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_2 * DEFAULT_REDUCTION_RATE_AXIS_2 * DEFAULT_MICROSTEPS_AXIS_2); //steps per cycle
  float degree_per_step_axis3 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_3 * DEFAULT_REDUCTION_RATE_AXIS_3 * DEFAULT_MICROSTEPS_AXIS_3); //steps per cycle

  float axis1_degree_steps = alpha / degree_per_step_axis1;
  float axis2_degree_steps = beta / degree_per_step_axis2;
  float axis3_degree_steps = gamma / degree_per_step_axis3;

  target[X_AXIS] = (int32_t)(axis1_degree_steps + 0.5);
  target[Y_AXIS] = (int32_t)(axis2_degree_steps + 0.5);
  target[Z_AXIS] = (int32_t)(axis3_degree_steps + 0.5); 

全部代码请见gitclub

Lesson 2: 硬件连接

一、硬件连接

Diego 3dof的硬件连接比较简单,Arduino和A4988是通过Arduino CNC sheild v3连接的,Arduino CNC sheild可以直接插在Arduino上,然后再把3个A4988插在Arduino CNC sheild上即可,连接效果如下:

CNC sheild可以支持4路步进电机,由于我们3轴码垛只使用到了3个步进电机,所以只需要插3个A4988即可,步进电机去A4988的对应关系如下图:

 

二、Arduino Pin对应关系

A4988及马达编号Arduino IO对应功能
1D2马达1步进脉冲
1D5马达1方向
2D3马达2步进脉冲
2D6马达2方向
3D4马达2步进脉冲
3D7马达2方向

 

 

三、其他

1.Diego 3轴机械臂的机械部分需要根据图纸进行钣金加工,或者也可以3D打印,钣金加工的精度要高与3D打印的精度

2. 安装过程中需要用到的不同规格的螺丝刀,电钻等工。

Lession 1: 物料清单

diego 3轴机械臂,采用三轴码垛结构,三轴都采用步进电机控制,装配好后如下图所示

如下表格列出了Diego 3轴机械臂所用硬件物料的详细清单,包括每个硬件图片、名称、数量、功能特性和在机械臂中的作用。

PhotoNameCountFeaturediego 3dof 码垛中的作用
Arduino UNO1•处理器 ATmega328
•工作电压 5V
•输入电压(推荐) 7-12V
•输入电压(范围) 6-20V
•数字IO脚 14 (其中6路作为PWM输出)
•模拟输入脚 6
IO脚直流电流 40 mA
•3.3V脚直流电流 50 mA
•Flash Memory 32 KB (ATmega328,其中0.5 KB 用于 bootloader)
•SRAM 2 KB (ATmega328)
•EEPROM 1 KB (ATmega328)
•工作时钟 16 MHz
•主控制板

Arduino cnc shield 1•步进电机驱动扩展板•驱动机械臂步进电机
A49883•步进电机驱动•驱动机械臂步进电机
42步进电机3•42步进电机
• 星形减速,减速比15:1
• 步进角度为1.8°
•机械臂3个轴的驱动马达
12V 18650 battery1•标称电压 :12V
•放电电流 :6.0A
•充电电流 :0.5A
•内阻 :≤60mΩ
•最高充电电压 :12.6V
•最低放电电压 :9.0V
•为6自由度机械臂舵机供电
•为机械爪舵机供电

硬件安装过程中使用的到工具,螺丝刀,老虎钳,电钻等工具未列出来,读者可以根据实际需求自行准备。

lesson 2: diego iot用户管理

在物联网平台中,非常重要的一个部分就是用户管理,最本质的问题是数据是谁的。一般情况下数据既不属于物联网平台,也不属于设备的制造商,而数据产生者才是数据的正真拥有者,这个拥有者可以是个人,比如现在比较流行健康手环,这个手环所产生的数据应该属于手环的使用者,而手环的生产者或者物联网平台,在未经购买者同意是无权查看和使用这些私人数据,否则就属于侵犯个人隐私;拥有者也可以是任何形式的组织,企业,公益组织等,这些组织使用第三方提供的设备所产生的数据,应该属于这些组织所有,而不是设备的制造者。所有在物联网的设计中必须充分考虑数据的安全性,基于此设计严格的用户权限管理体系。

  1. 用户管理的整体架构

从整个物联网平台层面来看,用户管理分为Iot平台的用户管理,和组织内部的用户管理

  • IOT平台用户管理,主要是负责整个平台运营层面的用户管理,而不涉及组织内部的用户管理,其是不可以查看组织内部数据的。
  • 组织内部的用户管理,主要针对组织内部的用户的用户管理,如哪些用户可以查看某个设备的状态数据,哪些用户进一步可以查看设备的运行数据,其用户权限管理的特征是,详细到对组织设备,设备产生的数据进行权限管理
  • 用户需要注册成为IOT平台用户,后可以创建组织

2. IOT 平台用户管理

对应IOT平台的用户,其主要是针对平台功能模块的权限管理,一个注册的组织,他具有的功能权限,而不具备组织的数据权限,一般有如下功能

  • 超级管理员,平台的管理员,其具有平台层面的全部权限
  • 组织管理,对组织的注册进行审核,监管
  • 服务管理,可以定义每个组织所能在平台所享有的服务权限
  • 交易管理,对于有偿服务,需向用户提供交易功能,这就会涉及到一系列有关交易过程的管理,比如订单管理,价格管理等

3. 组织用户管理

下图是组织层面的用户管理架构图

  • 管理员,平台用户创建组织后,即成为该组织的管理员,对组织内的用户管理,设备管理等具有全部的权限,可以管理用户组,和用户。
  • 权限的管理粒度一般为对象级,如一个设备,可以定义此设备的可视权限,设备的数据权限,定义的粒度越细则管理越精细。权限一般不能被修改,系统会根据设计,自动生产对象的相关权限。
  • 用户组可以说是一组权限和用户的对应集合。一个用户组拥有多个权限,同时也可拥有多个用户;管理员对用户组有增删改的权限;一般系统会创建一个管理员的用户组,在此组内的用户都具有管理员的权限
  • 用户即针对使用系统的单一个体用户,组织的管理员,可以创建用户,并将用户加入到某个用户组,以分配相应的用户权限。用户也可以接受组织的邀请,成为组织下的用户,一个用户可以属于多个组织。

4. 实现

这里只做主要部分的设计,具体的代码可以查看开源代码(目前还在开发中,代码有很多问题,故还为upload到GitHub,呆开发到一定程度即会开源)

4.1界面设计

  • 在用户管理菜单下,依次有User,User Group, Permission,Organization
  • 用户可以通过右上角的组织菜单,切换自己所属的不同组织,组织切换完成,相应的权限都基于组织进行切换

4.2. 数据结构

注:diego iot采用python开发,并使用到Django 的web架构,如下是数据结构的设计

class User_Ext(models.Model):
   user =models.ForeignKey(User)
   mobile_phone = models.CharField(max_length=30)
   qq = models.CharField(max_length=20)
   wechat = models.CharField(max_length=20)
class Organization(models.Model):
   name = models.CharField(max_length=40)
   description = models.CharField(max_length=50)
   address = models.CharField(max_length=50)
   city = models.CharField(max_length=60)
   state_province = models.CharField(max_length=30)
   country = models.CharField(max_length=50)
   logo=models.CharField(max_length=40,default="logo")

#the relation between user and organization
class User_Organization(models.Model):
   user=models.ForeignKey(User)
   organization=models.ForeignKey(Organization)
   role_in_organization=models.IntegerField()#0:owner,1:Admin,2:Viewer

#the iot permission object
class Permission_Ext(models.Model):
   name = models.CharField(max_length=40,default='new permission')
   for_object = models.IntegerField()#0:eqp
   object_id = models.IntegerField(default=1)
   organization=models.ForeignKey(Organization)
   description = models.CharField(max_length=50)
   permission_code = models.CharField(max_length=50,default='permission code')

#the iot group object
class Group_Ext(models.Model):
   name=models.CharField(max_length=40,default='new group')
   organization=models.ForeignKey(Organization)
   description = models.CharField(max_length=50)

#the relationship for permission_ext and group_ext
class Permission_Group_Ext(models.Model):
   permission_ext=models.ForeignKey(Permission_Ext)
   group_ext=models.ForeignKey(Group_Ext)

#the relationship for user and group_ext
class User_Group_Ext(models.Model):
   user=models.ForeignKey(User)
   group_ext=models.ForeignKey(Group_Ext)

 

 

lesson 1: diego iot的整体架构

现在网上讨论的有关物联网的帖子非常之多,但大部分都是介绍理论或者有关硬件,通讯相关的问题,比如物联网模块,物联网通讯协议MQTT、XMPP、NB_IOT等,个人认为这些只是物联网中一部分,而涉及到物联网的设备如何管理,用户如何管理,数据包如何解析,大数据如何展示等也是物联网模块中非常重要的部分,所以作者就根据自身工作中总结出来的建构在云端的物联网平台基本架构分享给大家,并基于此架构如何一步一步来开发一套物联网平台。

物联网平台,应该是基于现在的互联网,通讯技术来建构,而不依赖与特定的硬件模块,用户可以基于自身的设备技术架构,简单轻松接入物联网。下图是物联网的核心架构:

1. 四大核心模块

在物联网中存在4大核心模块,那就是设备管理,用户管理,数据传输管理,数据管理,只有具备了这四大核心模块,才能认为是一个完整的物联网平台,而所有其他的功能模块都是基于此四大功能模块的延展。

1.1 设备管理

  • 设备类型管理:定义设备的类型,此功能一般由设备的制造商来定义,一种设备类型最重要的是关联到一套独有的数据解析方法,数据的存储方法,已经设备规格等数据,也只有设备的制造商才可以编辑有关设备类型的数据,而设备的使用者只能浏览设备类型的相关信息
  • 设备管理:设备管理定义设备相关信息,每个设备必须定义其设备类型,设备类型有使用者属性,设备在完成销售,并被使用者激活后设备就属于设备使用者了,这时候设备使用者对设备有完全的控制权,可以控制设备的哪些数据可以被制造商查看,可以被哪些用户查看等权限

1.2 用户管理

  • 组织管理:在物联网平台中一个很重要的观念就是组织,所有的设备,用户,数据都是基于组织的管理的,设备制造商是一个组织,设备的使用者是一个组织,家庭都可以是一个组织。
  • 用户管理:用户是基于一个组织下的人员构成,每个组织下面都有管理员角色,管理员可以为其服务的组织添加不通的用户,并分配每个用户不同的权限。一个用户也可以属于多个不同的组织,并且扮演不同的组织
  • 用户组:一组用户,也是基于组织的用户组管理,同一用户组的用户拥有相同的权限
  • 权限管理:同样是基于组织的权限管理,主要是针对对象级别的权限细分,如设备的浏览权限,可以控制每个用户是否看到这个设备;设备数据浏览权限定义是否可以查看设备的运行数据

1.3 数据传输管理

1.31 基本格式

数据传输管理,定义针对一类型设备的数据传输协议,基本格式是:

  • 每一个设备有厂商唯一的序列号,因为每个制造商有自己的编码格式,固此序列号没有固定格式。
  • 命令码,为此条数据的作用,比如是上传数据,或者服务器下发给设备的命令等,一般采用2位数字编码00~99
  • 数据,此部分是此条报文,所包含的数据部分,每个协议可以定义不同的解析方式,比如服务器在收到数据包后,会根据预先定义好的解析方式解析数据字段,并按照规则存储

1.32 数据解析定义

  • 每种设备类型可以定义多条命令,每个命令都有自己不同的解析方式,组织的管理员可以为自己的设备类型定义解析方式
  • 服务器接收到数据后,会自动根据预先定义的解析方式解析数据字段
  • 设备开发者要根据在IOT平台定义的数据格式,自行开发自己设备的解析代码
  • 数据字段都按照HEX方式收发

1.33 数据的存储

  • 存储要支持分布式架构,可以为每个设备定义不同的存储位置,在diego iot中数据存储使用mysql数据库,实现不同的设备存储在不同的mysql数据库中
  • 每条数据定义生命周期,在生命结束后,系统将自动删除

1.4 数据管理

  • 权限管理,数据的权限在物联网平台中是至关重要,数据属于谁是一个非常重要的概念,只有设备的拥有者才能定义数据可以给谁看
  • 大数据,物联网数据本身就是海量的数据,我们可以借助一些开源的大数据平台来实现数据的可视化分析,只有经过分析的数据才是有价值的数据
  • 数据的导出,用户可以导出数据到本地做分析

2. 网络通讯

现在所有的云端的物联网平台和设备之间的通讯,本质上都是建构在TCP/IP协议之上的,只是对数据包的再封装而已,基于此我们可以是用wifi,4g来实现设备和云平台的通讯,不过设备与设备之间的通讯,可以有wifi,Bluetooth,zigbee等,下面介绍几种常用的通讯架构

2.1 基于移动3/4G通讯

此架构是最简单的架构,设备就如同我们的手机,基于移动通讯来上网,其主要需要考虑如下几点

  • 每个设备都需要一个SIM卡,可以到移动服务器商办理专门针对物联网的SIM卡
  • 数据流量问题,这种架构完全是走数据流量,如果有视频数据,将会产生比较大的流量费用,这都是要考虑的
  • 通讯质量问题,这完全依赖于移动服务商的网络覆盖状况,就如同我们手机一样,在有些环境下是没有信号的,也就没办法收发数据

2.2 基于wifi局域网

此中架构,适合于所有的物联网设备都是运行在一个局部环境中,设备通过wifi或者有线连接到路由器,而由路由器统一连接的物联网服务器,就如同我们家中装一个wifi路由器上网一样的架构,需要注意的事项:

  • 局域网内的智能设备,是没有公网独立的ip的,只有一个局域网内的ip,带来的问题就是,设备可以直接给物联网服务器发送数据包,而物联网服务器是不能直接给设备发送数据包,就因为设备没有公网独立ip
  • 功耗问题,对于使用wifi接入的设备,最好不是电池供电,因为wifi的功耗比较大
  • 干扰问题,如果在大型的厂房部署这种架构,一定要考虑,厂房内是否有强干扰源,如电磁干扰,可以考虑采用工业级的无线路由器,一般抗干扰能力比较强

2.3 基于蓝牙通讯

一般的基于蓝牙的物联网,会考虑通过蓝牙网关来部署。

蓝牙由于其点对点的通讯方式,所以要考虑如下问题:

  • 蓝牙网关的容量问题,也就是一个蓝牙网关能接入几个蓝牙设备,这取决于蓝牙网关中使用了多少个蓝牙设备
  • 蓝牙的配对问题,蓝牙设备直接的通讯都首先配对才能通讯,如果实现自动配对,如果不能自动配对,大规模部署,将是一个很麻烦的事情

还有一种场景是针对不需要一直在线的物联网设备,而只是在某种特殊需求的情况下,需要连上服务器,这中场景下,我们可以通过手机的蓝牙功能来让设备接入物联网

蓝牙手环是这种架构的一种典型应用模式

2.4 基于zigbee

ZigBee也是一种流行的组网模式,zigbee本身设计是针对传感器之间的联网,具有非常强的低功耗能力

zigbee接入网络也依赖于zigbee网关,网关本身也是一个zigbee设备,zigbee设备是自组网的,在使用过程中注意的问题有

  • 数据量的问题,设备能力和功耗本身是自相矛盾的,由于ZigBee是超低功耗方案,固在通信能力上也是打折扣的,很适合一些传感器数据的采集,如温度湿度,但如果对大数据量的视频类的就不适用了

这里主要介绍了,几种常用的物联网部署架构,至于物联网协议,这里就不多介绍,网上文章非常多

3. 智能设备

diego iot设计的初衷是让智能设备开发者摆脱对特殊模块的依赖,对于智能设备的开发,只要具备联网功能即可,没有特别多的要求。

Lesson 26:Diego 1# —物体识别和定位

google最近公布了基于tensorflow物体识别的Api,本文将利用Diego1#的深度摄像头调用物体识别API,在识别物体的同时计算物体与出机器人摄像头的距离。原理如下:

  • Object Detection 订阅Openni发布的Image消息,识别视频帧中的物体
  • Object Depth 订阅Openni发布的Depth Image消息,根据Object Detection识别出的物体列表,对应到Depth Image的位置,计算Object的深度信息
  • Publish Image将视频帧经过处理,增加识别信息Lable后,以Compressed Image消息发布出去,可以方便其他应用订阅

1.创建diego_tensorflow包

由于我们使用的tensorflow,所以我们首先需要安装tensorflow,可以参考tensorflow官方安装说明https://www.tensorflow.org/install/install_linux

Object_detection相关依赖安装见官方安装说明https://github.com/tensorflow/models/blob/master/object_detection/g3doc/installation.md

执行如下命令创建diego_tensorflow包

catkin_create_pkg diego_tensorflow std_msgs rospy roscpp cv_bridge

在diego_tensorflow目录下创建两个子目录

  • scripts:存放相关代码
  • launch:存放launch启动文件

创建完成后diego_tensorflow目录如下图所示:

下载object_detection包:https://github.com/tensorflow/models

下载后将object_detection包上传到diego_tensorflow/scripts目录下,如果自己做模型训练还需有上传slim包到diego_tensorflow/scripts目录下

物体识别的代码都写在ObjectDetectionDemo.py文件中,其中有关识别的代码大部分参考tensorflow官方示例,这里将其包装成为一个ROS节点,并增加物体深度数据的计算

2.ROS节点源代码

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image as ROSImage
from sensor_msgs.msg import CompressedImage as ROSImage_C
from cv_bridge import CvBridge
import cv2
import matplotlib
import numpy as np
import os
import six.moves.urllib as urllib
import sys
import tarfile
import tensorflow as tf
import zipfile
import uuid
from collections import defaultdict
from io import StringIO
from PIL import Image
from math import isnan

# This is needed since the notebook is stored in the object_detection folder.
from object_detection.utils import label_map_util
from object_detection.utils import visualization_utils as vis_util

class ObjectDetectionDemo():
    def __init__(self):
	rospy.init_node('object_detection_demo')
	
	# Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        self.depth_image =None
        
        self.depth_array = None
        
        model_path = rospy.get_param("~model_path", "")
        image_topic = rospy.get_param("~image_topic", "")
        depth_image_topic = rospy.get_param("~depth_image_topic", "")
        if_down=False
        self.vfc=0
        
        self._cv_bridge = CvBridge()
        
        # What model to download.
	#MODEL_NAME = 'ssd_mobilenet_v1_coco_11_06_2017'
	#MODEL_NAME='faster_rcnn_resnet101_coco_11_06_2017'
	MODEL_NAME ='ssd_inception_v2_coco_11_06_2017'
	#MODEL_NAME ='diego_object_detection_v1_07_2017'
	#MODEL_NAME ='faster_rcnn_inception_resnet_v2_atrous_coco_11_06_2017'
	MODEL_FILE = MODEL_NAME + '.tar.gz'
	DOWNLOAD_BASE = 'http://download.tensorflow.org/models/object_detection/'

	# Path to frozen detection graph. This is the actual model that is used for the object detection.
	PATH_TO_CKPT = MODEL_NAME + '/frozen_inference_graph.pb'

	# List of the strings that is used to add correct label for each box.
	PATH_TO_LABELS = os.path.join(model_path+'/data', 'mscoco_label_map.pbtxt')
	#PATH_TO_LABELS = os.path.join('data', 'mscoco_label_map.pbtxt')

	NUM_CLASSES = 90
	
	if if_down:
		opener = urllib.request.URLopener()
		opener.retrieve(DOWNLOAD_BASE + MODEL_FILE, MODEL_FILE)
		tar_file = tarfile.open(MODEL_FILE)
		for file in tar_file.getmembers():
			file_name = os.path.basename(file.name)
			if 'frozen_inference_graph.pb' in file_name:
        			tar_file.extract(file, os.getcwd())


	rospy.loginfo("begin initilize the tf...")
	self.detection_graph = tf.Graph()
	with self.detection_graph.as_default():
		od_graph_def = tf.GraphDef()
		with tf.gfile.GFile(PATH_TO_CKPT, 'rb') as fid:
			serialized_graph = fid.read()
			od_graph_def.ParseFromString(serialized_graph)
			tf.import_graph_def(od_graph_def, name='')

	label_map = label_map_util.load_labelmap(PATH_TO_LABELS)
	categories = label_map_util.convert_label_map_to_categories(label_map, max_num_classes=NUM_CLASSES, use_display_name=True)
	self.category_index = label_map_util.create_category_index(categories)
	
	# Subscribe to the registered depth image
        rospy.Subscriber(depth_image_topic, ROSImage, self.convert_depth_image)
        
        # Wait for the depth image to become available
        #rospy.wait_for_message('depth_image', ROSImage)
	
	self._sub = rospy.Subscriber(image_topic, ROSImage, self.callback, queue_size=1)	
	self._pub = rospy.Publisher('object_detection', ROSImage_C, queue_size=1)
	
	rospy.loginfo("initialization has finished...")
	
	
    def convert_depth_image(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        # The depth image is a single-channel float32 image
        self.depth_image = self._cv_bridge.imgmsg_to_cv2(ros_image, "32FC1")

        # Convert the depth image to a Numpy array
        self.depth_array = np.array(self.depth_image, dtype=np.float32)
        #print(self.depth_array)
        
    def callback(self,image_msg):
	if self.vfc<12:
		self.vfc=self.vfc+1
	else:
		self.callbackfun(image_msg)
		self.vfc=0	
		    	
    def box_depth(self,boxes,im_width, im_height):
	# Now compute the depth component
        depth=[]
	for row in boxes[0]:
		n_z = sum_z = mean_z = 0
		# Get the min/max x and y values from the ROI
		if row[0]<row[1]:
			min_x = row[0]*im_width
			max_x = row[1]*im_width
		else:
			min_x = row[1]*im_width
			max_x = row[0]*im_width
			
		if row[2]<row[3]:
			min_y = row[2]*im_height
			max_y = row[3]*im_height
		else:
			min_y = row[3]*im_height
			max_y = row[2]*im_height
		# Get the average depth value over the ROI
		for x in range(int(min_x), int(max_x)):
            		for y in range(int(min_y), int(max_y)):
                		try:
					z = self.depth_array[y, x]
				except:
					continue
                
				# Depth values can be NaN which should be ignored
				if isnan(z):
					z=6
					continue
				else:
					sum_z = sum_z + z
					n_z += 1 
			mean_z = sum_z / (n_z+0.01)
		depth.append(mean_z)
	return depth
    def callbackfun(self, image_msg):
	with self.detection_graph.as_default():
		with tf.Session(graph=self.detection_graph) as sess:
			 cv_image = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
			 #cv_image = (self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8"))[300:450, 150:380]
			 pil_img = Image.fromarray(cv_image)			 
			 (im_width, im_height) = pil_img.size			 
			 # the array based representation of the image will be used later in order to prepare the
			 # result image with boxes and labels on it.
			 image_np =np.array(pil_img.getdata()).reshape((im_height, im_width, 3)).astype(np.uint8)
			 # Expand dimensions since the model expects images to have shape: [1, None, None, 3]
			 image_np_expanded = np.expand_dims(image_np, axis=0)
			 image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0')
			 # Each box represents a part of the image where a particular object was detected.
			 boxes = self.detection_graph.get_tensor_by_name('detection_boxes:0')
			 # Each score represent how level of confidence for each of the objects.
			 # Score is shown on the result image, together with the class label.
			 scores = self.detection_graph.get_tensor_by_name('detection_scores:0')
			 classes = self.detection_graph.get_tensor_by_name('detection_classes:0')
			 num_detections = self.detection_graph.get_tensor_by_name('num_detections:0')
			
			 # Actual detection.
			 (boxes, scores, classes, num_detections) = sess.run(
			 	[boxes, scores, classes, num_detections],
			 	feed_dict={image_tensor: image_np_expanded})
			 box_depths=self.box_depth(boxes,im_width,im_height)
			 print(box_depths)
			 # Visualization of the results of a detection.
			 vis_util.visualize_boxes_and_labels_on_image_array(
			 	image_np,
			 	np.squeeze(boxes),
			 	np.squeeze(classes).astype(np.int32),
			 	np.squeeze(scores),
			 	self.category_index,
			 	use_normalized_coordinates=True,
			 	line_thickness=8)
			 
			 ros_compressed_image=self._cv_bridge.cv2_to_compressed_imgmsg(image_np)
			 self._pub.publish(ros_compressed_image)
			
    
    def shutdown(self):
        rospy.loginfo("Stopping the tensorflow object detection...")
        rospy.sleep(1) 
        
if __name__ == '__main__':
    try:
        ObjectDetectionDemo()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("RosTensorFlow_ObjectDetectionDemo has started.")

下面我们来解释主要的代码逻辑

    def __init__(self):
	rospy.init_node('object_detection_demo')
	
	# Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        self.depth_image =None
        
        self.depth_array = None
        
        model_path = rospy.get_param("~model_path", "")
        image_topic = rospy.get_param("~image_topic", "")
        depth_image_topic = rospy.get_param("~depth_image_topic", "")
        if_down=False
        self.vfc=0
        
        self._cv_bridge = CvBridge()

以上代码是ROS的标准初始化代码,变量的初始化,及launch文件中参数的读取

  • model_path定义object_detection所使用的模型路径
  • image_topic订阅的image主题
  • depth_image_topic订阅的深度image主题
        # What model to download.
	#MODEL_NAME = 'ssd_mobilenet_v1_coco_11_06_2017'
	#MODEL_NAME='faster_rcnn_resnet101_coco_11_06_2017'
	MODEL_NAME ='ssd_inception_v2_coco_11_06_2017'
	#MODEL_NAME ='diego_object_detection_v1_07_2017'
	#MODEL_NAME ='faster_rcnn_inception_resnet_v2_atrous_coco_11_06_2017'
	MODEL_FILE = MODEL_NAME + '.tar.gz'
	DOWNLOAD_BASE = 'http://download.tensorflow.org/models/object_detection/'

	# Path to frozen detection graph. This is the actual model that is used for the object detection.
	PATH_TO_CKPT = MODEL_NAME + '/frozen_inference_graph.pb'

	# List of the strings that is used to add correct label for each box.
	PATH_TO_LABELS = os.path.join(model_path+'/data', 'mscoco_label_map.pbtxt')
	#PATH_TO_LABELS = os.path.join('data', 'mscoco_label_map.pbtxt')

	NUM_CLASSES = 90
	
	if if_down:
		opener = urllib.request.URLopener()
		opener.retrieve(DOWNLOAD_BASE + MODEL_FILE, MODEL_FILE)
		tar_file = tarfile.open(MODEL_FILE)
		for file in tar_file.getmembers():
			file_name = os.path.basename(file.name)
			if 'frozen_inference_graph.pb' in file_name:
        			tar_file.extract(file, os.getcwd())

以上代码设置object_detection所使用的模型,及下载解压相应的文件,这里设置了一个if_down的开关,第一次运行的时候可以打开此开关下载,以后可以关掉,因为下载的时间比较长,下载一次后面就无需再下载了。

self.detection_graph = tf.Graph()
	with self.detection_graph.as_default():
		od_graph_def = tf.GraphDef()
		with tf.gfile.GFile(PATH_TO_CKPT, 'rb') as fid:
			serialized_graph = fid.read()
			od_graph_def.ParseFromString(serialized_graph)
			tf.import_graph_def(od_graph_def, name='')

	label_map = label_map_util.load_labelmap(PATH_TO_LABELS)
	categories = label_map_util.convert_label_map_to_categories(label_map, max_num_classes=NUM_CLASSES, use_display_name=True)
	self.category_index = label_map_util.create_category_index(categories)

以上代码是tensorflow的初始化代码。

        # Subscribe to the registered depth image
        rospy.Subscriber(depth_image_topic, ROSImage, self.convert_depth_image)
        
        # Wait for the depth image to become available
        #rospy.wait_for_message('depth_image', ROSImage)
	
	self._sub = rospy.Subscriber(image_topic, ROSImage, self.callback, queue_size=1)	
	self._pub = rospy.Publisher('object_detection', ROSImage_C, queue_size=1)

以上代码,我们定义此节点订阅depth_image和image两个topic,同时发布一个名为object_detection的Compressed Imagetopic
depth_image的回调函数是convert_depth_image
image的回调函数是callback

    def convert_depth_image(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        # The depth image is a single-channel float32 image
        self.depth_image = self._cv_bridge.imgmsg_to_cv2(ros_image, "32FC1")

        # Convert the depth image to a Numpy array
        self.depth_array = np.array(self.depth_image, dtype=np.float32)
        #print(self.depth_array)

以上是depth_image处理的回调函数,首先将depth_image主题转换成opencv类型的,然后在将图片转换为numpy数组,赋值给depth_array成员变量

    def callback(self,image_msg):
	if self.vfc<12:
		self.vfc=self.vfc+1
	else:
		self.callbackfun(image_msg)
		self.vfc=0

以上是image处理的回调函数,这里控制视频帧的处理频率,主要是为了减少运算量,可以灵活调整,最终视频帧的处理是在callbackfun中处理的

    def box_depth(self,boxes,im_width, im_height):
	# Now compute the depth component
        depth=[]
	for row in boxes[0]:
		n_z = sum_z = mean_z = 0
		# Get the min/max x and y values from the ROI
		if row[0]<row[1]:
			min_x = row[0]*im_width
			max_x = row[1]*im_width
		else:
			min_x = row[1]*im_width
			max_x = row[0]*im_width
			
		if row[2]<row[3]:
			min_y = row[2]*im_height
			max_y = row[3]*im_height
		else:
			min_y = row[3]*im_height
			max_y = row[2]*im_height
		# Get the average depth value over the ROI
		for x in range(int(min_x), int(max_x)):
            		for y in range(int(min_y), int(max_y)):
                		try:
					z = self.depth_array[y, x]
				except:
					continue
                
				# Depth values can be NaN which should be ignored
				if isnan(z):
					z=6
					continue
				else:
					sum_z = sum_z + z
					n_z += 1 
			mean_z = sum_z / (n_z+0.01)
		depth.append(mean_z)
	return depth

以上代码是深度数据计算,输入参数boxes就是object_detection识别出来的物体的矩形标识rect,我们根据物体的矩形范围,匹配深度图片相应的区域,计算区域内的平均深度值作为此物体的深度数据。返回一个与boxes想对应的1维数组
由于深度图,和一般图片是异步处理,可能出现帧不对应的问题,在这里处理的比较简单,没有考虑此问题,可以通过缓存深度图片的方式来解决,通过时间戳来匹配最近的深度图片。

def callbackfun(self, image_msg):
	with self.detection_graph.as_default():
		with tf.Session(graph=self.detection_graph) as sess:
			 cv_image = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
			 #cv_image = (self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8"))[300:450, 150:380]
			 pil_img = Image.fromarray(cv_image)			 
			 (im_width, im_height) = pil_img.size			 
			 # the array based representation of the image will be used later in order to prepare the
			 # result image with boxes and labels on it.
			 image_np =np.array(pil_img.getdata()).reshape((im_height, im_width, 3)).astype(np.uint8)
			 # Expand dimensions since the model expects images to have shape: [1, None, None, 3]
			 image_np_expanded = np.expand_dims(image_np, axis=0)
			 image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0')
			 # Each box represents a part of the image where a particular object was detected.
			 boxes = self.detection_graph.get_tensor_by_name('detection_boxes:0')
			 # Each score represent how level of confidence for each of the objects.
			 # Score is shown on the result image, together with the class label.
			 scores = self.detection_graph.get_tensor_by_name('detection_scores:0')
			 classes = self.detection_graph.get_tensor_by_name('detection_classes:0')
			 num_detections = self.detection_graph.get_tensor_by_name('num_detections:0')
			
			 # Actual detection.
			 (boxes, scores, classes, num_detections) = sess.run(
			 	[boxes, scores, classes, num_detections],
			 	feed_dict={image_tensor: image_np_expanded})
			 box_depths=self.box_depth(boxes,im_width,im_height)
			 # Visualization of the results of a detection.
			 vis_util.visualize_boxes_and_labels_on_image_array(
			 	image_np,
			 	np.squeeze(boxes),
			 	np.squeeze(classes).astype(np.int32),
			 	np.squeeze(scores),
			 	self.category_index,
                                box_depths,
			 	use_normalized_coordinates=True,
			 	line_thickness=8)
			 
			 ros_compressed_image=self._cv_bridge.cv2_to_compressed_imgmsg(image_np)
			 self._pub.publish(ros_compressed_image)

以上代码是图片的回调函数,主要是将image消息转换为opencv格式,然后再转换成numpy数组,调用object_detection来识别图片中的物体,再调用 vis_util.visualize_boxes_and_labels_on_image_array将识别出来的物体在图片上标识出来,最后将处理后的图片发布为compressed_image类型的消息

现在我们只需要简单修改一下Object_detection/utils目录下的visualization_utils.py文件,就可以显示深度信息

def visualize_boxes_and_labels_on_image_array(image,
                                              boxes,
                                              classes,
                                              scores,
                                              category_index,
	                                      box_depths=None,
                                              instance_masks=None,
                                              keypoints=None,
                                              use_normalized_coordinates=False,
                                              max_boxes_to_draw=20,
                                              min_score_thresh=.5,
                                              agnostic_mode=False,
                                              line_thickness=4):

在visualize_boxes_and_labels_on_image_array定义中增加box_depths=None,缺省值为None

  for i in range(min(max_boxes_to_draw, boxes.shape[0])):
    if scores is None or scores[i] > min_score_thresh:
      box = tuple(boxes[i].tolist())
      if instance_masks is not None:
        box_to_instance_masks_map[box] = instance_masks[i]
      if keypoints is not None:
        box_to_keypoints_map[box].extend(keypoints[i])
      if scores is None:
        box_to_color_map[box] = 'black'
      else:
        if not agnostic_mode:
          if classes[i] in category_index.keys():
            class_name = category_index[classes[i]]['name']
          else:
            class_name = 'N/A'
          display_str = '{}: {}%'.format(
              class_name,
              int(100*scores[i]))
        else:
          display_str = 'score: {}%'.format(int(100 * scores[i]))
          
        #modify by diego robot
        if box_depths!=None:
        	display_str=display_str+"\ndepth: "+str(box_depths[i])
        	global depth_info
        	depth_info=True
        else:
        	global depth_info
        	depth_info=False
        #######################
        box_to_display_str_map[box].append(display_str)
        if agnostic_mode:
          box_to_color_map[box] = 'DarkOrange'
        else:
          box_to_color_map[box] = STANDARD_COLORS[
              classes[i] % len(STANDARD_COLORS)]

在第一个for循环里面,的box_to_display_str_map[box].append(display_str)一句前面增加如上diego robot修改部分代码

depth_info=False

在文件的开头部分定义全局变量,表示是否有深度信息传递进来

 # Reverse list and print from bottom to top.
  for display_str in display_str_list[::-1]: 
    text_width, text_height = font.getsize(display_str)    
    #modify by william
    global depth_info
    if depth_info:
  	text_height=text_height*2
    ###################
    	
    margin = np.ceil(0.05 * text_height)
    draw.rectangle(
        [(left, text_bottom - text_height - 2 * margin), (left + text_width,
                                                          text_bottom)],
        fill=color)
    draw.text(
        (left + margin, text_bottom - text_height - margin),
        display_str,
        fill='black',
        font=font)
    text_bottom -= text_height - 2 * margin

在draw_bounding_box_on_image函数的margin = np.ceil(0.05 * text_height)一句前面增加如上diego robot修改部分代码

3.launch文件

<launch>
   <node pkg="diego_tensorflow" name="ObjectDetectionDemo" type="ObjectDetectionDemo.py" output="screen">

       <param name="image_topic" value="/camera/rgb/image_raw" />

       <param name="depth_image_topic" value="/camera/depth_registered/image" />

       <param name="model_path" value="$(find diego_tensorflow)/scripts/object_detection" />

   </node>

</launch>

launch文件中定义了相应的参数,image_topic,depth_image_topic,model_path,读者可以根据自己的实际情况设定

4.启动节点

启动openni

roslaunch diego_vision openni_node.launch 

启动object_detection

roslaunch diego_tensorflow object_detection_demo.launch

5.通过手机APP订阅object_detection

我们只需要设置一个image_topic为object_detection,既可以在手机上看到物体识别的效果

object_detection物体识别是建立在训练模型上的,本文所采用的是官方所提供的训练好的模型,识别率还是比较高的。但是如果机器人所工作的环境与训练的图片差异比较大,识别率会大大降低。所以我们在实际使用中可以训练自己物体模型,在结合机器人的其他传感器和装置来达到实际应用的要求。比如可以应用在人员流量的监控,车辆的监控,或者特定物体的跟踪,再配合机械臂,深度相机,可以实现物体位置的判断,抓取等功能。

Lesson 25:Diego 1# 4WD —3:上位机通讯

ROS Arduino Bridge本质上是上位机通过串口发送控制命令来实现对Arduino的控制,所以我们要实现4驱的控制,我们也必须的修改通讯部分

1.Arduino firmware修改

1.1 command.h

在此文件中增加4驱所需的命令及宏定义

#ifndef COMMANDS_H
#define COMMANDS_H

#define ANALOG_READ    'a'
#define GET_BAUDRATE   'b'
#define PIN_MODE       'c'
#define DIGITAL_READ   'd'
#define READ_ENCODERS  'e'
#define MOTOR_SPEEDS   'm'
#define PING           'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE    's'
#define SERVO_READ     't'
#define UPDATE_PID     'u'
#define DIGITAL_WRITE  'w'
#define ANALOG_WRITE   'x'
#define LEFT            0
#define RIGHT           1
#define LEFT_H          2 //新增
#define RIGHT_H         3 //新增
#define READ_PIDOUT    'f'
#define READ_PIDIN     'i'
#define READ_MPU6050   'g'

#endif

1.2 RosArduinoBridge-diego.ino

此文件是主程序,由于此文件代码比较多,故这里只介绍新增部分代码,完整的代码请见github

在runCommand()函数中修改在4驱模式下读取pidin 的代码

    case READ_PIDIN:
      Serial.print(readPidIn(LEFT));
      Serial.print(" ");
#ifdef L298P      
      Serial.println(readPidIn(RIGHT));
#endif
#ifdef L298P_4WD 
      Serial.print(readPidIn(RIGHT));
      Serial.print(" ");
      Serial.print(readPidIn(LEFT_H));
      Serial.print(" ");
      Serial.println(readPidIn(RIGHT_H));
#endif      
      break;

在runCommand()函数中修改在4驱模式下读取pidout 的代码

    case READ_PIDOUT:
      Serial.print(readPidOut(LEFT));
      Serial.print(" ");
#ifdef L298P      
      Serial.println(readPidOut(RIGHT));
#endif
#ifdef L298P_4WD 
      Serial.print(readPidOut(RIGHT));
      Serial.print(" ");
      Serial.print(readPidOut(LEFT_H));
      Serial.print(" ");
      Serial.println(readPidOut(RIGHT_H));
#endif       
      break;

在runCommand()函数中修改在4驱模式下读取编码器数据的代码

    case READ_ENCODERS:
      Serial.print(readEncoder(LEFT));
      Serial.print(" ");
#ifdef L298P 
      Serial.println(readEncoder(RIGHT));
#endif
#ifdef L298P_4WD
      Serial.print(readEncoder(RIGHT));
      Serial.print(" ");
      Serial.print(readEncoder(LEFT_H));
      Serial.print(" ");
      Serial.println(readEncoder(RIGHT_H));
#endif      
      break;

在runCommand()函数中修改在4驱模式下设置马达速度的代码

    case MOTOR_SPEEDS:
      /* Reset the auto stop timer */
      lastMotorCommand = millis();
      if (arg1 == 0 && arg2 == 0) {
#ifdef L298P        
        setMotorSpeeds(0, 0);
#endif

#ifdef L298P_4WD
        setMotorSpeeds(0, 0, 0, 0);
#endif        
        moving = 0;
      }
      else moving = 1;
      leftPID.TargetTicksPerFrame = arg1;
      rightPID.TargetTicksPerFrame = arg2;
#ifdef L298P_4WD      
      leftPID_h.TargetTicksPerFrame = arg1;
      rightPID_h.TargetTicksPerFrame = arg2;
#endif       
      Serial.println("OK");
      break;

在runCommand()函数中修改在4驱模式下更新PID参数的代码

    case UPDATE_PID:
      while ((str = strtok_r(p, ":", &p)) != '\0') {
        pid_args[i] = atoi(str);
        i++;
      }

      left_Kp = pid_args[0];
      left_Kd = pid_args[1];
      left_Ki = pid_args[2];
      left_Ko = pid_args[3];

      right_Kp = pid_args[4];
      right_Kd = pid_args[5];
      right_Ki = pid_args[6];
      right_Ko = pid_args[7];

#ifdef L298P_4WD

      left_h_Kp = pid_args[0];
      left_h_Kd = pid_args[1];
      left_h_Ki = pid_args[2];
      left_h_Ko = pid_args[3];

      right_h_Kp = pid_args[4];
      right_h_Kd = pid_args[5];
      right_h_Ki = pid_args[6];
      right_h_Ko = pid_args[7];
#endif
      
      Serial.println("OK");
      break;

在setup()函数中设置在4驱模式下新增的pin对应的寄存器的操作

void setup() {
  Serial.begin(BAUDRATE);
#ifdef USE_BASE
#ifdef ARDUINO_ENC_COUNTER
  //set as inputs
  DDRD &= ~(1 << LEFT_ENC_PIN_A);
  DDRD &= ~(1 << LEFT_ENC_PIN_B);
  DDRC &= ~(1 << RIGHT_ENC_PIN_A);
  DDRC &= ~(1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  DDRD &= ~(1 << LEFT_H_ENC_PIN_A);
  DDRD &= ~(1 << LEFT_H_ENC_PIN_B);
  DDRC &= ~(1 << RIGHT_H_ENC_PIN_A);
  DDRC &= ~(1 << RIGHT_H_ENC_PIN_B);
#endif  

  //enable pull up resistors
  PORTD |= (1 << LEFT_ENC_PIN_A);
  PORTD |= (1 << LEFT_ENC_PIN_B);
  PORTC |= (1 << RIGHT_ENC_PIN_A);
  PORTC |= (1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  PORTD &= ~(1 << LEFT_H_ENC_PIN_A);
  PORTD &= ~(1 << LEFT_H_ENC_PIN_B);
  PORTC &= ~(1 << RIGHT_H_ENC_PIN_A);
  PORTC &= ~(1 << RIGHT_H_ENC_PIN_B);
#endif    
  // tell pin change mask to listen to left encoder pins
  PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B);
  // tell pin change mask to listen to right encoder pins
  PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  // tell pin change mask to listen to left encoder pins
  PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B) | (1 << LEFT_H_ENC_PIN_A) | (1 << LEFT_H_ENC_PIN_B);
  // tell pin change mask to listen to right encoder pins
  PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B) | (1 << RIGHT_H_ENC_PIN_A) | (1 << RIGHT_H_ENC_PIN_B);
#endif
  // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
  PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
  initMotorController();
  resetPID();
#endif

  /* Attach servos if used */
#ifdef USE_SERVOS
  int i;
  for (i = 0; i < N_SERVOS; i++) {
    servosPos[i]=90;
  }
  servodriver.begin();
  servodriver.setPWMFreq(50);
#endif
}

在loop()函数中修改在4驱模式下自动停止的逻辑

  // Check to see if we have exceeded the auto-stop interval
  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
    ;
#ifdef L298P    
    setMotorSpeeds(0, 0);
#endif
#ifdef L298P_4WD
    setMotorSpeeds(0, 0, 0, 0);
#endif
    moving = 0;
  }

#endif

2.上位机代码修改

上位机的代码修改主要是针对arduino_driver.py和base_controller.py的修改。

2.1 arduino_driver.py修改

修改def get_pidin(self):使其支持4个电机的pidin读取

    def get_pidin(self):
        values = self.execute_array('i')
        print("pidin_raw_data: "+str(values))
        if len(values) not in [2,4]:
            print "pidin was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:                                                                  
            return values

修改def get_pidout(self):使其支持4个电机的pidout读取

    def get_pidout(self):
        values = self.execute_array('f')
        print("pidout_raw_data: "+str(values))
        if len(values) not in [2,4]:
            print "pidout was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:                                                                  
            return values

修改def get_encoder_counts(self):使其支持4个电机的编码器数据读取

    def get_encoder_counts(self):
        values = self.execute_array('e')
        if len(values) not in [2,4]:
            print "Encoder count was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:

修改完后,将arduino的固件更新,就可以启动4驱底盘控制了,要注意的时候要同时打开上位机和arduino上的4驱开关,如果是使用2驱的代码,也要记得关掉4驱的开关。

3.启动4驱底盘控制

现在执行如下命令可以启动4驱底盘控制

roslaunch diego_nav diego_arduino_run.launch 

现在我们可以通过配套的ROS APP连接Diego#进行控制

首先创建一个新的ROS机器人连接,设置相应的ROS Master ip,和cmd_vel主题。

连接上机器人后,可以选择操作杆,和重力感应来对机器人操作

Lesson 24:Diego 1# 4驱底盘 —2:马达控制

针对马达控制我们主要需要修改两部分:

  • 驱动部分,使其可以支持同时控制4个马达
  • PID调速部分,试验表明,虽然是一样的型号的马达,但是给相同的PWM值,马达的转速也不一样,所以我们需要分别对4个马达进行PID调速

1.马达驱动

1.1. DualL298PMotorShield4WD.h文件的修改
增加了4个马达对应的PWM和方向控制的Pin引脚定义,和4个马达速度的设置函数

#ifndef DualL298PMotorShield4WD_h
#define DualL298PMotorShield4WD_h

#include 

class DualL298PMotorShield4WD
{
  public:  
    // CONSTRUCTORS
    DualL298PMotorShield4WD(); // Default pin selection.
    
    // PUBLIC METHODS
    void init(); // Initialize TIMER 1, set the PWM to 20kHZ. 
    void setM1Speed(int speed); // Set speed for M1.
    void setM2Speed(int speed); // Set speed for M2.
    void setM3Speed(int speed); // Set speed for M3.
    void setM4Speed(int speed); // Set speed for M4.
    void setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed); // Set speed for both M1 and M2.
    
  private:
  
    // left motor
    static const unsigned char _M1DIR = 12;
    static const unsigned char _M2DIR = 7;
    static const unsigned char _M1PWM = 10;
    static const unsigned char _M2PWM = 6;
    
    // right motor
    static const unsigned char _M4DIR = 8;
    static const unsigned char _M3DIR = 13;
    static const unsigned char _M4PWM = 9;
    static const unsigned char _M3PWM = 11;
    
};

#endif

1.2. DualL298PMotorShield4WD.cpp文件的修改

主要是针对4个马达速度设置函数的实现,逻辑都是一样的,这里只截取一个马达的代码,代码的逻辑请看代码注释

// Set speed for motor 4, speed is a number betwenn -400 and 400
void DualL298PMotorShield4WD::setM4Speed(int speed)
{
  unsigned char reverse = 0;
  
  if (speed < 0) //速度是否小于0 { speed = -speed; // 如果小于0,则是反转 reverse = 1; // 反转标志变量设置为1 } if (speed > 255)  // 限定最大速度为255
    speed = 255;
  if (reverse)  //反转状态下
  {
    digitalWrite(_M4DIR,LOW);//设定马达转向的pin位低电平
    analogWrite(_M4PWM, speed);
  }
  else //正向转动状态下
  {
    digitalWrite(_M4DIR,HIGH);设定马达转向的pin位高电平
    analogWrite(_M4PWM, speed);
  }
}

在setSpeeds函数中分别调用4个马达的速度设置函数。

// Set speed for motor 1, 2, 3, 4
void DualL298PMotorShield4WD::setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed)
{
  setM1Speed(m1Speed);
  setM2Speed(m2Speed);
  setM3Speed(m3Speed);
  setM4Speed(m4Speed);  
}

DualL298PMotorShield4WD.h和DualL298PMotorShield4WD.cpp修改完成后,在arduino 的library目录下新建一个名为dual-L298P-motor-shield-master-4wd的目录,将两个文件放到此文件夹下

现在我们打开android ide的library就可以看到我们刚才添加的库

1.1.3 motor_driver.h的修改

增加4驱马达控制的函数setMotorSpeeds,参数为4个马达的速度

void initMotorController();
void setMotorSpeed(int i, int spd);
#ifdef L298P
void setMotorSpeeds(int leftSpeed, int rightSpeed);
#endif
#ifdef L298P_4WD
void setMotorSpeeds(int leftSpeed_1, int leftSpeed_2, int rightSpeed_1, int rightSpeed_2);
#endif

1.1.4 motor_driver.ino的修改

#ifdef L298P_4WD
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed_1, int leftSpeed_2, int rightSpeed_1, int rightSpeed_2){
  setMotorSpeed(1, leftSpeed_1);
  setMotorSpeed(3, rightSpeed_1);
  setMotorSpeed(2, leftSpeed_2);
  setMotorSpeed(4, rightSpeed_2);
}
#endif
#else
#error A motor driver must be selected!
#endif

2.PID控制

PID控制都在diff_controller.h文件中定义修改

新增两个在4驱模式下的PID控制变量

#ifdef L298P_4WD
SetPointInfo leftPID_h, rightPID_h;
#endif

新增两个在4驱模式下的PID控制参数

#ifdef L298P_4WD

int left_h_Kp=Kp;
int left_h_Kd=Kd;
int left_h_Ki=Ki;
int left_h_Ko=Ko;

int right_h_Kp=Kp;
int right_h_Kd=Kd;
int right_h_Ki=Ki;
int right_h_Ko=Ko;

#endif

新增两个在4驱模式下的PID控制函数

#ifdef L298P_4WD
/* PID routine to compute the next motor commands */
void dorightPID_h(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input =  p->Encoder-p->PrevEnc ;
  Perror = p->TargetTicksPerFrame - input;

  /*
  * Avoid derivative kick and allow tuning changes,
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
  // p->PrevErr = Perror;
  output = (right_h_Kp * Perror - right_h_Kd * (input - p->PrevInput) + p->ITerm) / right_h_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;
  // Accumulate Integral error *or* Limit output.
  // Stop accumulating when output saturates
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += left_h_Ki * Perror;

  p->output = output;
  p->PrevInput = input;
//  Serial.println("right output:");
//  Serial.println(p->output);
}
#endif
...
#ifdef L298P_4WD

/* PID routine to compute the next motor commands */
void doleftPID_h(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input = p->Encoder-p->PrevEnc ;
  Perror = p->TargetTicksPerFrame - input;

  /*
  * Avoid derivative kick and allow tuning changes,
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
  // p->PrevErr = Perror;
  output = (left_h_Kp * Perror - left_h_Kd * (input - p->PrevInput) + p->ITerm) / left_h_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;
  // Accumulate Integral error *or* Limit output.
  // Stop accumulating when output saturates
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += left_h_Ki * Perror;

  p->output = output;
  p->PrevInput = input;
//  Serial.println("left output:");
//  Serial.println(p->output);
}
#endif

修改readPIDIn函数,增加在4驱模式下,pidin的读取

long readPidIn(int i) {
  long pidin=0;
  if (i == LEFT){
    pidin = leftPID.PrevInput;
  }else if (i == RIGHT){
    pidin = rightPID.PrevInput;
  }
#ifdef L298P_4WD
  else if (i== RIGHT_H){
    pidin = rightPID_h.PrevInput;
  }else{
    pidin = leftPID_h.PrevInput;
  }
#endif  
  return pidin;
}

修改readPIDOut函数,增加在4驱模式下,pidOut的读取

long readPidOut(int i) {
  long pidout=0;
  if (i == LEFT){
    pidout = leftPID.output;
  }else if (i == RIGHT){
    pidout = rightPID.output;
  }
#ifdef L298P_4WD
  else if (i == RIGHT_H){
    pidout = rightPID_h.output;
  }else{
    pidout = leftPID_h.output;
  }
#endif   
  return pidout;
}

修改updatePID函数,增加在4驱模式下马达PID控制的调用

void updatePID() {
  /* Read the encoders */
  leftPID.Encoder =readEncoder(LEFT);
  rightPID.Encoder =readEncoder(RIGHT);
#ifdef L298P_4WD 
  leftPID_h.Encoder =readEncoder(LEFT_H);
  rightPID_h.Encoder =readEncoder(RIGHT_H);
#endif
  
  /* If we're not moving there is nothing more to do */
  if (!moving){
    /*
    * Reset PIDs once, to prevent startup spikes,
    * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
    * PrevInput is considered a good proxy to detect
    * whether reset has already happened
    */
#ifdef L298P    
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
#endif
#ifdef L298P_4WD
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0 || leftPID_h.PrevInput != 0 || rightPID_h.PrevInput != 0) resetPID();
#endif    
    return;
  }

  /* Compute PID update for each motor */
  dorightPID(&rightPID);
  doleftPID(&leftPID);
#ifdef L298P_4WD
  dorightPID_h(&rightPID_h);
  doleftPID_h(&leftPID_h);
#endif  

  /* Set the motor speeds accordingly */
#ifdef L298P   
  setMotorSpeeds(leftPID.output, rightPID.output);
#endif

#ifdef L298P_4WD
  setMotorSpeeds(leftPID.output,leftPID_h.output, rightPID.output,rightPID_h.output);
#endif

}

至此4个马达已经可以独立驱动,独立的PID调速了,在下一篇教程中,会介绍如和上位机互动起来,让4驱底盘跑起来。

Lesson 23:Diego 1# 4驱底盘 —1:编码器

4驱底盘由于四个轮子可以独立控制,所以具有优秀的通过性,这篇文章介绍Diego 1#的四驱底盘,所有源代码都已经上传到github。

这里会分几篇文章来介绍4驱动版diego 1#的开发,这篇我们主要说明4驱动底盘4个马达编码器数据的读取。

1.1硬件说明

  • 底盘材质:铝合金材质
  • 轮胎:12cm 橡胶轮胎
  • 电机:370直流电机,带霍尔码盘测速,输出AB项编码信号

1.2 控制器

  • 主控制器 arduino UNO,使用uno分别对4个电机进行方向,PWM控制,并采用终端方式采集4个电机输出的编码信号
  • 电机控制器 两块L298p, 网上采购,控制引脚不同
  • 上位机:树莓派,或者mini pc

2.编码器数据读取

所以代码都基于diego 1# github上的代码进行修改,这里只说明4驱版本部分的代码

首先我们在ROSAduinoBridge_diego.ino文件中定义一个预编译符号,这个预编译符号可以启动或者关闭4驱的代码,这样方便我们开关4驱的功能

#define L298P_4WD

在diego 1 4wd中实现了使用arduino uno读取4个马达的AB项编码输出,一般的网上说明中arduino uno只有两个外部中断,好像只能读取一个马达的AB项编码输出,但事实上arudino中所有IO引脚都可以作为中断使用,通过操作中断寄存器的方式。

首先我们在encoder_driver.h文件中定义编码器连接的Arduino uno引脚:

#ifdef ARDUINO_ENC_COUNTER
  //below can be changed, but should be PORTD pins; 
  //otherwise additional changes in the code are required
  #define LEFT_ENC_PIN_A PD2  //pin 2
  #define LEFT_ENC_PIN_B PD3  //pin 3
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_ENC_PIN_A PC2  //pin A2
  #define RIGHT_ENC_PIN_B PC3   //pin A3

#ifdef L298P_4WD
  #define LEFT_H_ENC_PIN_A PD4  //pin 4
  #define LEFT_H_ENC_PIN_B PD5  //pin 5
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_H_ENC_PIN_A PC0  //pin A0
  #define RIGHT_H_ENC_PIN_B PC1   //pin A1
#endif  
#endif

从此文件中可以看到在原来2驱的基础上增加了4WD模式下的编码器连接引脚定义,其中左后方的电机AB项连接数字IO的D4和D5,而右后方电机AB项连接模拟IO的A0和A1,这样4个马达的编码器数据读取我们就用到8个IO口,加上PWM控制,转动方向的控制,在Diego 1# 4WD版本中,底盘控制一共用了16个IO,最后只剩D0,D1作为串口和上位机通讯,和A4,A5作为I2C的接口与I2C接口模块通讯,可以说Arduino UNO做到了充分利用。

在encoder_driver.ino文件中增加对4WD新增引脚的中断处理。

arduino uno中一共有3个引脚中断函数分别是

  • ISR (PCINT0_vect)对应 D8 to D13
  • ISR (PCINT1_vect) 对应 A0 to A5
  • ISR (PCINT2_vect) 对应 D0 to D7

diego1# 4wd中只需要用到两个中断处理函数 ISR(PCINT2_vect)和ISR(PCINT1_vect),代码如下:

  ISR (PCINT2_vect){
     static uint8_t enc_last=0;
#ifdef L298P_4WD        
     static uint8_t enc_last_h=0;
#endif          
     enc_last <<=2; //shift previous state two places
     enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits

#ifdef L298P_4WD
     enc_last_h<<=2;
     enc_last_h |=(PIND & (3 << 4))>>4;
#endif 
  
     left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
#ifdef L298P_4WD   
     left_h_enc_pos +=ENC_STATES[(enc_last_h & 0x0f)];
#endif    
  }
  
  /* Interrupt routine for RIGHT encoder, taking care of actual counting */
  ISR (PCINT1_vect){
     static uint8_t enc_last=0;
#ifdef L298P_4WD        
     //uint8_t pinct=PINC;
     static uint8_t enc_last_h=0;
#endif   	
     enc_last <<=2; //shift previous state two places
     enc_last |= (PINC & (3 << 2)) >> 2; //read the current state into lowest 2 bits

#ifdef L298P_4WD
     enc_last_h<<=2;
     enc_last_h |=(PINC & 3);
#endif  
     right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
#ifdef L298P_4WD   
     right_h_enc_pos +=ENC_STATES[(enc_last_h & 0x0f)];
#endif
  }

这段代码中主要是针对中断寄存器PINC和PIND的操作,每个IO pin都对应PINC或者PIND的一位,IO有中断产生时,对应的位就会被置位,我们只需要读取相应为即可,如(PIND & (3 << 4))>>4读取的就是PIND的第4,5位,也就是D4,D5的数据。

Posts navigation

1 2 3 4 5 6
Scroll to top