• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

Webots 机器人仿真平台(九) 添加IMU传感器

人工智能 熊猫飞天 2460次浏览 0个评论

添加IMU传感器

  • 1. 添加姿态测量传感器 InertialUnit
    • 1.1添加 InertialUnit实体
    • 1.2 添加InertialUnit读取代码
  • 2 添加陀螺仪 Gyro
    • 2.1 添加 Gyro 实体
    • 2.2 添加 Gyro 读取代码
  • 3 添加加速度计 Accelerometer
    • 3.1 添加 Accelerometer实体
    • 3.2 添加 Accelerometer 读取代码
  • 4 添加磁力计 Compass
    • 4.1 添加 Compass 实体
    • 4.2 添加 Compass 读取代码
  • 5. 测试效果
  • 参考资料

在webots中 InertialUnit 只对外提供pitch、roll、yaw三个角度,如需要加速度、陀螺仪和磁力计的数据还需要添加 AccelerometerGyro 和 Compass 组件。这几个传感器添加实体的方式基本相同,只有数据读取的接口差别较大,其次要注意 InertialUnit 输出的姿态角度(单位是 rad)Accelerometer 输出的加速度值(单位是 m/s^2)、Gyro 输出的是车体旋转速度(单位是单位是 rad/s )。

1. 添加姿态测量传感器 InertialUnit

1.1添加 InertialUnit实体

step1: 在Robot中添加 InertialUnit(IMU传感器)  
Webots 机器人仿真平台(九) 添加IMU传感器   step2: 设置这个InertialUnit传感器的children中添加shape节点,并设置外观和形状。设置半径 0.015 高度0.01 ,偏移量为(x=0,y=0.02,z=0)  
Webots 机器人仿真平台(九) 添加IMU传感器  
Webots 机器人仿真平台(九) 添加IMU传感器  
Webots 机器人仿真平台(九) 添加IMU传感器   step3: 设置IMU传感器的名称(这个在控制器中会用到)  
Webots 机器人仿真平台(九) 添加IMU传感器   注意:添加IMU的过程中不设置boundingObject属性和 physics属性。

1.2 添加InertialUnit读取代码

初始化代码段:  

#include <webots/InertialUnit.hpp>  //头文件

  InertialUnit *imu;
  imu== robot->getInertialUnit("imu");
  imu->enable(TIME_STEP);

  IMU角度数据读取代码段:(单位是 弧度/rad)  

    std::cout<< "IMU roll: " <<imu->getRollPitchYaw()[0]<< " pitch: " <<imu->getRollPitchYaw()[1]
    << " yaw: " <<imu->getRollPitchYaw()[2] <<std::endl;      

   

2 添加陀螺仪 Gyro

2.1 添加 Gyro 实体

添加陀螺仪 Gyro 实体的步骤与添加 InertialUnit 实体的操作方式一样,只需要把 step1: 中选中Gyro 实体,后续的操作与 step2 和 step3 一样。  
Webots 机器人仿真平台(九) 添加IMU传感器  

2.2 添加 Gyro 读取代码

初始化代码段:  

  Gyro *gyro;
  gyro=robot->getGyro("gyro");
  gyro->enable(TIME_STEP);

    陀螺仪数据读取代码段:(单位是 rad/s)  

 std::cout<< "Gyro X: " <<gyro->getValues()[0]
    << " Y: " <<gyro->getValues()[1]<< " Z: " <<gyro->getValues()[2] <<std::endl;    

   

3 添加加速度计 Accelerometer

3.1 添加 Accelerometer实体

添加陀螺仪 Gyro 实体的步骤与添加 InertialUnit 实体的操作方式一样,只需要把 step1: 中选中Gyro 实体,后续的操作与 step2 和 step3 一样。  
Webots 机器人仿真平台(九) 添加IMU传感器  

3.2 添加 Accelerometer 读取代码

初始化代码段:  

  Gyro *gyro;
  gyro=robot->getGyro("gyro");
  gyro->enable(TIME_STEP);

    加速度数据读取代码段:(单位是 m/s^2)  

 std::cout<< "Gyro X: " <<gyro->getValues()[0]
    << " Y: " <<gyro->getValues()[1]<< " Z: " <<gyro->getValues()[2] <<std::endl;    

   

4 添加磁力计 Compass

4.1 添加 Compass 实体

添加陀螺仪 Gyro 实体的步骤与添加 InertialUnit 实体的操作方式一样,只需要把 step1: 中选中Gyro 实体,后续的操作与 step2 和 step3 一样。  
Webots 机器人仿真平台(九) 添加IMU传感器  
Webots 机器人仿真平台(九) 添加IMU传感器  

4.2 添加 Compass 读取代码

初始化代码段:  

  Compass *compass;
  compass=robot->getCompass("compass");
  compass->enable(TIME_STEP);

  加速度数据读取代码段:  

 std::cout<< "Compass X: " <<compass->getValues()[0]
    << " Y: " <<compass->getValues()[1]<< " Z: " <<compass->getValues()[2] <<std::endl;    

   

