Webots平台NAO机器人寻路避障实现

一、实验目的

  在Webots机器人仿真平台上，使用NAO机器人在自建的场馆中完成寻路以及避障等功能。

二、功能模块

1. 场馆搭建

• 场馆中包含8幅画，为本次实验的8个目标点。场馆中央有两条长椅，在机器人前往不同的目标地点的途中会避开长椅，以展示避障功能。除此之外还添加了灯光以及鲜花，使场馆更加地真实自然。
  

2. 机器人功能

2.1 自动寻路

• NAO机器人可以在给定的8个目标点之间来回行走，并且不是按照预设固定路线，而是实时检测方位，理论上可以走到场馆中任意一个给定坐标的地方，实现机器人的智能化。

2.2 自动避障

• 在NAO机器人前往目标点的过程中，会利用声呐系统进行实时检测身体周围的障碍物，并对障碍物做出反应，尽可能地避开障碍物前往目标地点。

2.3 到达检测

• 当NAO机器人到达指定区域后，会挥手示意，之后便可下达下一个目标点的指令，继续前往一个目标点。

三、实现方法

1. 基础模块

• 部分继承于nao_demo.py。

Nao类变量

• ZERO_ANGLE：零度角设定
• IS_GO：是否前进标志位（默认为前进）
• Flag：go函数启动标志位（防止多个go函数同时启动造成冲突 ）
• ARRIVED：到达检测标志位
• ISDETECING：是否进行转向检测标志位
• Destinations：目标区域预设，每一行代表一个目标区域，前两个为x轴线，后两个为z轴参数

	PHALANX_MAX = 8
    ZERO_ANGLE  = 1.5828
    IS_GO = True
    Flag = True
    ARRIVED = False
    ISDETECTING = False
    Destinations = [
        [[-2.6, -2.00], [3.4, 2.62]],
        [[-2.6, -2.00], [0.4, -0.4]],
        [[-2.6, -2.00], [-2.4, -3.2]],
        [[-1.8, -1.4], [-3.2, -3.47]],
        [[1.4, 1.8], [-3.2, -3.47]],
        [[2.1, 2.4], [-2.4, -3.2]],
        [[2.1, 2.4], [0.4, -0.4]],
        [[2.1, 2.4], [3.4, 2.62]]
    ]

loadMotionsFiles

• 加载Motions文件以执行动作。
• handWave：挥手
• forwards：前进
• backwards：后退
• sideStepLeft：向左横向移动
• sideStepRight：向右横向移动
• turnLeft60：左转60度
• turnRight60：右转60度

def loadMotionFiles(self):
        self.handWave = Motion('../../motions/HandWave.motion')
        self.forwards = Motion('../../motions/Forwards50.motion')
        self.backwards = Motion('../../motions/Backwards.motion')
        self.sideStepLeft = Motion('../../motions/SideStepLeft.motion')
        self.sideStepRight = Motion('../../motions/SideStepRight.motion')
        self.turnLeft60 = Motion('../../motions/TurnLeft60.motion')
        self.turnRight60 = Motion('../../motions/TurnRight60.motion')

startMotion

• 如果当前存在动作则停止，同时对于不同的动作设定不同的系统休眠时间，保证动作不被立刻打断。

def startMotion(self, motion):
	if self.currentlyPlaying:
	    self.currentlyPlaying.stop()
	
	motion.play()
	self.currentlyPlaying = motion
	# 前进动作休眠1.5秒
	if motion == self.forwards:
	    time.sleep(1.5)
	# 横向移动休眠2秒
	if motion == self.sideStepLeft or \
	    motion == self.sideStepRight:
	    time.sleep(2)
	    self.IS_GO = True
	self.currentlyPlaying = None

findAndEnableDevices

• 启动设备，继承于nao_demo.py未做修改。

getCenter

• 获取目标区域的中心点坐标

def getCenter(self, destID):
	# 计算x坐标
	x = (self.Destinations[destID - 1][0][0] + \
	         self.Destinations[destID - 1][0][1]) / 2
	# 计算y坐标（z坐标）
	y = (self.Destinations[destID - 1][1][0] + \
	        self.Destinations[destID - 1][1][1]) / 2
	center = [x, y]
	
	return center

