分类目录归档:机器人

PS3手柄控制EV3

在ev3dev官方网站提供的爱好者Projects中,开心的发现了其中有对PS3手柄的支持。

于是根据project顺藤摸瓜,研究了一下PS3手柄的调用方式,如下:

一、连接手柄

打开EV3里面的蓝牙功能,然后将PS3手柄开启,首次使用配对需要用USB线连接EV3和PS3手柄。此时EV3屏幕会显示找到PS3手柄蓝牙设备,是否配对,Accept后连接成功。

二、调用方式

利用evdev模块的InputDevice()函数可以调用PS3手柄。这个evdev.InputDevice()函数可以调用所有EV3支持的输入设备,只是需要传入设备文件名作为函数参数。

如何知道PS3手柄的设备文件名?evdev模块的list_devices()函数可以列出所有连接的设备,而不同设备的名称并不相同,注意这里的设备名称与设备文件名不同。熟悉Linux的话,可以知道设备文件名是设备映射到系统中/dev目录下的某个系统文件。而设备名称则是给人看的设备描述,例如:PLAYSTATION(R)3 Controller就是PS3手柄的设备名称。这样就可以通过设备名称反向获得设备文件名,从而实现InputDevice()的参数传入。

写了这么多有点绕,不如Code直接一些:

import evdev

# 获取所有已连接的输入设备
devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()]

# 找出PS3手柄对应的输入设备文件名
for device in devices:
  if device.name == 'PLAYSTATION(R)3 Controller':
  ps3dev = device.fn

# 连接PS3手柄
gamepad = evdev.InputDevice(ps3dev)

三、获取手柄动作

PS3手柄有2个摇杆,17个按键(摇杆也可以按),在使用上分为摇杆模式和按键模式,当然这两种模式可以共存,但ev3dev在调取摇杆和按键的读值时会有一定区别。

在获取PS3手柄动作上,共定义了3个变量,分别是type, code, value。

  • type:手柄所使用的模式:摇杆or按键
  • code:摇杆or按键代码
  • value:摇杆or按键读值

来张图可以很清晰看

PS3手柄对应ev3dev参数

PS3手柄对应ev3dev参数

在value上按键和摇杆还是有区别的:按键只是按下(1)和抬起(0)两个动作;摇杆更复杂些,需要精确读取X和Y方向上的位置,精度为256,读值范围:0-255。

更新:还是将Anton Vanhoucke做好的图贴出来吧,更好看一些……我也test好久才画出来的好不: (

PS3 event codes

PS3 event codes

获取参数的方法,是利用read_loop()函数持续读取所有按键信息,之后可通过type、code、value读取。还是用Code解释:

for event in gamepad.read_loop():
  print event.type, event.code, event.value

四、EV3机器人

设计一个EV3小车,用PS3手柄控制他的移动,EV3搭建采用Lauren Valk的EXPLOR3R机器人,搭建图在这里

explor3r

explor3r

五、程序设计

#!/usr/bin/python

import evdev
import ev3dev.auto as ev3
import threading
import time

#Helpers

#def clamp(n, (minn, maxn)):
def clamp(n, *mn):
  """
  Given a number and a range, return the number, or the extreme it is closest to.
  :param n: number
  :param *mn: mn[0] minn, mn[1] maxn
  :return: number
  """
  return max(min(mn[1], n), mn[0])


def scale(val, src, dst):
  """
  Scale the given value from the scale of src to the scale of dst.
  val: float or int
  src: tuple
  dst: tuple
  example: print scale(99, (0.0, 99.0), (-1.0, +1.0))
  """
  return (float(val - src[0]) / (src[1] - src[0])) * (dst[1] - dst[0]) + dst[0]

# 将0~255范围的值 按比例 转换为-100~100范围的值
def scalestick(value):
  return scale(value,(0,255),(-100,100))

# 确保转换后的值 始终在-100~100范围内
def dc_clamp(value):
  duty_cycle_range = (-100, 100)
  return clamp(value, *duty_cycle_range)

print "Finding ps3 controller..."
devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()]
for device in devices:
  if device.name == 'PLAYSTATION(R)3 Controller':
  ps3dev = device.fn

gamepad = evdev.InputDevice(ps3dev)


turn_speed = 0
fwd_speed = 0
running = True

class MotorThread(threading.Thread):
  def __init__(self):
    # 两个马达分别接在A和D口
    self.right_motor = ev3.LargeMotor(ev3.OUTPUT_A)
    self.left_motor = ev3.LargeMotor(ev3.OUTPUT_D)
    threading.Thread.__init__(self)

  def run(self):
    print "Engines running!"
    while running:
      # 2个马达分别控制2个轮子,所以跟1个马达控制转向、1个马达控制前进后退不同
      # 在前进和后退时,相互配合转弯的控制相反
      if fwd_speed < 0:
        self.left_motor.run_forever(duty_cycle_sp = dc_clamp(-fwd_speed-turn_speed))
        self.right_motor.run_forever(duty_cycle_sp = dc_clamp(-fwd_speed+turn_speed))
      else:
        self.left_motor.run_forever(duty_cycle_sp = dc_clamp(-fwd_speed+turn_speed))
        self.right_motor.run_forever(duty_cycle_sp = dc_clamp(-fwd_speed-turn_speed))

    self.left_motor.stop()
    self.right_motor.stop()