5. 测试效果

 
Webots 机器人仿真平台(九) 添加IMU传感器   完整的控制器代码如下:  

#include <webots/Robot.hpp>
#include <webots/GPS.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Motor.hpp>
#include <webots/Keyboard.hpp>
#include <webots/InertialUnit.hpp>
#include <webots/Gyro.hpp>
#include <webots/Accelerometer.hpp>
#include <webots/Compass.hpp>
#include <stdio.h>

#define TIME_STEP 64
// All the webots classes are defined in the "webots" namespace
using namespace webots;
 
int main(int argc, char **argv) {
  // create the Robot instance.
  Robot *robot = new Robot();
  Keyboard kb;
  kb.enable(TIME_STEP);
  
  DistanceSensor *ds[2];
  char dsNames[2][10] = {"ds_right","ds_left"};
  for (int i = 0; i < 2; i++) {
    ds[i] = robot->getDistanceSensor(dsNames[i]);
    ds[i]->enable(TIME_STEP);
  }

  GPS *gps;
  gps = robot->getGPS("global_gps");
  gps->enable(TIME_STEP);
  
  InertialUnit *imu;
  imu=robot->getInertialUnit("imu");
  imu->enable(TIME_STEP);
  
  Gyro *gyro;
  gyro=robot->getGyro("gyro");
  gyro->enable(TIME_STEP);
  
  Accelerometer *accelerometer;
  accelerometer=robot->getAccelerometer("accelerometer");
  accelerometer->enable(TIME_STEP);
  
  Compass *compass;
  compass=robot->getCompass("compass");
  compass->enable(TIME_STEP);
  // initialise motors
  Motor *wheels[4];
  char wheels_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
  for (int i = 0; i < 4; i++) {
    wheels[i] = robot->getMotor(wheels_names[i]);
    wheels[i]->setPosition(INFINITY);
    wheels[i]->setVelocity(0);
  }
  printf("init successd ...\n");
  

  double leftSpeed = 0.0;
  double rightSpeed = 0.0;

   // Main loop:
  // - perform simulation steps until Webots is stopping the controller
 while (robot->step(TIME_STEP) != -1)
 {
    int key = kb.getKey();
    if(key== 315)
    {
      leftSpeed = 3.0;
      rightSpeed = 3.0;
    }
    else if(key== 317)
    {
      leftSpeed = -3.0;
      rightSpeed = -3.0;
    }
    else if(key== 314)
    {
      leftSpeed = -3.0;
      rightSpeed = 3.0;
    }
    else if(key== 316)
    {
      leftSpeed = 3.0;
      rightSpeed = -3.0;
    }
    else  
    {
      leftSpeed = 0.0;
      rightSpeed = 0.0;
    }
    std::cout<< " Right Sensor Value:" <<ds[0]->getValue() << "  Left Sensor Value:" <<ds[1]->getValue() <<std::endl;  
    std::cout<< "GPS Value X: " <<gps->getValues()[0]
    << " Y: " <<gps->getValues()[1]<< " Z: " <<gps->getValues()[2] <<std::endl;    
    
    
    std::cout<< "IMU roll: " <<imu->getRollPitchYaw()[0]<< " pitch: " <<imu->getRollPitchYaw()[1]
    << " yaw: " <<imu->getRollPitchYaw()[2] <<std::endl;    
   
     std::cout<< "Gyro X: " <<gyro->getValues()[0]
    << " Y: " <<gyro->getValues()[1]<< " Z: " <<gyro->getValues()[2] <<std::endl;    
   
    std::cout<< "Accel X: " <<accelerometer->getValues()[0]
    << " Y: " <<accelerometer->getValues()[1]<< " Z: " <<accelerometer->getValues()[2] <<std::endl;    
   
    std::cout<< "Compass X: " <<compass->getValues()[0]
    << " Y: " <<compass->getValues()[1]<< " Z: " <<compass->getValues()[2] <<std::endl;    
   
    
     wheels[0]->setVelocity(leftSpeed);
     wheels[1]->setVelocity(rightSpeed);
     wheels[2]->setVelocity(leftSpeed);
     wheels[3]->setVelocity(rightSpeed);
  };

  // Enter here exit cleanup code.

  delete robot;
  return 0;
}

   

参考资料

[1] 1https://cyberbotics.com/doc/reference/index?version=R2020a-rev1 [2] https://www.cyberbotics.com/doc/reference/motion 如果大家觉得文章对你有所帮助,请大家帮忙点个赞。O(∩_∩)O 欢迎大家在评论区交流讨论(cenruping@vip.qq.com)  


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明Webots 机器人仿真平台(九) 添加IMU传感器
喜欢 (0)

您必须 登录 才能发表评论!

加载中……