** 作为一个刚接触webots数日的新手，来着手使用webots搭建一个二轮小车并实现避障的任务,以及使用webots进行强化学习的尝试。该文章主要有如下的内容：**
大概内容：
1.Webots简要介绍
2.Webots搭建双轮小車
3.Webots双轮小车避障简单的控制逻辑
4.Pycharm编写控制程序连接并控制Webots中实体的方法
5.Tensorflow编写DQN强化学习算法用于小车的避障
6.Webots搭建八自由度四足机器狗子尝试（多有不足，刚接触webots，只是控制尝试）
1.Webots简要介绍
** 既然是简要介绍，好像也没什么好说的。比如webots是 一款开源的机器人仿真软件。主要功能是机器人的建模、控制与仿真，用于开发、测试和验证机器人算法。
下面给一个小例子，可以用webots来组建温馨的港湾。**
** 里面也有许多现成的实体可供使用。**
** 然后很关键的一点，就是可以根据需求自行搭建模型。下面就开始搭建双轮小车。**
2.Webots搭建双轮小車
** 从最基本的开始：
2.1 新建一个项目：**
** 向导→新项目目录**
** 然后根据提示下一步→选择文件存储的路径→勾选Add a rectangle arena→完成创建，大概就是这个样子：**
** 完成创建之后的样子如下：**
** 左侧用来添加节点，中间是仿真的显示区域，右侧用来编写控制器程序。下方显示输出信息。**
** 2.2 创建二轮小车实体：**
** 按照下图中图示的顺序创建一个机器人节点**
** 然后在创建的机器人节点中的孩子节点（children node）下，添加一个transform节点。这个是为后续添加boundingObject做准备的，操作过程如下（注意添加transform之后将translation中y坐标的值设置为0.1或者其他的正值，将robot节点的坐标系抬高，要不然创建出来的实体会一半嵌进地里）：**
** 修改transform节点的坐标 translation，将y值设置为0.1，上移坐标的位置**
** 然后在创建的transform节点下的children节点添加一个Shape，后面并通过这个shape选择一个形状，将会选一个圆柱，操作如下：**
** 添加圆柱并设置圆柱的外观显示。单纯添加圆柱为白色，添加外观设置之后可视化效果更好**
** 外观设置中，2，3，4步骤为对应的选项，操作2中颜色自选，操作3中Value可以设置为1，操作4设置为0，就可以得到下面这个粉柱子**
** 然后修改圆柱的大小，将高度设置为2cm，对应里面的坐标为0.02，圆柱的半径设置为0.01，即1cm。**
** 然后添加边界属性与物理属性。不加边界掉地里就找不着了。为了更方便的区分，将transform节点改一个名字，比如Body。方法如下，单击Transform节点，然后在Node DEF下面输入Body**
** 完成上一步之后，就可以添加上文提到的两个属性，分别是boundingObject与physics。boundingObject我们使用刚才改名的Body，然后physics属性点击弹出后直接添加即可，如下**
** 需要注意的是，在添加physics属性之前，可以先将仿真的按钮关闭。否则添加physics属性之后，实体就会直接掉在地上。按钮的位置如下：**
** 添加完成后，如果出现围绕实体的白色的边界线，就说明设置成功。如果边界线跟添加实体的边界有偏差，就说明坐标等细节有问题。可能是Transform（改名后为Body）节点下的坐标设置有问题，如果出现问题可以自查。弄完后大概就是这个样子：**
** 然后就可以进行下一步，添加一个轮子。单击Transform（改名后为Body）节点，选择Add（就那个“+”号），打开节点目录，选择一个HingeJoint节点。这个HingeJoint节点先不展开说，大概上可以理解为添加一个转轴，转轴的两端连接两个节点，一端是主体（Body），另一端需要连接一个Solid节点，下面会使用这个Solid节点制作一个轮子**
** 然后打开新增的HingeJoint节点，里面有三个属性，第一个是针对主节点的，第二个Device用来添加一个Motor，第三个终端节点就是要添加轮子的节点。下面两张图对应着前两个节点的修改**
** 然后打开添加的Motor节点，给Motor改改名字。这里就改为motorRight。修改名字方便后续对于Motor的调用**
** 然后创建一个轮子出来。点击endPoint，添加一个Solid节点。然后单击Solid节点，同样的方法添加一个Shape节点。然后用上文相同的方法，在Shape节点下添加一个圆柱**
** 单击Shape节点，添加一个圆柱。然后请各位自行修改颜色属性，就不过多赘述了，添加好大概样子如下（后面会修改Solid的坐标以及圆柱的大小，来让endPoint变成一个合格的轮子）**
** 修改圆柱的大小，并修改Solid节点的translation与rotation属性，将轮子放在指定的位置上。修改的数值如下（这个可以根据各位的喜好自行定义），可以按照图示的位置与数值修改**
** 然后给新添加的Shape节点改个名字。这个修改操作是为了给endPoint节点添加边界属性准备的。在添加boundingObject之后，添加物理属性，同时也给physics属性也改一个名字。操作如下：**
** 下面给物理属性改个名字，同上操作**
** 下面就是很关键的一步，这一步确保轮子能绕轴转动。打开HingeJoint节点下的endPoint节点，把translation的数值，直接复制到HingeJoint的第一个属性下面的anchor属性中，保持二者相同。然后点击position观察轮子是否按照指定转轴转动。如果转轴异常，可以修改axis的数值为 “0 1 0”或“0 0 1”，然后变动position的数值继续测试**
** 这样第一个轮子就创建成功了，记得保存各位，要不然辛苦白费了。第二个轮子会很轻松，因为是对称的，只需修改少量数据即可。点击刚创建的HingeJoint节点，复制，粘贴，出现一个相同的HingeJoint节点。然后打开新节点，按照图示修改数值。这样小車的主体部分就设计好了。**
** 2.3 运动测试：**
** 先保存最后的结果，很重要，很重要。然后点开仿真按钮，小車就掉下来了。双击小車的主体，在左侧会出现两个电机的控制台。然后就可以进行简单的运动测试了**
** 2.4 编写控制器：**
** 向导→新机器人控制器→下一步→Python→…可劲点就行了，点到结束。这里使用Python语言来创建控制器程序。若只用C/C++什么的看到这里就可以了，就不要浪费时间向下看了。**
** 先简单写一个包含前进/后退/转向/静止的代码逻辑，通过键盘输入进行控制。按键就是你键盘上最光亮的几个按键。代码如下：**
from controller import Robot
robot = Robot()
timestep =int(robot.getBasicTimeStep())#获取键盘输入
key = robot.getKeyboard()
key.enable(timestep)#获取电机
motor_right = robot.getMotor("motorRight")
motor_left = robot.getMotor("motorLeft")#设置电机模式与电机初始速度
motor_right.setPosition(float("inf"))
motor_right.setVelocity(0.0)
motor_left.setPosition(float("inf"))
motor_left.setVelocity(0.0)while robot.step(timestep)!=-1:
key_ = key.getKey()if key_ ==ord('W'):
motor_right.setVelocity(-7.0)
motor_left.setVelocity(-7.0)if key_ ==ord('S'):
motor_right.setVelocity(7.0)
motor_left.setVelocity(7.0)if key_ ==ord('A'):
motor_right.setVelocity(4.0)
motor_left.setVelocity(-4.0)if key_ ==ord('D'):
motor_right.setVelocity(-4.0)
motor_left.setVelocity(4.0)if key_ ==ord(' '):
motor_right.setVelocity(0.0)
motor_left.setVelocity(0.0)
** 下面是效果图：**
** 2.5 添加距离传感器**
** 我也是第一次做这个，接触了没几天，包括避障什么的。所以可能写的代码里多有逻辑不严谨或者是错误，还请加以分辨，请自行修正，抱歉。**
** 我是在小車的大脑门上加了三个距离传感器。方法类似于上文中的步骤。在Tramsform（Body）节点的基础上，添加一个Solid节点，是和前面添加的HingeJoint并列的节点。然后在Solid节点的children属性里添加Shape，并在Shape下添加一个圆柱，并调节尺寸，坐标，边界与物理属性。这些所有的步骤上文都有详细的介绍。还请上翻查看。**
** 按照图中标记的顺序。第一步：单击Transform节点，添加Solid。第二步：单击Solid节点，添加Shape。**
** 第一步：单击Transform节点，添加Solid。**
** 第二步：单击Solid节点，添加Shape。**
** 第三步：单击Shape节点，添加圆柱（Cylinder）。**
** 第四步：修改Cylinder属性值，修改Solid下的translation属性值。**
** 第五步：修改Shape的名称为sensor，然后保存保存保存。**
** 然后添加Solid节点下的boundingObject属性与physics属性。boundingObject属性用刚才修改的 sensor 。physics正常添加即可。可以通过白色的边界线来确定边界添加的准确与否。**
** 在刚才添加的Solid节点上单击，选择Add，添加一个距离传感器。**
** 然后把Solid节点的translation的数值直接复制到距离传感器的translation中。这时就可以看到坐标的移动了。然后点击查看→可选显示→显示距离传感器射线。这个时候可以看到距离传感器的射线，不过应该不太明显。**
** 接着修改距离传感器的rotation，使得距离传感器的射线朝向小車的前方，并保存。如下图：**
** 然后复制粘贴再添加两个距离传感器，修改rotation的angle数值使得距离传感器的射线朝向两个轮子，与原来前向垂直。然后修改距离传感器的名字，打开距离传感器节点，里面有name选项，直接修改名字即可。最后添加控制器程序，我直接写在原来的控制器里，如下图：**
** 修改之后的代码如下（这个时候可以运行仿真，控制小车运动，然后可以观察传感器的输出，print（）还没写，请自行添加）：**
from controller import Robot
robot = Robot()
timestep =int(robot.getBasicTimeStep())#获取三个距离传感器的数值
ds_forward = robot.getDistanceSensor("sensor_forward")
ds_forward.enable(timestep)
ds_right = robot.getDistanceSensor("sensor_right")
ds_right.enable(timestep)
ds_left = robot.getDistanceSensor("sensor_left")
ds_left.enable(timestep)#获取键盘输入
key = robot.getKeyboard()
key.enable(timestep)#获取电机
motor_right = robot.getMotor("motorRight")
motor_left = robot.getMotor("motorLeft")#设置电机模式与电机初始速度
motor_right.setPosition(float("inf"))
motor_right.setVelocity(0.0)
motor_left.setPosition(float("inf"))
motor_left.setVelocity(0.0)while robot.step(timestep)!=-1:
distance_f = ds_forward.getValue()
distance_l = ds_left.getValue()
distance_r = ds_right.getValue()
key_ = key.getKey()if key_ ==ord('W'):
motor_right.setVelocity(-7.0)
motor_left.setVelocity(-7.0)if key_ ==ord('S'):
motor_right.setVelocity(7.0)
motor_left.setVelocity(7.0)if key_ ==ord('A'):
motor_right.setVelocity(4.0)
motor_left.setVelocity(-4.0)if key_ ==ord('D'):
motor_right.setVelocity(-4.0)
motor_left.setVelocity(4.0)if key_ ==ord(' '):
motor_right.setVelocity(0.0)
motor_left.setVelocity(0.0)
3.Webots双轮小车避障简单的控制逻辑
** 3.1 下面是避障大橙子的效果图**
** 3.2 避障的基本逻辑**
** 小車有三个距离传感器，前/左/右各有一个。小车在循环中处于直行状态，当检测到前方有障碍物时，判断左右两侧的距离传感器的数值，进而转向距离传感器大的方向。在直行的同时，也会检测两侧的距离，当两侧距离障碍物过于接近的时候，也会启动转向的判定逻辑。控制代码如下：**
from controller import Robot
import time
robot = Robot()
timestep =int(robot.getBasicTimeStep())
speed =6.0
lm = robot.getMotor('motorLeft')
rm = robot.getMotor('motorRight')
lm.setPosition(float('inf'))
lm.setVelocity(0.0)
rm.setPosition(float('inf'))
rm.setVelocity(0.0)
dist = robot.getDistanceSensor("sensor_forward")
dist.enable(timestep)
distance_left = robot.getDistanceSensor("sensor_left")
distance_left.enable(timestep)
distance_right = robot.getDistanceSensor("sensor_right")
distance_right.enable(timestep)defLeft():for i inrange(5):if robot.step(timestep)!=-1:
ls =-speed
rs = speed
lm.setVelocity(ls)
rm.setVelocity(rs)defRight():for i inrange(5):if robot.step(timestep)!=-1:
ls = speed
rs =-speed
lm.setVelocity(ls)
rm.setVelocity(rs)defForward():whileTrue:if robot.step(timestep)!=-1:
ls =-speed
rs =-speed
lm.setVelocity(ls)
rm.setVelocity(rs)
ds = dist.getValue()
ds_left = distance_left.getValue()
ds_right = distance_right.getValue()iffloat(ds)<700.0orfloat(ds_left)<200.0orfloat(ds_right)<200.0:# or float(ds_left)<1000.0 or float(ds_right)<1000.0:breakif ds_left>ds_right:return"L"return"R"whileTrue:
direction = Forward()if direction =="L":
Left()else:
Right()
4.Pycharm编写控制程序连接并控制Webots中实体的方法
**
特别注意的是，这一步可能会有许多问题。也有很多的方法。如果下面的方法不能帮您解决，请自行百度，有很多方法可以选择并能解决问题，请保持耐心。
**
** 特别注意的是，这一步可能会有许多问题。也有很多的方法。如果下面的方法不能帮您解决，请自行百度，有很多方法可以选择并能解决问题，请保持耐心。**
** 如果说要做强化学习，基本上离不开Tensorflow，Pytorch之类的库，但这些玩意想在Webots里运行是不现实的。所以说可以使用Pycharm来运行控制程序，然后调用Webots提供的接口来控制机器人的运动。下面介绍Pycharm连接Webots的方法。**
** 这个其实可以搜到很多的方法，大概上可以这样做：**
** 找到该项目的文件集合，找到controllers目录，使用Pycharm打开。**
** 然后设置Pycharm。打开File→选择Settings→打开Project ：controllers→打开Project ：Structure→选择Add，然后添加Webots下的目录，根据自己安装的Webots来定。我的是“D:\Webots\lib\controller\python37”**
** 然后设置Configurations，在环境变量里添加：;Path=D:\Webots\lib\controller;D:\Webots\msys64\mingw64\bin;D:\Webots\msys64\mingw64\bin\cpp 也是根据自己的Webots目录来定（注意一开始的 “；”）。然后点击Apply应用。如果正常的话，现在打开Webots仿真，在controller里选择 “extern”。然后在Pycharm运行程序，就可以实现控制。**
**
如果问题没有解决，请考虑修改主机的环境变量。百度上可以搜到文章，关于环境变量的修改。
**
5.Tensorflow编写DQN强化学习算法用于小车的避障(这个会用到两种方式来编写代码，一种是直接拿我其他文章中提到的DQN代码，另一种会改写DQN的代码逻辑)
** 这里先弄第一种，第二种后续测试完了会补上。**
** 5.0 我承认我忘了，这是刚添加上的第零项**
** 我们需要在刚才的小車的基础上修改一下坐标。意思就是让小车整体向下移动。下面我会给出图示，将Robot节点的children下的所有节点中的translation中的y值都减少0.08。使得小車的整体向下移动。**
** 然后很关键的一点，我们需要使用Supervisor来重置机器人的状态。只需要把Supervisor对应的节点选择为True即可。**
** 5.1 思路：**
** 这个算是为了后续的强化学习任务做铺垫，案例以及算法逻辑多有不足，还请理解。**
** 设置了一个圈子，并设置围栏的高度。DQN算法的Observation仅仅包含三个距离传感器数据。其实这个数据偏少，各位可以给小車自行添加GPS，陀螺仪等，也可以使用激光雷达代替距离传感器，以此来丰富Observation的数据量。然后因为DQN算法只适用于离散值，所以会预先编写一个控制器，将动作离散化，即小車只有三个动作：前进 / 左转+前进 / 右转+前进。当然也可以通过DDPG算法来控制连续的动作，DDPG算法的文章过一阵子也会发。**
** 这里先说个小点子，如果动作只有前进/ 左转 / 右转，且Reward不加特殊设定的话，训练出来的小車很容易原地自转，避了个好障。**
** 5.2 控制器程序：**
** 如果接触过强化学习，应该对Gym不陌生。这个控制器的作用就是将小車改造成类似于Gym中的强化学习环境。因此，需要编写Reset()与Step()函数。Reset()函数用于重置系统的状态。Step()函数用于驱动小車运动，这个项目的运动输入是 [0,1,2]。因为小车没有安装陀螺仪等传感器，无法获得小车的姿态数据。因此在驱动小车进行转向（转向均为旋转90度）时默认为开环控制，不对旋转角度进行修正。各位有实际需求的，可以自行加装闭环控制程序，像是典型的PID之类。然后下面是控制器类的代码：**
from controller import Supervisor
import time
import numpy as np
classEnv():def__init__(self):
self.name ="car"
self.version =1.0
self.action_space =3
self.obs_space =4
self.terminal =False
self.robot = Supervisor()
self.timestep =32
self.max_speed =6.28
self.motor_1 = self.robot.getMotor("motorLeft")
self.motor_2 = self.robot.getMotor("motorRight")
self.motor_1.setVelocity(0.0)
self.motor_2.setVelocity(0.0)
self.motor_1.setPosition(float("inf"))
self.motor_2.setPosition(float("inf"))
self.ds_front = self.robot.getDistanceSensor("sensor_forward")
self.ds_left = self.robot.getDistanceSensor("sensor_left")
self.ds_right = self.robot.getDistanceSensor("sensor_right")
self.ds_front.enable(self.timestep)
self.ds_left.enable(self.timestep)
self.ds_right.enable(self.timestep)defstep(self,action):
df = dr = dl =0if action ==0:for i inrange(11):
self.robot.step(self.timestep)
self.motor_1.setVelocity(-6.0)
self.motor_2.setVelocity(-6.0)
df = self.ds_front.getValue()
dl = self.ds_left.getValue()
dr = self.ds_right.getValue()elif action ==1:for i inrange(11):
self.robot.step(self.timestep)
self.motor_1.setVelocity(-6.0)
self.motor_2.setVelocity(6.0)for i inrange(11):
self.robot.step(self.timestep)
self.motor_1.setVelocity(-6.0)
self.motor_2.setVelocity(-6.0)
df = self.ds_front.getValue()
dl = self.ds_left.getValue()
dr = self.ds_right.getValue()else:for i inrange(11):
self.robot.step(self.timestep)
self.motor_1.setVelocity(6.0)
self.motor_2.setVelocity(-6.0)for i inrange(11):
self.robot.step(self.timestep)
self.motor_1.setVelocity(-6.0)
self.motor_2.setVelocity(-6.0)
df = self.ds_front.getValue()
dl = self.ds_left.getValue()
dr = self.ds_right.getValue()
self.motor_1.setVelocity(0)
self.motor_2.setVelocity(0)
terminal =False
reward =-1if df<400.0or dr<400.0or dl<400.0:
terminal =True
reward =-15.0return[np.array([df,dl,dr]).reshape(1,1,3),reward,terminal]defreset(self):
self.robot.simulationResetPhysics()
Name = self.robot.getFromDef("car")
trans= Name.getField("translation")
rotation = Name.getField("rotation")
trans.setSFVec3f([0,-0.01,0])return np.array([1000.0,1000.0,1000.0]).reshape(1,1,3)
** 5.3 DQN Algorithm**
from tensorflow.keras import Sequential, layers
from tensorflow.keras.models import load_model
from collections import deque
from Env import Env
import numpy as np
import random
import time
classAgent(object):def__init__(self,):
self.steps =0
self.var =1e-1/2
self.e =1e-5
self.Model = self.model_()
self.Target = self.model_()
self.replay_memory = deque(maxlen=1000)defmodel_(self):
model = Sequential(name="DQN")
model.add(layers.Dense(100,"relu",input_shape=(None,3)))
model.add(layers.Dense(100,"relu"))
model.add(layers.Dense(3,None))
model.compile("adam","mse")return model
defadd(self, obs, action, reward, n_s, done):if action ==0andnot done:
reward +=2.0
self.replay_memory.append((obs, action, reward, n_s, done))defsample(self, obs):
self.var -= self.e
if np.random.uniform()<= self.var:return np.random.randint(3)return np.argmax(self.Model.predict(obs))defdata(self):
batch = random.sample(self.replay_memory,64)
Obs, Action, Reward, N_s, Done =[],[],[],[],[]for(obs, action, reward, n_s, done)in batch:
Obs.append(obs)
Action.append(action)
Reward.append(reward)
N_s.append(n_s)
Done.append(done)return np.array(Obs).astype("float32"), np.array(Action).astype("int64"), np.array(Reward).astype("float32"), \
np.array(N_s).astype("float32"), np.array(Done).astype("float32")deflearn(self):if self.steps %50==0:
self.Target.set_weights(self.Model.get_weights())if self.steps %2==0andlen(self.replay_memory)>=80:
Obs, Action, Reward, N_s, Done = self.data()
Q = self.Model.predict(Obs.reshape(64,1,3))
Q_ = self.Target.predict(N_s.reshape(64,1,3))for i inrange(64):if Done[i]:
Q[i][0][Action[i]]=0.01* Reward[i]
Q[i][0][Action[i]]=0.01*(Reward[i]+0.9* np.argmax(Q_[i][0]))
self.Model.fit(Obs.reshape(64,1,3), Q, verbose=0)
env = Env()
agent = Agent()
Scores =[]
agent.Model = load_model("car.h5")
agent.Target = agent.Model
for times inrange(500):
time_ =0
s = env.reset()
Score =0whileTrue:
time_ +=1
agent.steps +=1
a = agent.sample(s)
next_s, reward, done = env.step(a)
agent.add(s, a, reward, next_s, done)
agent.learn()
Score += reward
s = next_s
if done or time_ ==30:
Scores.append(Score)print('episode:', times,'score:', Score,'max:', np.max(Scores))break# 提前终止训练#if np.mean(Scores[-3:]) >= -25:# agent.Model.save("car.h5")# break
agent.Model.save("car.h5")
** 5.4 训练过程**
6.Webots搭建八自由度四足机器狗子尝试（多有不足，刚接触webots，只是控制尝试）
**
稍微熟悉软件之后，就可以尝试做点别的。我是简单凑了个狗子，足端等还未改进。步态也没去设计，所以多有缺陷。下面是简单的效果样例，然后这篇文章到此也正式结束了。感谢各位能看到结束，莫大的感谢。其中定有不足与错误，还请各位指教。
**
版权归原作者 小雅不采薇 所有， 如有侵权，请联系我们删除。