资讯详情

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

[Webots将传感器添加到机器人中并控制它

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

在本文中，我们将继续改进我们的机器人，我们将在机器人上安装传感器并使用它 C 语言实现了传感器的使用。

开始吧。

1.召唤控制器

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

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

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

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

2.距离传感器

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

1、添加距离传感器

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

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

  1. 成功添加后，在 右眼 在子节点下，会有一个子节点 DistanceSensor 但此时机器人上没有变化；
  2. 在菜单栏中点击 查看 ，勾选可选显示 传感器射线显示距离；箭头在下图中所指的白色射线是距离传感器的射线，但我目前距离传感器的射线方向有点奇怪；

  1. 先改名字。展开距离传感器的节点，找到 name，修改成你喜欢的名字。我这里修改成 ，用这个名字控制一段时间。
  2. 我们希望射线向机器人前进。 rotation，在 angle 里输入 1.57 (PI让它绕过 Z 轴逆时针旋转90°；

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

2.实现距离传感器控制

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

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

代码控制如下：

/* * File: my_controller.c * Date: * Description: * Author: * Modifications: */  /* * You may need to add include files like <webots/distance_sensor.h> or * <webots/motor.h>, etc. */ #include <webots/robot.h> #include <webots/distance_sensor.h> //添加头文件  #include <stdio.h>  /* * 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；

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

2、实现速度计算

编码器的控制思路如下：

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

代码实现如下：

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

/* * You may need to add include files like <webots/distance_sensor.h> or * <webots/motor.h>, etc. */
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/position_sensor.h>

#include <stdio.h>

/* * 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" 的新节点；

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

  1. 修改激光朝向。点击 Lidar"lidar" 节点，在 rotation 里设置 angle为1.57，最终参数如下：
x = 0
y = 0
z = 1
angle = 1.57
  1. 我们只需要激光显示一个图层，所以需要修改 numberOfLayers 为 1；
  2. 修改激光雷达的参数如下所示，获得右图显示的效果；

2、使用激光雷达

控制思路如下：

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

代码实现如下：

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

/* * You may need to add include files like <webots/distance_sensor.h> or * <webots/motor.h>, etc. */
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/lidar.h>

#include <stdio.h>

/* * 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

阅读原文

标签： 用什么传感器来控制距离如何添加传感器

锐单商城拥有海量元器件数据手册、 IC替代型号，打造 电子元器件IC百科大全！

锐单商城 - 一站式电子元器件采购平台