[Webots仿真入门]在机器人中添加传感器并控制它

[Webots仿真入门]在机器人中添加传感器并控制它

本文章最早发布于我的个人博客
http://120.76.72.211/archives/webots-fang-zhen-ru-men–zai-ji-qi-ren-zhong-tian-jia-chuan-gan-qi-bing-kong-zhi-ta

如需转载，请联系

如有侵权，请联系我

上一篇中，我们在 Webots 中创建了一个两轮的差速控制机器人，并且写了一段简单的控制程序让机器人动了起来。

在这一篇中，我们将继续完善我们的机器人，我们将在机器人上安装传感器，并用 C 语言实现传感器的使用。

那就开始吧。

1、召唤控制器

我们上一篇的机器人搭建里已经创建了一个控制器了，可以直接在这个控制器的基础上继续修改。

1. 回到节点树，选择 Robot"robot" ；
2. 右键选择 编辑控制器 ；
3. 在窗口的右边，就会显示当前机器人使用的控制器。

你也可以重新建一个新的控制器。

点击菜单栏的 向导，选择 新机器人控制器 ，记得创建的是 C 语言的控制器。

2、距离传感器

我们把距离传感器安装到机器人的眼睛上，左右各一个。

1、添加距离传感器

1. 展开机器人节点 Robot"robot"；
2. 展开机器人节点的子节点 children；
3. 展开机器人的右眼 DEF Eye_Right Solid；
4. 展开机器人右眼的子节点 children；

5. 选择新增，在弹出的窗口中展开 Base nodes，选择 DistanceSensor，添加；

6. 成功添加后，在 右眼 的子节点下面，就会有一个 DistanceSensor 的节点。但是这时候机器人上什么变化也没有；
7. 点击菜单栏中的 查看 ，在可选显示里勾选 显示距离传感器射线；在下图中箭头所指的白色射线就是距离传感器的射线，只不过我现在的距离传感器的射线方向有点奇怪；

8. 先改一下名字呗。展开距离传感器的节点，找到 name，修改成你喜欢的名字。我这里修改成 distance_sensor_right，一会控制的时候是要用到这个名字的。
9. 我们希望射线是朝着机器人前进的方向的。点击 rotation，在 angle 里输入 1.57 (PI/2)，使其绕着 Z 轴逆时针旋转90°；

10. 以同样的方式添加左眼的距离传感器。

2、实现距离传感器控制

距离传感器控制的思路如下：

1.获取距离传感器的设备句柄，通过 name；
2.使能距离传感器的控制；
3.获取距离传感器的值；

代码控制如下：

/*
 * File:          my_controller.c
 * Date:
 * Description:
 * Author:
 * Modifications:
 */

/*
 * You may need to add include files like  or
 * , etc.
 */
#include 
#include 		//头文件要添加

#include 

/*
 * You may want to add macros here.
 */
#define TIME_STEP     64
#define MAX_SPEED     (6.28f)

/*
 * This is the main program.
 * The arguments of the main function can be specified by the
 * "controllerArgs" field of the Robot node
 */
int main(int argc, char **argv) {
  /* necessary to initialize webots stuff */
  wb_robot_init();

  /*
   * You should declare here WbDeviceTag variables for storing
   * robot devices like this:
   *  WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
   *  WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
   */
   
   //Initial motor
   WbDeviceTag left_motor  = wb_robot_get_device("motor_2");
   WbDeviceTag right_motor = wb_robot_get_device("motor_1");
   
   wb_motor_set_position(left_motor,  INFINITY);
   wb_motor_set_position(right_motor, INFINITY);
   
   wb_motor_set_velocity(left_motor,  0.0f);
   wb_motor_set_velocity(right_motor, 0.0f);
   
   // 获取控制句柄
   WbDeviceTag left_dis_sensor  = wb_robot_get_device("distance_sensor_left");
   WbDeviceTag right_dis_sensor = wb_robot_get_device("distance_sensor_right");
   
   // 使能控制
   wb_distance_sensor_enable(left_dis_sensor,  TIME_STEP);
   wb_distance_sensor_enable(right_dis_sensor, TIME_STEP);

  /* main loop
   * Perform simulation steps of TIME_STEP milliseconds
   * and leave the loop when the simulation is over
   */
  while (wb_robot_step(TIME_STEP) != -1) {
    /*
     * Read the sensors :
     * Enter here functions to read sensor data, like:
     *  double val = wb_distance_sensor_get_value(my_sensor);
     */
      
     // 获取传感器的值
     float dis_left_val  = wb_distance_sensor_get_value(left_dis_sensor);
     float dis_right_val = wb_distance_sensor_get_value(right_dis_sensor);
     
     printf("distance: %.03f, %.03f\r\n", dis_left_val, dis_right_val);
     

    /* Process sensor data here */

    /*
     * Enter here functions to send actuator commands, like:
     * wb_motor_set_position(my_actuator, 10.0);
     */
     
     wb_motor_set_velocity(left_motor,  MAX_SPEED);  // rad/s
     wb_motor_set_velocity(right_motor, MAX_SPEED);  // rad/s
  };

  /* Enter your cleanup code here */

  /* This is necessary to cleanup webots resources */
  wb_robot_cleanup();

  return 0;
}