if __name__ == "__main__":
  motor_thread = MotorThread()
  motor_thread.setDaemon(True)
  motor_thread.start()

  for event in gamepad.read_loop(): #this loops infinitely
    if event.type == 3: #A stick is moved

      if event.code == 1: #Y axis on left stick
        fwd_speed = scalestick(event.value)

      if event.code == 0: #X axis on left stick
        turn_speed = scalestick(event.value)

    if event.type == 1 and event.code == 302 and event.value == 1:
      print "X button is pressed. Break."
      running = False
      time.sleep(0.5) # Wait for the motor thread to finish
      break

[已解决]win7下使用ev3dev-2015-12-30版本黑屏死机的问题

按照官方关于ev3dev-jessie-2015-12-30版本介绍

There are lots of changes in this release, so even if you are not new to ev3dev check out the Getting Started guide.
这个版本中出现了较大的变化,及时你是一个ev3dev的老玩家最好也看看入门教程。

而在我翻译ev3dev接口文档的过程中,确实发现了许多跟以前不一样的地方。最新版的镜像把之前许多“混乱”的内容进行了整理。因此决定更新到最新版的ev3dev系统。

可就在我安装完镜像,在Windows7也完成了Remote NDIS驱动的安装,当通过ssh远程登录EV3时,系统一片黑屏,死机了。在排除了系统安装问题、USB接口问题、USB线路问题之后,我认为应该是设备驱动出了问题。

而确定这个问题和寻找解决办法的过程异常痛苦,因为关于ev3dev系统使用的搜索内容非常少,而出现这个问题的搜索结果更是没有,更重要的是——每一次尝试都面临着死机、等待、重启。我只能一点一点捋,从设备类型、驱动名称、驱动文件名开始搜索可能的原因。

在这中间我确实放弃了一段时间。

最后终于在升级了的一个文件之后问题得以解决,至少目前为止还没有出现黑屏情况。关于升级的文章链接:https://support.microsoft.com/zh-cn/kb/2719857

解魔方机器人[六]-还原动作

当利用可执行文件处理打乱的魔方,会得到一串类似F1U2B1D3……的字符,这些字符代表了还原魔方的动作,了解他们的具体含义就是最后我们需要解决的问题,而这个问题跟前面的比起来要简单得多。

6.1 动作表示

魔方一共有6个面,针对某1个面我们来拧他,分解后实际会得到3个基本动作:顺时针90度、顺时针180度、逆时针90度。看到这些有没有觉得亲切?这与之前我们分解EV3底座动作很类似。

对这3种动作用1 2 3来表示:

  • 1:顺时针90度
  • 2:顺时针180度
  • 3:逆时针90度

结合6个面的表示法U F R B L D,就可以完成每个面的动作表示。比如:F1表示F面顺时针90度、R2表示R面顺时针180度……

最后就是对这些魔方动作,结合EV3机器人的动作,完成程序设计。

6.2 程序设计

# 转魔方的机器人马达指令
# U面旋转
def twist_u_clockwise():
  base_anticlockwise()
  hand_turn()
  hand_press()
  base_anticlockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()
  base_clockwise()
def twist_u_anticlockwise():
  base_anticlockwise()
  hand_turn()
  hand_press()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()
  base_clockwise()
def twist_u_reverse():
  base_anticlockwise()
  hand_turn()
  hand_press()
  base_clockwise()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()
  base_clockwise()

# D面旋转
def twist_d_clockwise():
  base_clockwise()
  hand_turn()
  hand_press()
  base_anticlockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()
  base_anticlockwise()
def twist_d_anticlockwise():
  base_clockwise()
  hand_turn()
  hand_press()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()
  base_anticlockwise()
def twist_d_reverse():
  base_clockwise()
  hand_turn()
  hand_press()
  base_clockwise()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()
  base_anticlockwise()

# F面旋转
def twist_f_clockwise():
  hand_turn()
  hand_turn()
  hand_press()
  base_anticlockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_raise()
def twist_f_anticlockwise():
  hand_turn()
  hand_turn()
  hand_press()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_raise()
def twist_f_reverse():
  hand_turn()
  hand_turn()
  hand_press()
  base_clockwise()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_raise()

# B面旋转
def twist_b_clockwise():
  hand_press()
  base_anticlockwise()
  hand_raise()
def twist_b_anticlockwise():
  hand_press()
  base_clockwise()
  hand_raise()
def twist_b_reverse():
  hand_press()
  base_clockwise()
  base_clockwise()
  hand_raise()

