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

一、实验目的
二、功能模块
- 1. 场馆搭建
- 2. 机器人功能
- - 2.1 自动寻路
  - 2.2 自动避障
  - 2.3 到达检测
三、实现方法
- 1. 基础模块
- 2. 到达检测
- 3. 自动避障
- 4. 自动寻路
- - 4.1 自动转向
  - 4.2 前进
- 5. 主函数
四、程序运行使用说明
五、实验结果及优缺点分析
六、总结心得
七、控制器源码

一、实验目的

在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 = 8ZERO_ANGLE  = 1.5828IS_GO = TrueFlag = TrueARRIVED = FalseISDETECTING = FalseDestinations = [[[-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 = Trueself.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]) / 2center = [x, y]return center

go

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

def go(self, destID):print("Go to painting {}".format(destID))self.IS_GO = Trueself.Flag = Trueself.ARRIVED = Falseself.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轴坐标修正alphaif alpha < 0:if x > 0:alpha = - alpha - 3.14else:if x > 0:alpha = 3.14 - alpha # 获取beta角beta = RollPitchYaw[2] - self.ZERO_ANGLEAngle = alpha + beta# Angle的绝对值要小于3.14，即转角小于180°if Angle < -3.14:Angle += 6.28if Angle > 3.14:Angle -= 6.28return Angle

detectAngle

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

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

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 = Falseself.findAndEnableDevices()self.loadMotionFiles()

run

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

def run(self):print("Ready!")key = -1while 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 - 48if self.Flag == True:self.go(destID)self.Flag = Falseif 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平台的机会，希望以后能有更多有趣的实验。

七、控制器源码

见附件

Webots平台NAO机器人寻路避障实现相关推荐

基于Webots平台的简易智能机器人避障算法的实现
资源下载地址:https://download.csdn.net/download/sheziqiong/85793665 资源下载地址:https://download.csdn.net/downl ...
【webots教程】简单的避障机器人
系列文章目录 [webots教程]简介与软硬件要求 [webots教程]安装 [webots教程]关于webots的超详细介绍 [webots教程]你在webots搭建的第一个仿真环境 [webots ...
传感器和算法如何让机器人实现避障？
博客转载自:http://robot.ofweek.com/2016-11/ART-8321203-11000-30070963.html 避障是指移动机器人在行走过程中,通过传感器感知到在其规划路线 ...
AGV搬运机器人自动避障
现如今,AGV应用越来越普遍,被广泛地应用于物流仓储及柔性化生产线中,可能会有这样的疑问,为什么AGV搬运机器人在遇到障碍时可以躲开,会像人一样选择绕行,其实AGV搬运机器人智能的一个重要标志就是自主 ...
机器人局部避障的动态窗口法(dynamic window approach) （转）
源:机器人局部避障的动态窗口法(dynamic window approach) 首先在V_m∩V_d的范围内采样速度: allowable_v = generateWindow(robotV, ro ...
机器人局部避障的动态窗口法(dynamic window approach) DWA
rosparam命令可对ROS参数服务器上的参数进行操作.通过rosparam -h命令,可以看到有下面的一些方法: Commands: rosparam set set parameter 设置参数 ...
基于SLAM的机器人导航避障方案
基于SLAM的机器人导航避障方案在实现机器人智能导航中,SLAM发挥了重要作用,可帮助机器人实现地图构建与即时定位,但仅有SLAM是还不够的,还需要加入路径规划和运动控制.在SLAM技术帮助机器人确 ...
ROS机器人视觉避障部署流程
ROS机器人视觉避障部署流程环境 Ubuntu 16.04; ROS Kinetic; realsense D415. (一)配置相机环境在Ubuntu安装realsense D415的sdk,官 ...
SLAM导航机器人零基础实战系列：（六）SLAM建图与自主避障导航——3.ros-navigation机器人自主避障导航...
SLAM导航机器人零基础实战系列:(六)SLAM建图与自主避障导航--3.ros-navigation机器人自主避障导航摘要通过前面的基础学习,本章进入最为激动的机器人自主导航的学习.在前面的学习 ...

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

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

一、实验目的

二、功能模块

1. 场馆搭建

2. 机器人功能

2.1 自动寻路

2.2 自动避障

2.3 到达检测

三、实现方法

1. 基础模块

2. 到达检测

3. 自动避障

4. 自动寻路

4.1 自动转向

4.2 前进

5. 主函数

四、程序运行使用说明

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

六、总结心得

七、控制器源码

Webots平台NAO机器人寻路避障实现相关推荐

最新文章

热门文章