分类目录归档:项目实验

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

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

当利用可执行文件处理打乱的魔方,会得到一串类似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'

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

要想让机器人完成解魔方的各种动作,我们先来分解每个马达的最基本的动作,然后通过这些基本的动作组合实现机器人更复杂的动作指令。根据马达的不同位置和作用,我们将EV3 MindCub3r的三个马达命名为:底座马达、手臂马达和探头马达。

底座马达基本动作

底座马达负责转动底座,自身可以完成魔方的旋转、配合手臂可以完成拧魔方最底层、配合探头可以完成魔方各个色块的颜色扫描。因此,底座马达的基本动作可以总结为:

  • 顺时针旋转90度
  • 逆时针旋转90度
  • 顺时针旋转180度(也可以采用逆时针,这里使用顺时针)
  • 顺时针旋转45度(也可以采用逆时针,区别在于色块扫描的顺序,这里使用顺时针)

第一个问题:如何让底座转90度?

仔细观察搭建图不难发现,动力系统是通过安装在大型马达上的一个小齿轮,带动底座下面的一个大齿轮完成的,并不很复杂。两个齿轮的传动比为1:3,即小齿轮转动3圈相当于大齿轮转动1圈。因此如果要底座转动90度,马达应转动270度,LEGO EV3的大型马达转1圈相应的走360步、即1步等于1度。

齿轮传动比1:3

齿轮传动比1:3

第二个问题:如何表示马达的运行位置?

EV3的马达属于伺服马达,这种马达的优势就是可以通过转动的步数精确计算马达的位置(position),无论马达处在停止还是运行状态都可以读取当前位置。对于位置可以这样理解:设想马达刚连接到EV3端口上,此时的位置为0;当顺时针转动一圈之后,此时的位置是360;再逆时针转一圈,位置又回到0;如果继续逆时针转动,位置就会变为相应的负数。如果持续的转动,位置的值也会持续的增加(或减少)。

对于马达运行位置的表示,EV3提供了两种方式:绝对位置、相对位置:

  • 绝对位置:采用这种表示方式,无论马达此时的位置值是多少,都会运行到设置的位置值。比如:此时马达的位置值是360,设置运行到绝对位置90,马达不会顺时针转动90步,而是逆时针转动270步,之后位置值变为90。
  • 相对位置:采用这种表示方式,运行的位置只是相对马达当前位置的位移。比如:此时马达的位置值是360,设置运行到绝对位置90,马达将会顺时针转动90步,之后位置值变为450。

对于底座马达,我们需要采用相对位置的运行方式。

第三个问题:马达每次停止都有一些位移误差?

显然,这种位移误差对于需要精准控制底座角度的需求是无法容忍的。要想解决这个问题,需要先了解EV3马达的停止模式。EV3马达有三种停止模式:coast、brake、hold:

  • coast模式:马达得到停止指令时,停止供电,随惯性再转会停止。
  • brake模式:马达得到停止指令时,停止供电,立刻停止转动,制动距离较coast模式要短得多。
  • hold模式:马达得到停止指令时,马达并不会停止供电,而是将马达稳稳的保持在停止时的位置,此时如果你用外力强制改变他的位置,马达会强有力的跟你较劲,还是会转回它的位置。

综上,使用hold模式能够更精准的控制马达所转动的角度,也不会使底座发生“变形”。

最后:马达的速度设置

EV3马达的速度有两种表达方式:速度值、占空比:

  • 速度值:马达的速度值单位为:步/秒,由于EV3马达一圈正好360步,因此等同于:角度/秒。
  • 占空比:使用占空比表达马达的速度,取值范围-100%到100%,100%为顺时针最大速度。

这两种方式对于底座马达都可以,通常如果对速度的精度需求不那么大的情况下,可以采用占空比的方式。

程序设计

import ev3dev

# 底座的EV3大型马达接入主控制器A口
base = ev3dev.large_motor('outA')

if base.connected == True:
  # 设置速度控制模式为占空比
  base.speed_regulation_enabled = 'off'
  # 设置马达停止模式为hold
  base.stop_command = 'hold'

# 底座顺时针旋转90度,占空比75%
def base_clockwise():
   base.run_to_rel_pos(position_sp=270, duty_cycle_sp=75)
   while 'holding' not in base.state: time.sleep(0.2)