# L面旋转
def twist_l_clockwise():
  hand_turn()
  hand_press()
  base_anticlockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()
def twist_l_anticlockwise():
  hand_turn()
  hand_press()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()
def twist_l_reverse():
  hand_turn()
  hand_press()
  base_clockwise()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_turn()
  hand_turn()
  hand_raise()

# R面旋转
def twist_r_clockwise():
  hand_turn()
  hand_turn()
  hand_turn()
  hand_press()
  base_anticlockwise()
  hand_raise()
  hand_turn()
  hand_raise()
def twist_r_anticlockwise():
  hand_turn()
  hand_turn()
  hand_turn()
  hand_press()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_raise()
def twist_r_reverse():
  hand_turn()
  hand_turn()
  hand_turn()
  hand_press()
  base_clockwise()
  base_clockwise()
  hand_raise()
  hand_turn()
  hand_raise()

# 动作对应关系
move_step = {
  "U1":twist_u_clockwise,
  "U2":twist_u_reverse,
  "U3":twist_u_anticlockwise,
  "D1":twist_d_clockwise,
  "D2":twist_d_reverse,
  "D3":twist_d_anticlockwise,
  "F1":twist_f_clockwise,
  "F2":twist_f_reverse,
  "F3":twist_f_anticlockwise,
  "B1":twist_b_clockwise,
  "B2":twist_b_reverse,
  "B3":twist_b_anticlockwise,
  "L1":twist_l_clockwise,
  "L2":twist_l_reverse,
  "L3":twist_l_anticlockwise,
  "R1":twist_r_clockwise,
  "R2":twist_r_reverse,
  "R3":twist_r_anticlockwise
}
###==================================================###

###=================MAIN FUNCTION====================###
def main():
  # 扫描魔方
  cube_scan = buildup_cube_module()
  # 建立模型
  cube_module = cvt2reid(cube_scan)
  # 植入算法
  cube_solved = cube_algorithm(cube_module)

  # 分解动作
  for i in range(len(cube_solved)/2):
    solve_step = cube_solved[i*2]+cube_solved[i*2+1]
    # 执行动作
    move_step[solve_step]

  # 结束任务
  end_of_mission()

if __name__ == '__main__':
  main()
###=====================EOF==========================###

写在最后

从打算做这个项目到最后完成,前后用了大约2个月的时间。不得不说,在程序设计和实验的过程中遇到了在做之前没有想到的困难。也许这个系列的文章中简单的一段话,在实际做的过程中却是经过了很长时间多次的学习、研究才完成的。

本来是想每年用LEGO完成一个项目,定的计划也是2015年完成数字识别机器人、2016年完成解魔方机器人,但随着自己对EV3各类传感器、马达以及ev3dev系统和Python语言的不断熟悉,今年的任务也就提前完成了。

整理文字的过程是一个很枯燥繁琐的过程,同样也是一个总结和分享的过程。不得不说,在写这段文字的时候还是很开心的。接下来做什么样的项目还没有想好,也许会跟网络编程或者物联网开放平台相关。

如果你对我关注的东西也感兴趣,欢迎共同探讨。

解魔方机器人[五]-植入算法

到此为止,魔方的扫描与计算机表示都已经完成,解决步骤的算法实现,应该说与EV3传感器和马达没有太多的关系,主要是由计算机完成的。魔方的解决是基于群论实现的,说实话我本人并没有对这种理论算法做进一步深入的研究,因为他确实需要时间去学习。好在一些网站早已经将这部分算法实现,并开源出来。Tomas Rokicki的个人主页包含了大量的算法和程序实现,感兴趣的同学强烈推荐:http://tomas.rokicki.com/

5.1 算法程序编译与使用

Tomas Rokicki曾发起过一次解魔方编程活动,所有参赛的程序都是开源的。同样,感兴趣研究魔方算法的同学可以自行围观:http://tomas.rokicki.com/cubecontest/winners.html

如前所述,我只是需要在EV3中使用这些程序,下载其中一个代码,然后按照代码所使用的编程语言进行编译,即可得到可执行的文件。

在此我使用页面中排名第2的程序(pochmann),作者使用C++,因此我们使用C++编译器将代码编译为可执行程序。以g++编译器为例,执行:

g++ JohnnyX.cpp -o pochmann.exe

所获得的pochmann.exe即为解魔方算法的可执行文件。参照这个程序的使用说明,程序的使用方法是:

  • 输入:可执行文件 魔方的状态表示
  • 输出:魔方的解法

新的问题出现了,程序输入中的魔方的状态表示是什么样的?跟我们在上一章整理的表达式有什么关系吗?

5.2 魔方色块表示法

先来看个结论,采用色块表示法,一个还原的魔方可表示为:

UF UR UB UL DF DR DB DL FR FL BR BL UFR URB UBL ULF DRF DFL DLB DBR

不要被这个东西吓到,我们慢慢来看,就会发现并没有那么难。

字母含义