编译完成后仿真，在控制台就可以看到打印的距离值。

如果你的窗口上没有控制台，点击菜单栏中的 工具 ，选择 New Console 创建一个新的控制台。

3、电机编码器

我们可以通过电机编码器计算当前机器人的车轮转速，进而计算出机器人当前的前进速度。

1、添加距离传感器

1. 找到右轮铰链的 device 节点；
2. 右键新建，在弹出的窗口中选择 PositionSensor；

3. 改名。我这里修改成 position_sensor_right；
4. 以同样的方式添加左轮的编码器，并修改名称 position_sensor_left；

2、实现速度计算

编码器的控制思路如下：

1.获取传感器的设备句柄，通过 name；
2.使能传感器的控制；
3.获取传感器的值；

代码实现如下：

/*
 * File:          my_controller.c
 * Date:
 * Description:
 * Author:
 * Modifications:
 */

/*
 * You may need to add include files like  or
 * , etc.
 */
#include 
#include 
#include 

#include 

/*
 * You may want to add macros here.
 */
#define TIME_STEP     64
#define MAX_SPEED     (6.28f)
#define WHEEL_RADIUS  (0.025f)
#define MATH_PI       (3.141593f)

/*
 * This is the main program.
 * The arguments of the main function can be specified by the
 * "controllerArgs" field of the Robot node
 */
int main(int argc, char **argv) {
  /* necessary to initialize webots stuff */
  wb_robot_init();

  /*
   * You should declare here WbDeviceTag variables for storing
   * robot devices like this:
   *  WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
   *  WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
   */
   
   //Initial motor
   WbDeviceTag left_motor  = wb_robot_get_device("motor_left");
   WbDeviceTag right_motor = wb_robot_get_device("motor_right");
   
   wb_motor_set_position(left_motor,  INFINITY);
   wb_motor_set_position(right_motor, INFINITY);
   
   wb_motor_set_velocity(left_motor,  0.0f);
   wb_motor_set_velocity(right_motor, 0.0f);
   
   WbDeviceTag left_pos  = wb_robot_get_device("position_sensor_left");
   WbDeviceTag right_pos = wb_robot_get_device("position_sensor_right");
   
   wb_position_sensor_enable(left_pos,  TIME_STEP);
   wb_position_sensor_enable(right_pos, TIME_STEP);
   
   float wheel_cirum = 2 * MATH_PI * WHEEL_RADIUS;
   float encoder_unit = wheel_cirum / (2 * MATH_PI);
   
   float last_left_pos_val  = 0.0f;
   float last_right_pos_val = 0.0f;

  /* main loop
   * Perform simulation steps of TIME_STEP milliseconds
   * and leave the loop when the simulation is over
   */
  while (wb_robot_step(TIME_STEP) != -1) {
    /*
     * Read the sensors :
     * Enter here functions to read sensor data, like:
     *  double val = wb_distance_sensor_get_value(my_sensor);
     */
     
     float left_pos_val  = wb_position_sensor_get_value(left_pos);
     float right_pos_val = wb_position_sensor_get_value(right_pos);     

    /* Process sensor data here */

    /*
     * Enter here functions to send actuator commands, like:
     * wb_motor_set_position(my_actuator, 10.0);
     */
     
     wb_motor_set_velocity(left_motor,  MAX_SPEED);  // rad/s
     wb_motor_set_velocity(right_motor, MAX_SPEED);  // rad/s
     
     float left_diff  = left_pos_val  - last_left_pos_val;
     float right_diff = right_pos_val - last_right_pos_val;
     
     printf("left_diff = %f, right_diff = %f\r\n", left_diff, right_diff);
     
     if (left_diff < 0.0001)
     {
       left_diff = 0.0f;
       left_pos_val = last_left_pos_val;
     }
       
     if (right_diff < 0.0001)
     {
       right_diff = 0.0f;
       right_pos_val = last_right_pos_val;
     }
     
     float left_speed = left_diff / (float)(TIME_STEP/1000.);  // rad/s
     left_speed = left_speed * encoder_unit;  // m/s
     
     float right_speed = right_diff / (float)(TIME_STEP / 1000.);
     right_speed = right_speed * encoder_unit;
     
     
     last_left_pos_val = left_pos_val;
     last_right_pos_val = right_pos_val;
     
     printf("left speed :%f m/s, right speed:%f m/s\r\n", left_speed, right_speed);
  };

  /* Enter your cleanup code here */

  /* This is necessary to cleanup webots resources */
  wb_robot_cleanup();

  return 0;
}

