添加IMU傳感器
在webots中 InertialUnit 只對外提供pitch、roll、yaw三個角度,如需要加速度、陀螺儀和磁力計的數據還需要添加 Accelerometer、Gyro 和 Compass 組件。這幾個傳感器添加實體的方式基本相同,只有數據讀取的接口差別較大,其次要注意 InertialUnit 輸出的姿態角度(單位是 rad)Accelerometer 輸出的加速度值(單位是 m/s^2)、Gyro 輸出的是車體旋轉速度(單位是單位是 rad/s )。
1. 添加姿態測量傳感器 InertialUnit
1.1添加 InertialUnit實體
step1: 在Robot中添加 InertialUnit(IMU傳感器)
step2: 設置這個InertialUnit傳感器的children中添加shape節點,並設置外觀和形狀。設置半徑 0.015 高度0.01 ,偏移量爲(x=0,y=0.02,z=0)
step3: 設置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 一樣。
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 一樣。
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
3.1 添加 Compass 實體
添加陀螺儀 Gyro 實體的步驟與添加 InertialUnit 實體的操作方式一樣,只需要把 step1: 中選中Gyro 實體,後續的操作與 step2 和 step3 一樣。
3.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;
4. 測試效果
完整的控制器代碼如下:
#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
歡迎大家在評論區交流討論([email protected])