这一串字符中,一共包含了6个字母:U F R B L D,分别代表了一个魔方的上面(Up)、前面(Front)、右面(Right)、后面(Back)、左面(Left)、底面(Down)

魔方的轴

也许你会发现一个问题,就是用这6个字母表示魔方的6个面,如果魔方变换了方位那6个面的方向不是也变了?这就需要说明一下魔方的轴。

如果你手边有一个魔方,那么仔细注意魔方6个面的中心点,然后保持中心点的方向不动,试着去转动6个面,你一定会发现,无论你怎么转动这些面,这6个中心点的位置是不会发生变化的。

比如下面这两幅图:打乱前和打乱后,白色、绿色、红色面的中心点并未变化。也即魔方的轴在拧魔方的过程中,相对位置是不会发生变化的。

还原状态

还原状态

打乱状态

打乱状态

魔方的色块

魔方由8个角块、12个棱块和6个中心块组成。中心块的位置固定在十字架上。每个角块3面有颜色、每个棱块2面有颜色、每个中心块1面有颜色。

cube block

由于中心块的相对位置不变,对于角块和棱块可以用每个面所属的方向进行描述。依然,一个还原的魔方棱块和角块可以表示为:

  • 棱块 UF UR UB UL DF DR DB DL FR FL BR BL
  • 角块 UFR URB UBL ULF DRF DFL DLB DBR

新的问题又来了,一个打乱的魔方怎么表示?

打乱魔方的色块表示

首先我们还是来看那个还原的魔方表示:

UF UR UB UL DF DR DB DL FR FL BR BL UFR URB UBL ULF DRF DFL DLB DBR

现在已经知道了那么表示棱块和角块,但其实这个表示是包含两个隐含属性的,分别是:色块位置、颜色方向

  • 色块位置 UF UR UB UL DF DR DB DL FR FL BR BL UFR URB UBL ULF DRF DFL DLB DBR
  • 颜色方向 UF UR UB UL DF DR DB DL FR FL BR BL UFR URB UBL ULF DRF DFL DLB DBR

色块位置很好理解,正如前文所述,就是每个色块位置的方向型表示。而颜色方向可以理解为,色块某一面的颜色与某个中心色块颜色相同,那么这个色块某一面的颜色所属方向,即为那个中心色块颜色的方向。

比如,在下面这个魔方里面,位置处于FR的色块,色块的F面是红色、R面是蓝色,分别与魔方F面和U面的中心点的颜色一样,所以这个色块的颜色方向为:FU。

色块表示

色块表示

如果理解了上面这个例子,那么再来看色块位置和颜色方向两行的表示,可以说角块的表示方法只是按照“颜色方向”对魔方进行描述,而描述的顺序是按照“色块位置”执行的。

5.3 程序设计

# 将6x9个面的表示转换为12+8个色块的表示
#                   +--5U-+
#                   |0 1 2|
#                   |3 4 5|
#                   |6 7 8|
# +--0F-+--1R-+--2B-+--3L-+
# |0 1 2|0 1 2|0 1 2|0 1 2|
# |3 4 5|3 4 5|3 4 5|3 4 5|
# |6 7 8|6 7 8|6 7 8|6 7 8|
# +-----+-----+-----+--4D-+
#                   |0 1 2|
#                   |3 4 5|
#                   |6 7 8|
#                   +-----+
# 输入:6x9个面的表示法
# 输出:12+8个块的表示法

def cvt2reid(orignal):
  c = str(orignal)

  # 各个面的中心块代表该面的颜色方向
  c2r = {c[5][4]:'U', c[0][4]:'F', c[1][4]:'R', c[2][4]:'B', c[3][4]:'L', c[4][4]:'D'}

  # 按照指定规律进行色块组合
  reid_mode = c2r[c[5][5]]+c2r[c[0][1]]+' '+c2r[c[5][1]]+c2r[c[1][1]]+' '+c2r[c[5][3]]+c2r[c[2][1]]+' '+c2r[c[5][7]]+c2r[c[3][1]]+' '+\
              c2r[c[4][5]]+c2r[c[0][7]]+' '+c2r[c[4][7]]+c2r[c[1][7]]+' '+c2r[c[4][3]]+c2r[c[2][7]]+' '+c2r[c[4][1]]+c2r[c[3][7]]+' '+\
              c2r[c[0][5]]+c2r[c[1][3]]+' '+c2r[c[0][3]]+c2r[c[3][5]]+' '+c2r[c[2][3]]+c2r[c[1][5]]+' '+c2r[c[2][5]]+c2r[c[3][3]]+' '+\
              c2r[c[5][2]]+c2r[c[0][2]]+c2r[c[1][0]]+' '+c2r[c[5][0]]+c2r[c[1][2]]+c2r[c[2][0]]+' '+\
              c2r[c[5][6]]+c2r[c[2][2]]+c2r[c[3][0]]+' '+c2r[c[5][8]]+c2r[c[3][2]]+c2r[c[0][0]]+' '+\
              c2r[c[4][8]]+c2r[c[1][6]]+c2r[c[0][8]]+' '+c2r[c[4][2]]+c2r[c[0][6]]+c2r[c[3][8]]+' '+\
              c2r[c[4][0]]+c2r[c[3][6]]+c2r[c[2][8]]+' '+c2r[c[4][6]]+c2r[c[2][6]]+c2r[c[1][8]]

  # 返回色块表示列表
  return reid_mode

