Webots平台NAO机器⼈寻路避障实现
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 =Fal
ISDETECTING =Fal
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(lf):
lf.handWave = Motion('../../ion')
lf.forwards = Motion('../../ion')
lf.backwards = Motion('../../ion')
lf.sideStepLeft = Motion('../../ion')
lf.sideStepRight = Motion('../../ion')
lf.turnLeft60 = Motion('../../ion')
lf.turnRight60 = Motion('../../ion')
startMotion
如果当前存在动作则停⽌，同时对于不同的动作设定不同的系统休眠时间，保证动作不被⽴刻打断。
def startMotion(lf, motion):
if lf.currentlyPlaying:
lf.currentlyPlaying.stop()
motion.play()
lf.currentlyPlaying = motion
# 前进动作休眠1.5秒
if motion == lf.forwards:
time.sleep(1.5)书信格式模板
# 横向移动休眠2秒
if motion == lf.sideStepLeft or \
motion == lf.sideStepRight:
time.sleep(2)
lf.IS_GO =True
lf.currentlyPlaying =None
findAndEnableDevices
启动设备，继承于nao_demo.py未做修改。
getCenter
获取⽬标区域的中⼼点坐标
def getCenter(lf, destID):
# 计算x坐标
x =(lf.Destinations[destID -1][0][0]+ \
lf.Destinations[destID -1][0][1])/2
# 计算y坐标（z坐标）
y =(lf.Destinations[destID -1][1][0]+ \
lf.Destinations[destID -1][1][1])/2
center =[x, y]
return center
go
初始化标志位，新建到达检测线程、转向线程和前进线程。
def go(lf, destID):挑来的媳妇
print("Go to painting {}".format(destID))
lf.IS_GO =True
lf.Flag =True
lf.ARRIVED =Fal
lf.ISDETECTING =Fal
文明礼貌用语# 新建到达检测
_thread.start_new_thread(lf.isDest,(destID,))
# 新建转向
_thread.start_new_thread(lf.turnAround,(destID,))
# 新建前进
_thread.start_new_Dest,(destID,))
2. 到达检测
通过GPS实时获取机器⼈位置，实时检测是否到达⽬标区域内，如果到达，停⽌当前动作，修改标志位，在控制台上打印到达信息，并挥⼿⽰意。
def isDest(lf, destID):
while True:
# 通过gps获取当前位置
pos = Values()
# 判断是否到达⽬标区域内
if(pos[0]> lf.Destinations[destID -1][0][0])and \
(pos[0]< lf.Destinations[destID -1][0][1])and \
(pos[2]< lf.Destinations[destID -1][1][0])and \
(pos[2]> lf.Destinations[destID -1][1][1]):
# 停⽌当前动作
if lf.currentlyPlaying:
lf.currentlyPlaying.stop()
lf.currentlyPlaying =None
# 标志到达
lf.ARRIVED =True
# 允许下⼀个go函数运⾏
lf.Flag =True
# 打印到达信息
print("Arrive painting {}".format(destID))
# 挥⼿⽰意
lf.handWave.tLoop(True)
lf.handWave.play()
lf.handWave.tLoop(Fal)
return
3. ⾃动避障
当检测到左侧有障碍物时向右横向移动，当检测到右侧有障碍物时向左横向移动。
无知的近义词def avoid(lf):
# 检测到左侧有障碍物
while lf.us[0].getValue()<0.5:
lf.startMotion(lf.sideStepRight)
# 检测到右侧有障碍物
while lf.us[1].getValue()<0.5:
lf.startMotion(lf.sideStepLeft)
4. ⾃动寻路
4.1 ⾃动转向
getAngle
获取需要旋转的⾓度。
alpha：当前朝向与⽬标点夹⾓
beta：当前⽬标点与0度⾓夹⾓
Angle：旋转所需⾓度
def getAngle(lf, 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:孝心日记100字
alpha =- alpha -3.14
el:
if x >0:
alpha =3.14- alpha
# 获取beta⾓
beta = RollPitchYaw[2]- lf.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(lf, before, angle):
while True:
now = RollPitchYaw()
# 修正⾓度
temp =abs(before[2]- now[2]- angle)
if temp >6:
temp -=6
if temp <0.4:
# 停⽌当前动作痛风能吃什么水果
if lf.currentlyPlaying:
lf.currentlyPlaying.stop()
lf.currentlyPlaying =None
# 允许前进
lf.IS_GO =True
# 允许下⼀个检测线程开启
lf.ISDETECTING =Fal
return酸菜卤肉饭
turnAround