# 底座逆时针旋转90度,占空比75%
def base_anticlockwise():
  base.run_to_rel_pos(position_sp=-270, duty_cycle_sp=75)
  while 'holding' not in base.state: time.sleep(0.2)

# 底座顺时针旋转45度,占空比30%
def base_step():
  base.run_to_rel_pos(position_sp=135, duty_cycle_sp=30)
  while 'holding' not in base.state: time.sleep(0.2)

解魔方机器人[一]-方案设计

目标

机器人解魔方的例子在网上已经有过很多,对于LEGO MINDSTORMS来说,从NXT开始就已经有了,EV3推出后许多玩家更是将解魔方机器人的搭建当成玩EV3的代表作品。那么机器人究竟是如何完成解魔方这件事情的,这是导致我这次选题来做的原因。

可惜我对工业设计并不了解,魔方的整体搭建基于David Gilday的结构,搭建图纸可以到他的官方网站下载:http://mindcuber.com

mindcub3r

本系列文章主要介绍程序设计方面的内容,一起探索如何从0开始设计解魔方机器人的程序实现。

设计思路

初次接触解魔方机器人可能会觉得无从下手,因此我们需要进行任务分解,将复杂的大任务逐步分解为简单的小任务。这种“解决问题”的系统都理解为三部分组成:输入、计算、输出,按照这种思路整理如下:

Input System Output
Scan
魔方扫描
Compute
算法计算
Solve
转动解决

动作分解

在分解动作之前,先来看看机器人所拥有的“能力”。EV3连接的主要设备包括:颜色传感器1个、大型马达2个、中型马达1个。各个设备的作用:

  • 颜色传感器:扫描魔方
  • 大型马达A:转动底座
  • 大型马达B:转动手臂
  • 中型马达:转动扫描头

而通过这些设备的组合,可以实现机器人所需要的基本动作,比如:拧魔方、翻转魔方、扫描每个色块。

接下来按照之前讨论的设计思路,对系统的三个部分分别进行动作分解。

输入-魔方扫描

先来想象一下扫描魔方的过程:扫描第1个面、翻转、扫描第2个面、翻转……扫描第6个面。

再想象扫描单面的过程:扫描中心块1、旋转45度、扫描角块2、宣传45度、扫描边块3……扫描边块9。

而关于翻转魔方,由于EV3机器人只能从一个方向翻转魔方,当翻转到第4个面后,需要将魔方旋转90度后才能继续将第5、第6个面翻转上来。

如果理解了扫描魔方的动作过程,那么每个马达所需要做的动作也就容易得出:

  • 大型马达(底座):顺时针转90度、逆时针转90度、顺时针转45度、逆时针转45度
  • 大型马达(手臂):抬起、扣住、后拉
  • 中型马达:定位中心块、定位角块、定位边块

计算-算法计算

通常算法是一些理论的实现,而工程设计是要将算法与实际相结合。就拿解魔方为例,解魔方的算法早已经得出,而计算机程序设计也已经实现,但EV3机器人若想应用这些算法和程序,就要做相应的兼容接口,为算法和机器人充当翻译的角色。

实现解魔方算法的程序往往已经有特定的输入与输出,因此EV3要做的第一件事就是要将扫描到的结果转化为与算法相同的表示方式;而计算之后还要将算法的输出转化为EV3能够识别的机器人动作。

不难想象,在这一步所需的动作分解:

  • 魔方表示方法的转化
  • 算法实现
  • 算法嵌入EV3程序

输出-转动解决

最后就是将已计算出的解魔方步骤转化为机器人动作,EV3的动作无法像人一样十指齐发多维度拧魔方,只能通过两个大型马达的配合每次拧最下面的一层,如果需要拧其他面就得先通过旋转、翻转把要拧的那一面挪到最下面才行。

魔方一共6个面,所以输出的动作分解可以理解为:

  • 拧正面:顺时针90度、逆时针90度、180度
  • 拧背面:顺时针90度、逆时针90度、180度
  • 拧上面:顺时针90度、逆时针90度、180度
  • 拧下面:顺时针90度、逆时针90度、180度
  • 拧左面:顺时针90度、逆时针90度、180度
  • 拧右面:顺时针90度、逆时针90度、180度

小结

通过本文可以从整体上对EV3解魔方的流程、环节、动作有一定了解,这些对下一步的任务实现具有非常重要的作用,尤其是对输入-计算-输出的理解,能够帮助我们在程序设计时能更加清晰的找到关键问题。