# 借助编译好的解魔方可执行文件,完成魔方还原步骤
# 此处使用pochmann提供的解决方法
def cube_algorithm(cube_unsolved):
  cube_solved = os.popen('pochmann.exe '+cube_unsolved).readline()
	
  # 返回还原魔方的步骤
  return cube_solved

解魔方机器人[四]-建立模型(下)

4.3 调整顺序

虽然魔方每个面都扫描完了,但是有没有发现什么问题?没错,色块的顺序有点乱,通常我们习惯下面这种顺序来表示:

                   +--5--+
                   |0 1 2|
                   |3 4 5|
                   |6 7 8|
 +--0--+--1--+--2--+--3--+
 |0 1 2|0 1 2|0 1 2|0 1 2|
 |3 4 5|3 4 5|3 4 5|3 4 5|
 |6 7 8|6 7 8|6 7 8|6 7 8|
 +-----+-----+-----+--4--+
                   |0 1 2|
                   |3 4 5|
                   |6 7 8|
                   +-----+

调整顺序其实就是某两个色块进行对调,同样并没有什么难度,需要的只是耐心和细心。

程序设计

# 输入未排序的魔方
def scan_reshape(orignal):
  result = np.zeros((6,9), np.uint8)
  for i in range(6):
    if i < 4:
      result[i,0] = orignal[i,4]
      result[i,1] = orignal[i,1]
      result[i,2] = orignal[i,2]
      result[i,3] = orignal[i,0]
      result[i,4] = orignal[i,3]
      result[i,5] = orignal[i,6]
      result[i,6] = orignal[i,7]
      result[i,7] = orignal[i,8]
      result[i,8] = orignal[i,5]
    else:
      result[i,0] = orignal[i,4]
      result[i,1] = orignal[i,8]
      result[i,2] = orignal[i,5]
      result[i,3] = orignal[i,2]
      result[i,4] = orignal[i,1]
      result[i,5] = orignal[i,0]
      result[i,6] = orignal[i,3]
      result[i,7] = orignal[i,6]
      result[i,8] = orignal[i,7]

  # 返回排好顺序的魔方
  return result

4.4 建立模型程序设计

# 扫描3遍取平均值作为扫描结果,按照顺序建立魔方模型
def buildup_cube_module():
  for i in range(3):
    ev3_tmp = scan_every_side()
    ev3_arr_rgb += ev3_tmp/3

  # RGB转HSV
  ev3_arr_hsv = cv2.cvtColor(ev3_arr_rgb, cv2.COLOR_RGB2HSV)

  # 取H值作为颜色区分的特征值
  ev3_arr_h = ev3_arr_hsv[:,:,0]
  h = ev3_arr_h.reshape(6*9)

  # 按照H值所处排序位置,确定色块的颜色类别
  pos = sort_pos(h).reshape(6, 9)

  # 9种颜色分别用0-8的数字表示
  for i in range(6):
    for j in range(9):
      h[pos[i,j]] = i
  ev3_arr_h = h.reshape(6, 9)
  ev3_cube = scan_reshape(ev3_arr_h)

  # 返回魔方模型
  return ev3_cube

4.5 引申阅读

下面这种方法通过RGB颜色空间进行色块划分,虽然我并没有采用,但从理论上我觉得是说得通的,感兴趣的同学可以看看并试验。

由于魔方需要识别的颜色和色块(点)也较少,因此可以考虑直接采用与每个面中心点的RGB值比较的方式进行归类。

当魔方转动时,其6个面中心点的绝对位置其实是不发生变化的。如果你把魔方安装下面这个方向放置,会发现无论怎样转动上面(UP)的中心点永远是白色、前面(FRONT)的中心点永远是绿色……

还原状态

还原状态

而RGB值的色彩空间模型实际上是一个以R、G、B为坐标轴值的一个正方体。这样可以想象6个面中心点的颜色也就分布在这个正方体的6个位置。

RGB模型

RGB模型

其余的48个色块所对应的RGB值,应该与各中心点之一的值接进,比如:所有白色色块的RGB值与上面(UP)中心点的值基本接近、所有绿色色块的RGB值与前面(FRONT)中心点的值基本接近……这种颜色的接近反映在RGB空间模型上,可以很好的理解为正方体上面有6×8个点分别与此前6个点的距离比较近。而这个距离又可以理解为三维空间的欧几里得距离,通过公式容易计算得出:

颜色差异 =  √((色块R值-中心R值)^2 + (色块G值-中心G值)^2 + (色块G值-中心G值)^2)