go

• 初始化标志位，新建到达检测线程、转向线程和前进线程。

def go(self, destID):
	print("Go to painting {}".format(destID))
	self.IS_GO = True
	self.Flag = True
	self.ARRIVED = False
	self.ISDETECTING = False
	# 新建到达检测
	_thread.start_new_thread(self.isDest, (destID, ))
	# 新建转向
	_thread.start_new_thread(self.turnAround, (destID, ))
	# 新建前进
	_thread.start_new_thread(self.goDest, (destID, ))

2. 到达检测

• 通过GPS实时获取机器人位置，实时检测是否到达目标区域内，如果到达，停止当前动作，修改标志位，在控制台上打印到达信息，并挥手示意。

def isDest(self, destID):
	while True:
		# 通过gps获取当前位置
	    pos = self.gps.getValues()
		
		# 判断是否到达目标区域内
	    if (pos[0] > self.Destinations[destID - 1][0][0]) and \
	        (pos[0] < self.Destinations[destID - 1][0][1]) and \
	        (pos[2] < self.Destinations[destID - 1][1][0]) and \
	        (pos[2] > self.Destinations[destID - 1][1][1]):
	        # 停止当前动作
	        if self.currentlyPlaying:
	            self.currentlyPlaying.stop()
	            self.currentlyPlaying = None
	        # 标志到达
	        self.ARRIVED = True
	        # 允许下一个go函数运行
	        self.Flag = True
	        # 打印到达信息
	        print("Arrive painting {}".format(destID))
	        # 挥手示意
	        self.handWave.setLoop(True)
	        self.handWave.play()
	        self.handWave.setLoop(False)
	        
	        return

3. 自动避障

• 当检测到左侧有障碍物时向右横向移动，当检测到右侧有障碍物时向左横向移动。

def avoid(self):
	# 检测到左侧有障碍物
	while self.us[0].getValue() < 0.5:
	    self.startMotion(self.sideStepRight)
	# 检测到右侧有障碍物
	while self.us[1].getValue() < 0.5:
	    self.startMotion(self.sideStepLeft)

4. 自动寻路

4.1 自动转向

getAngle

• 获取需要旋转的角度。
• alpha：当前朝向与目标点夹角
• beta：当前目标点与0度角夹角
• Angle：旋转所需角度

def getAngle(self, destCenter, Pos, RollPitchYaw):
        x = destCenter[1] - Pos[2]
        y = destCenter[0] - Pos[0]
        sin = y / math.sqrt(x ** 2 + y ** 2)
        alpha = math.asin(sin)

		# 根据x轴坐标修正alpha
        if alpha < 0:
            if x > 0:
                alpha = - alpha - 3.14
        else:
            if x > 0:
                alpha = 3.14 - alpha 
		
		# 获取beta角
        beta = RollPitchYaw[2] - self.ZERO_ANGLE
        
        Angle = alpha + beta
		
		# Angle的绝对值要小于3.14，即转角小于180°
        if Angle < -3.14:
            Angle += 6.28
        if Angle > 3.14:
            Angle -= 6.28

        return Angle

detectAngle

• 检测当前已转角度，在误差范围内即算朝向正确。

def detectAngle(self, before, angle):
	while True:
	    now = self.inertialUnit.getRollPitchYaw()
	    # 修正角度
	    temp = abs(before[2] - now[2] - angle)
	    if temp > 6:
	        temp -= 6
	    if temp < 0.4:
	    	# 停止当前动作
	        if self.currentlyPlaying:
	            self.currentlyPlaying.stop()
	            self.currentlyPlaying = None
	        # 允许前进
	        self.IS_GO = True
	        # 允许下一个检测线程开启
	        self.ISDETECTING = False
	        return

turnAround

• 实时检测偏差角度，当偏差角度超过误差范围即刻开始转向修正。

