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平台的机会,希望以后能有更多有趣的实验。
七、控制器源码
- 见附件