再对这48个差异值从小到大排序,前8位的色块就是与该中心点颜色相同的色块。

写在本章最后

如果你分步骤的执行程序会发现一个问题,就是色彩传感器经常无法对准每一个魔方色块,这也就直接导致了无法准确扫描到该色块的颜色,造成识别误差。这个问题目前还没有更好的解决办法,即使采用原作者的程序解开的成功率也并不高,原因就在于此。可以使用的办法就是多扫描几遍,或者在扫描时多取几个扫描点,最终取平均值作为扫描结果。

解魔方机器人[四]-建立模型(上)

我们已经得到了6×9个RGB值,但这些都代表什么颜色?我们需要的是不同色块对应到6种不同的颜色,因此必须对这54个RGB值进行划分,使他们归到自己的颜色分类中。关于颜色分类其实属于数字图像处理学中的一个重要研究领域,在此仅对比较通用的一种方法进行介绍。

4.1 色彩模型转换

色彩模型是描述使用一组值(通常使用3个、4个值或者颜色成分)表示颜色方法的抽象数学模型。例如三原色光模式(RGB) 和印刷四分色模式(CMYK)都是色彩模型。EV3颜色传感器获得的就是RGB模型,可以表示为三维直角坐标颜色系统的一个单位正方体。

在大多数的彩色图形显示设备一般都是使用红、绿、蓝三原色,我们的真实感图形学中的主要的颜色模型也是RGB模型,但是红、绿、蓝颜色模型用起来不太方便,它与直观的颜色概念如色调、饱和度和亮度等没有直接的联系。比如,我们很难从RGB的取值上区分这个颜色属于哪个色域,因此根据不同应用,还有HSV、HSI、CHL、LAB、CMY等其他色彩模型。由于颜色本身并没有发生变化,只是各个色彩模型表示颜色的方法不同,色彩模型之间可以通过固定的公式进行相互转换。

HSV、HSI、LAB都可以很好的将色相以一定规律排列出来。在本项目中,使用HSV模型作为颜色识别的色彩模型。关于HSV模型的详细描述,可以参见这里

交给OpenCV

在程序实现方面,我更建议使用已经投入大量应用的函数库,而不是自己重新写一遍。因为已投入应用的函数库经过了长期的优化,能够保证实现的效率和安全。尽管RGB转HSV的程序实现很简单,只要按照公式直译成程序即可,但我还是使用了OpenCV的cvtColor函数。

在此之前,需要按照OpenCV模块,官方网站给出了比较详细的安装方法。ev3dev基于Debian系统,可以通过apt-get install python-opencv指令进行模块及相关依赖包的安装。

OpenCV为图像处理提供了大量的算法实现函数,在处理魔方色块时,我们可以将6×9个色块理解为一张大小为6×9的图像,每个色块为图像中的一个像素点,然后对这张图像进行RGB到HSV的转换。

对于这个图像与魔方色块的对应表示关系为:

  • [0,0]表示图像第1行第1列的像素点,对应魔方上为第1个扫描面的第1个扫描色块
  • [0,1]表示图像第1行第2列的像素点,对应魔方上为第1个扫描面的第2个扫描色块
  • [3,4]表示图像第3行第4列的像素点,对应魔方上为第3个扫描面的第4个扫描色块

OpenCV的程序实现

# 仅为示例,非实际使用程序
image_hsv = cv2.cvtColor(image_rgb, cv2.COLOR_RGB2HSV)

此时得到的image_hsv为一张大小为6×9的HSV图像,image_hsv[0,0,0]为第1行第1列的H值,image_hsv[0,0,1]为第1行第1列的S值,image_hsv[0,0,2]为第1行第1列的V值。

4.2 色块划分

利用HSV色彩空间的性质,通过对所有色块H值的排序,然后再以每9个色块为一组,可以得到每个色块的分组,也就得到了该色块所属的颜色类别。

通过一个简单的例子能帮助更好理解。假设我们有9个色块,一共3种颜色,这9个色块的H值分别如下:

位置 [0] [1] [2] [3] [4] [5] [6] [7] [8]
H值 22 121 25 186 190 118 24 124 188

接下来我们按这9个色块的H值从小到大排序,排序的同时他们的位置也随着调整:

位置 [0] [6] [2] [5] [1] [7] [3] [8] [4]
H值 22 24 25 118 121 124 186 188 190

我们知道一共有3种颜色,按照这个排序,每3个色块归为一组。也就是说位置在[0][6][2]的色块是同一种颜色,[5][1][7]为同一种颜色,[3][8][4]为同一种颜色。

位置排序程序

如果理解了以上的划分原理,开始写程序会发现一个问题。与通常的排序不同,这里不是只对位置上的值排序,而是通过位置上值的大小完成对位置的排序。在程序设计过程中,需要额外增加一个变量来记录位置的变换。