3、速度计算方式

我们在电机控制里，给电机设置的速度 wb_motor_set_velocity(left_motor, MAX_SPEED); ，其中 MAX_SPEED 的单位是 rad/s。

从编码器中获取到的值 wb_position_sensor_get_value(left_pos);，其单位是 rad，表示当前转过的弧度。

在数学关系中，车轮转过一圈的长度 2 * PI * r，对应的弧度是 2 * PI。

因此，我们需要计算在单位时间内，轮子转过的弧度，在匹配当前轮子的半径，就可以得出在单位时间内，车子前进的速度。
$$
\begin{align} \
车轮周长 l = 2 * PI * radius\

转动单位 unit = \frac{2 * PI * radius}{2 * PI} \

编码器弧度差 Val_d = Val_c - Val_l\

单位时间转动量 Val_s = \frac{Val_d}{TIMESTEP * 1000} \

转速 V = Val_s * unit
\end{align} \
$$

$$

$$

4、激光雷达

先在机器人的脑袋上加上一个点，给激光雷达一个支点。

简单粗暴点的方法就是复制一个眼睛的节点，修改名称，删除距离传感器，然后修改参数即可。

修改后的坐标参数如下：

x = 0
y = 0
z = 0.08

1、添加激光雷达

1. 选择刚才添加的激光雷达的支点节点；
2. 右键选择 新增；
3. 在弹出的窗口中，展开 Base nodes ，选择 Lidar，添加；
4. 这时候在机器人的节点树中将多出了一个 Lidar"lidar" 的新节点；

5. 显示雷达照射范围和点云。点击菜单栏可选显示，勾选 Show Lidar Ray Paths 和 Show Lidar Point Cloud；显示效果如下，但这时候还有问题，激光的朝向是错误的；

6. 修改激光朝向。点击 Lidar"lidar" 节点，在 rotation 里设置 angle为1.57，最终参数如下：

x = 0
y = 0
z = 1
angle = 1.57

7. 我们只需要激光显示一个图层，所以需要修改 numberOfLayers 为 1；
8. 修改激光雷达的参数如下所示，获得右图显示的效果；

2、使用激光雷达

控制思路如下：

1.获取激光雷达的设备句柄，通过 name；
2.使能激光雷达；
3.使能激光雷达点云；
4.获取激光雷达队列指针；
5.通过索引获取相应点的值；

代码实现如下：

/*
 * File:          my_controller.c
 * Date:
 * Description:
 * Author:
 * Modifications:
 */

/*
 * You may need to add include files like  or
 * , etc.
 */
#include 
#include 
#include 

#include 

/*
 * You may want to add macros here.
 */
#define TIME_STEP     64
#define MAX_SPEED     (6.28f)
#define WHEEL_RADIUS  (0.025f)
#define MATH_PI       (3.141593f)

/*
 * This is the main program.
 * The arguments of the main function can be specified by the
 * "controllerArgs" field of the Robot node
 */
int main(int argc, char **argv) {
  /* necessary to initialize webots stuff */
  wb_robot_init();

  /*
   * You should declare here WbDeviceTag variables for storing
   * robot devices like this:
   *  WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
   *  WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
   */
   
   //Initial motor
   WbDeviceTag left_motor  = wb_robot_get_device("motor_left");
   WbDeviceTag right_motor = wb_robot_get_device("motor_right");
   
   wb_motor_set_position(left_motor,  INFINITY);
   wb_motor_set_position(right_motor, INFINITY);
   
   wb_motor_set_velocity(left_motor,  0.0f);
   wb_motor_set_velocity(right_motor, 0.0f);
   
   WbDeviceTag lidar = wb_robot_get_device("lidar");
   wb_lidar_enable(lidar, TIME_STEP);
   wb_lidar_enable_point_cloud(lidar);

  /* main loop
   * Perform simulation steps of TIME_STEP milliseconds
   * and leave the loop when the simulation is over
   */
  while (wb_robot_step(TIME_STEP) != -1) {
    /*
     * Read the sensors :
     * Enter here functions to read sensor data, like:
     *  double val = wb_distance_sensor_get_value(my_sensor);
     */
     
     // 小车前进的左边为队列起点
     const float *lidar_array = wb_lidar_get_range_image(lidar);
     printf("lidar distance degree 90:%f\r\n", lidar_array[90]);	// 90°位置的距离值
  };

  /* Enter your cleanup code here */

  /* This is necessary to cleanup webots resources */
  wb_robot_cleanup();

  return 0;
}

显示效果如下图，蓝色的线和点是激光的点云。

5、其他传感器

[Webots官方传感器文档](Webots: (cyberbotics.com))

[Webots控制器节点和API函数文档](Webots: (cyberbotics.com))

Webots Github仓库

本地有关于传感器使用介绍的文档

C:\Program Files\Webots\docs\reference

---

阅读原文