def turnAround(self, destID):
	while not self.ARRIVED:
	    Pos = self.gps.getValues()
	    before = self.inertialUnit.getRollPitchYaw()
	    destCenter = self.getCenter(destID)
	    angle = self.getAngle(destCenter, Pos, before)
	
	    if abs(angle) >= 0.4:
	    	# 停止前进
	        self.IS_GO = False
	        # 检测是否已开启转向检测进程
	        if not self.ISDETECTING:
	            self.ISDETECTING = True
	            _thread.start_new_thread(
	                self.detectAngle, 
	                (before, angle, )
	            )
	        # 当偏差角度小于0时左转
	        if angle < 0:
	            if not self.currentlyPlaying:
	                self.startMotion(self.turnLeft60)
	        # 当偏差角度大于0时右转
	        else:
	            if not self.currentlyPlaying:
	                self.startMotion(self.turnRight60)

4.2 前进

• 当尚未到达目标区域且前进标志位为True时，进行避障和前进。

def goDest(self, destID):
	while not self.ARRIVED:
	    if self.IS_GO:
	    	# 等待当前动作停止
	        if not self.currentlyPlaying:
	        	# 避障
	            self.avoid()
	            # 前进
	            self.startMotion(self.forwards)

5. 主函数

__init__

• 初始化Nao

def __init__(self):
	Robot.__init__(self)
	self.currentlyPlaying = False
	self.findAndEnableDevices()
	self.loadMotionFiles()

run

• 实时检测键盘输入，数字键1-8代表目标地点1-8，确定一个目标地点后无法修改

def run(self):
	print("Ready!")
	key = -1
	
	while robot.step(self.timeStep) != -1:
	    key = self.keyboard.getKey()
	    if key > 0:
	        break
	        
	# 实时检测键盘输入
	while True:
	    key = self.keyboard.getKey()
		
		# 当输入目标地点1-8时执行go，并置标志位为False防止多次执行
	    if key > 48 and key < 57:
	        destID = key - 48
	        if self.Flag == True:
	            self.go(destID)
	            self.Flag = False
	
	    if robot.step(self.timeStep) == -1:
	        break

四、程序运行使用说明

• 当控制器成功加载时会在控制台打印Ready！信号。
  
• 在键盘上按下数字键1-8即可前往指定目标地点，同时会在控制台上打印目标点，但中途不可修改。到达后，会在控制台打印到达信息，并且Nao机器人会挥手示意，此时可指定下一目标点。

• 该控制器具有可移植性，只需设置0度角以及目标区域，可以在任意场馆使用。
• 即修改ZERO_ANGLE为起始位置的朝向，同时修改Destinations

五、实验结果及优缺点分析

  Nao机器人可以连续前往多个目标点，在途中可以进行简单的避障（详细效果见附件视频）。
  本次实验所设计的Nao机器人最大的优点在于实时检测，只需规定0度角以及目标区域即可完成寻路，并且途中可以规避障碍物，不受场馆限制，可移植性强。同时，也存在着一定缺点，首先则是无法中途修改目标地点，其次则是对于正前方的障碍物尚不能做出很好的应对策略。

六、总结心得

  实验最难的部分就是开始，刚开始的时候对Webots平台十分陌生，让机器人动起来都十分困难，后来通过修改nao_demp.py成功地让机器人动了起来，但思维又局限于怎样修改motion来转需要角度。经过一段时间的思考，思维跳脱了出来，可以在转到需要角度时停下，而不是通过修改motion这种治标不治本的办法。
  由于对Webots平台的陌生，导致线程爆炸，计算资源过度浪费，程序极度卡顿，经过反复地调试终于控制住了线程的数量。在避障方面，很遗憾没能写出更加优秀的避障算法，只完成了基本的避障功能，有机会的话会继续加以改进。
  本次实验给我最大的感悟就在于多角度思考问题，修改不了motion就在满足时停下，同时适当的误差可以接受不必强求，没有办法做到绝对的精准。
  感谢老师给了我了解Webots平台的机会，希望以后能有更多有趣的实验。

七、控制器源码

• 见附件