# 按照数组值的大小完成数组位置的排序
# 输入:l为要排序的数组
def sort_pos(l):

  # 定义pos数组,与l相同大小,初始化为l的原始位置
  pos = np.arange(len(l))
  for i in range(len(l)-1):
    for j in range(len(l)-i-1):

      # 按照l数组的值的大小进行比对
      if l[j] > l[j+1]:

        # 调整pos的值,即记录l位置的变化
        pos[j], pos[j+1] = pos[j+1], pos[j]

  # 返回排序后位置的变化情况
  return pos

这一节确实有一些难理解,先多看看上面表格里排序的例子,再去试着理解程序会更好一些。

解魔方机器人[三]-魔方扫描

拥有了基本动作这些工具之后,回想最开始的方案设计,我们现在该着手解决系统中的“输入”了。输入即EV3对魔方进行扫描,通过计算机语言数字化的对魔方进行表示。仍然本着任务分解的思路,我们将扫描魔方这个任务继续分解为更小的任务:扫描色块、扫描单面、扫描六面。

3.1 扫描色块

上一章已经介绍,扫描魔方时色彩传感器为RGB模式,因此在扫描后会得到R、G、B值,每个色块的颜色就需要采用包含3个值的数组来存储。例如:color[0]=R、color[1]=G、color[2]=B。

程序设计

# 这个示例仅为说明RGB采集,并非实际应用于机器人中的程序
for i in range(3):
  color[i] = cs.value(i)

3.2 扫描单面

EV3每次扫描最上面的面,我们按照先中间再四周的顺序进行扫描,单面一共9个色块,扫描流程为:

  1. 扫描中间块
  2. 顺时针转45度
  3. 扫描角块
  4. 顺时针转45度
  5. 扫描边块
  6. 顺时针转45度
  7. 扫描角块
  8. 顺时针转45度
  9. 扫描边块
  10. 顺时针转45度
  11. 扫描角块
  12. 顺时针转45度
  13. 扫描边块
  14. 顺时针转45度
  15. 扫描角块
  16. 顺时针转45度
  17. 扫描边块
  18. 探头收回

从机器人的正面俯视,容易知道每个色块的扫描顺序。

======+-----+  +-----+
      [	    ]  |3 2 1|   +=============
hand motor  ]  |4 0 8|  [X]color sensor
      [	    ]  |5 6 7|   +=============
======+-----+  +-----+

程序设计

# 扫描魔方单面
def scan_one_side():
  # 定义一个9x3的二维数组,存放9个色块的RGB值
  ev3_scan_side = np.zeros((9,3), np.uint8)
  # 扫描中间块
  head_center()
  for i in range(3):
    cv = cs.value(i)
    if cv<=255:
      ev3_scan_side[0,i] = cv
    else:
      ev3_scan_side[0,i] = 255
  # 扫描四周
  for j in range(8):
    base_step()
    if j%2 == 0:
      head_side_coner()
    else:
      head_side_edge()
    for i in range(3):
      cv = cs.value(i)
      if cv<=255:
        ev3_scan_side[j+1,i] = cv
      else:
        ev3_scan_side[j+1,i] = 255
  # end_of_mission()函数实际为hand_raise()和head_back()函数的组合,确保机器人恢复初始化状态
  end_of_mission()

  # 返回扫描单面后的二维数组
  return ev3_scan_side

3.3 扫描六面

扫描六面基本思想就是扫描完一面后,将魔方翻转,再扫描另一面。需要注意的是:面向机器人的魔方正面和背面在扫描的时候,需要先进行底座旋转,并且此时扫描后得到的色块顺序相对于其他面也是不一样的。尤其是色块顺序,在之后对魔方进行建模时非常重要,其实也并不难,只是需要足够的耐心来认真确认。扫描流程:

  1. 扫描单面
  2. 翻动魔方
  3. 扫描单面
  4. 翻动魔方
  5. 扫描单面
  6. 翻动魔方
  7. 扫描单面
  8. 底座逆时针转90度
  9. 翻动魔方
  10. 扫描单面
  11. 翻动魔方
  12. 翻动魔方
  13. 扫描单面
  14. 底座顺时针转90度
  15. 翻动魔方
  16. 底座顺时针转90度

定义从机器人的正面俯视魔方的面为第1个面,得到魔方各个面及色块的扫描顺序:

                   +--5--+
                   |5 4 3|
                   |6 0 2|
                   |7 8 1|
 +--0--+--1--+--2--+--3--+
 |3 2 1|3 2 1|3 2 1|3 2 1|
 |4 0 8|4 0 8|4 0 8|4 0 8|
 |5 6 7|5 6 7|5 6 7|5 6 7|
 +-----+-----+-----+--4--+
                   |5 4 3|
                   |6 0 2|
                   |7 8 1|
                   +-----+

程序设计

def scan_every_side():
  定义一个6x9x3的三维数组保存魔方6个面每个面9个色块的RGB值
  ev3_every_side = np.zeros((6,9,3), np.uint8)
  for i in range(6):

    # 扫描第1个面
    if i==0:
      ev3_every_side[i,:] = scan_one_side()

    # 扫描第2-4个面
    elif i>0 and i< 4:
      hand_turn()
      hand_raise()
      ev3_every_side[i,:] = scan_one_side()

    # 扫描第5个面
    elif i==4:
      base_anticlockwise()
      hand_turn()
      hand_raise()
      ev3_every_side[i,:] = scan_one_side()

    # 扫描第6个面
    elif i==5:
      hand_turn()
      hand_turn()
      hand_raise()
      ev3_every_side[i,:] = scan_one_side()
      base_clockwise()
      hand_turn()
      hand_raise()
      base_clockwise()

  # 返回保存扫描结果的三维数组
  return ev3_every_side

解魔方机器人[二]-基本动作(下)

如果理解了底座马达的基本动作,就会很好整理出手臂马达和探头马达的基本动作。唯一区别是,手臂马达和探头马达由于动作比较固定,采用的位置控制方式均为绝对位置。

另外还有一个机器人的输入设备,颜色传感器。颜色传感器的主要作用就是扫描魔方色块的颜色,在传感器工作模式上与LEGO官方定义的三种模式(环境光、反射光、颜色)也略有不同。

手臂马达

机器人的手臂动作主要包括:

  • 完全抬起,这样可以进行扫描和方向旋转
  • 按住魔方,这样可以配合底座进行拧魔方
  • 拉推魔方,这样可以将魔方翻面,而这个动作又可以分为拉和推

对应于马达的基本动作为:

  • 抬起
  • 按住
  • 拉回(推与按住相同)

按照机器人的搭建结构,完成这4种基本动作是通过大型马达所转动的位置实现的。我们设定手臂抬起是马达的初始化状态,即此时位置为0,之后通过试验可以很容易的得到每种动作所对应的马达的位置。

抬起

抬起

按下

按下

拉起

拉起

程序设计

# EV3手臂马达连接在端口B
hand = ev3dev.large_motor('outB')
if hand.connected == True:
  hand.speed_regulation_enabled = 'off'
  hand.duty_cycle_sp = 65
  hand.stop_command = 'hold'

# 设定手臂完全抬起时为初始化状态,位置值position = 0
def hand_raise():
  hand.run_to_abs_pos(position_sp=0)
  while 'holding' not in hand.state: time.sleep(0.2)
# 手臂按住
def hand_press():
  hand.run_to_abs_pos(position_sp=110)
  while 'holding' not in hand.state: time.sleep(0.2)

# 手臂拉起,确保拉起的位置比魔方翻动时的最高点要高
def hand_pull():
  hand.run_to_abs_pos(position_sp=233)
  while 'holding' not in hand.state: time.sleep(0.2)

# 翻动模式,为拉起和按下的动作组合
def hand_turn():
  hand_pull()
  hand_press()

探头马达

探头马达用来控制颜色传感器进行定位扫描,按照扫描位置的不同可以分为:中心块、边块、角块,再加上完全收起的位置,因此探头马达的基本动作有4种:

  • 收起
  • 扫描中心块
  • 扫描边块
  • 扫描角块

按照搭建结构,完成以上动作是通过中型马达所转动的位置实现。我们设定探头收起是马达的初始化状态,即此时位置为0,之后通过试验可以容易得到每种动作所对应的马达位置。

扫描中心

扫描中心

扫描边块

扫描边块

扫描角块

扫描角块

程序设计

# 探头马达为中型马达,连接在EV3的端口C上
head = ev3dev.medium_motor('outC')
if head.connected == True:
  head.speed_regulation_enabled = 'off'
  head.duty_cycle_sp = 50
  head.stop_command = 'hold'
else:
  print 'head is not connected'

# 探头收起,设定探头完全收起时为初始化位置,此时位置值为0
def head_back():
  head.run_to_abs_pos(position_sp=0)
  while 'holding' not in head.state: time.sleep(0.2)

# 扫描中心块
def head_center():
  head.run_to_abs_pos(position_sp=-380)
  while 'holding' not in head.state: time.sleep(0.2)

# 扫描边块
def head_side_edge():
  head.run_to_abs_pos(position_sp=-260)
  while 'holding' not in head.state: time.sleep(0.2)

# 扫描角块
def head_side_coner():
  head.run_to_abs_pos(position_sp=-212)
  while 'holding' not in head.state: time.sleep(0.2)

颜色传感器

颜色传感器在LEGO官方给出的模式有三种:环境光模式、反射光模式和彩色模式。其中彩色模式可以识别出7种不同的颜色,可惜这些颜色对于魔方的色块来说有些不适用,好在传感器实际的能力是可以识别出颜色的R、G、B值,因此采用RGB模式直接读取R、G、B值的方式来进行魔方色块的识别。

程序设计

# 连接色彩传感器,并设定为RGB模式
cs = ev3dev.color_sensor()
cs.mode = 'RGB-RAW'