在这里插入图片描述
这个是本人在大三期间做的项目 ---- 基于MIT的Cheetah方案设计的十二自由度并联四足机器人,这个项目获得过两个国家级奖项和一个省级奖项。接下来我会将这个机器人的控制部分所有代码进行开源,并配有相关的教程博客,希望能够帮助到在学习相关领域知识或者进行项目开发的同学。

学习建议

Webots是一款开源的多平台机器人仿真软件,也是目前最优秀的一款移动机器人仿真平台之一。
支持多种不同类型的机器人仿真,如工业机械臂,轮式机器人,足式机器人,履带式机器人,汽车,无人机,水下机器人,航天器等。
支持多种虚拟传感器,如相机,雷达,力传感器,位置传感器,陀螺仪,惯性单元,GPS等
还支持多种复杂环境的模拟,如室内,室外,崎岖路面,空中环境,水下环境等。

在本项目的开发中,主要使用了webots,并结合ROS进行仿真环境的搭建,实现了四足机器人力矩控制的仿真,大大缩减了实体测试成本
在这里插入图片描述

因为webots在国内的普及程度不高,所以网上的教程也不多,很多细节问题还是需要读者在自己使用的过程中一点点摸索。
学习资料:我的学习笔记
ROS联合webots实战案例
webots建模指南

学习内容

搭建环境

webots的环境搭建主要包括有场景搭建以及参数配置两部分。

场景搭建

场景的地面一般是采用网格,大小按需配置,再把灯光打上,背景配上即可。

在这里插入图片描述
webots内置有很多完整的模型以及各种几何模型,从右方的列表中拖拽进仿真环境中,并进行组合,即可构建复杂的仿真环境。
在这里插入图片描述

参数配置

WorldInfo:包含模拟的全局参数。
Viewpoint:定义主要视点相机参数。
TexturedBackground:定义场景的背景(如果稍微旋转视点,您应该看到远处的山)
TexturedBackroundLight:定义与上述背景关联的光。
RectangleArena:定义到目前为止您在该场景中看到的唯一对象。

webots最核心的参数配置是仿真的控制频率:下图中的2代表仿真中每2ms执行一次控制指令,以及反馈外设数据,这个值最小为1。但是受限于电脑的CPU性能,实际值与设置值会有比较大的出入,可能无法达到这么高的控制频率。所以在仿真过程中,需要结合自己的电脑性能来确定该值,并且为了保证一致性,在实体控制时最好也与仿真的控制频率保持一致。
在这里插入图片描述
摩擦力系数:需要与实际测试场地的地面情况相匹配。
在这里插入图片描述
其余比较常用的属性:阴影开启/关闭,FPS值,坐标系方向等,则根据情况进行调整。
更加详细的内容可以参考该文章:webots环境建模

搭建机器人模型

webots的机器人模型主要有三个来源:软件内置urdf模型导入以及软件内搭建

软件内置

webots内置有非常多的机器人模型,并且大都配有相应的控制器,可以直接改写进行控制。
在这里插入图片描述

urdf模型导入

参考我的学习笔记:
sw模型生成urdf文件
urdf模型导入webots

软件内搭建

参考该文章:webots机器人建模

设计控制器

webots适配多种类型的控制器,如cpp,python,ROS等。这里主要介绍cpp+ROS的控制器设计方式。

webots是提供了内置的ROS控制器的,用户通过客户端-服务器的通信方式,即可控制机器人模型对应的外设。但是由于每个外设对应一个服务端,对于四足机器人(腿部一共有12个电机),这种方式会造成比较大的控制时延,即驱动不同步,会导致机器人仿真与实体控制的不一致。所以在本项目中,我重新编写了ROS控制器代码,提升了仿真控制效率。

ROS和webots联合编译配置参考该文章:webots模型联合ros仿真,实现ros自定义控制器

1. 配置外设名称

webots的外设以节点形式存在,名称是其唯一的标识,所以要保证每个外设的名称正确并且均不相同,如gyro,imu,RB-joint-1等。
在这里插入图片描述

2. 编写webots端控制器

主要负责有以下的功能:

  1. 初始化外设
  2. 监听webots反馈的外设数据,并打包发布
  3. 监听控制指令,并进行数据解包,控制外设
/* 以下是本项目所用控制器的部分代码截取,仅供参考 */

// 包含相关的外设库
#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/InertialUnit.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/Supervisor.hpp>
#include <webots/Accelerometer.hpp>
#include <webots/Gyro.hpp>

// 外设初始化
void webotsController::initDevice(void) {
    imu = robot->getInertialUnit("imu");
    imu->enable(timeStep);

    accel = robot->getAccelerometer("accelerometer");
    accel->enable(timeStep);

    gyro = robot->getGyro("gyro");
    gyro->enable(timeStep);

    for (int i = 0; i < 12; i++) {
        m[i / 3][i % 3] = robot->getMotor(motor_name[i / 3][i % 3]);
        p[i / 3][i % 3] = robot->getPositionSensor(sensor_name[i / 3][i % 3]);
        p[i / 3][i % 3]->enable(timeStep);

        // 使用supervisor来观测电机转速
        motor_v_visor[i / 3][i % 3] = robot->getFromDef(motor_name[i / 3][i % 3]);
    }
    
    // 初始化世界坐标系下的pv观测器
    world_p_v_visor = robot->getFromDef("Zouwu");
}

// imu数据更新
void webotsController::updateImuData(void){
    const double* data;

    // quat
    data = imu->getRollPitchYaw();
    Vec3<float> rpy(-data[0], data[1], data[2]);
    Quat<float> q = ori::rpyToQuat(rpy);
    imu_data.quat.w = q.w();
    imu_data.quat.x = q.x();
    imu_data.quat.y = q.y();
    imu_data.quat.z = q.z();

    // accel
    data = accel->getValues();
    imu_data.accel[0] = data[0];
    imu_data.accel[1] = data[1];
    imu_data.accel[2] = data[2];

    // gyro
    data = gyro->getValues();
    imu_data.gyro[0] = data[0];
    imu_data.gyro[1] = data[1];
    imu_data.gyro[2] = data[2];

    // publish
    imu_puber.publish(imu_data);
}

// 执行控制指令
void webotsController::robotCmd_Callback(const common::RobotCommandConstPtr& msg) {
    float tau;
    static int64_t iter = 0;
    // location control & force control
    for (int leg = 0; leg < 4; leg++) {
        if(msg->flags[leg]){
            // abad
            if(msg->mode_abad[leg]){
                tau = msg->kp_abad[leg] * (msg->q_des_abad[leg] - leg_data.q_abad[leg]) +
                    msg->kd_abad[leg] * (msg->qd_des_abad[leg] - leg_data.qd_abad[leg]) + msg->tau_abad_ff[leg];
                m[leg][0]->setTorque(tau);
            }
            else m[leg][0]->setPosition(msg->q_des_abad[leg]);

            // hip
            if(msg->mode_hip[leg]){
                tau = msg->kp_hip[leg] * (msg->q_des_hip[leg] - leg_data.q_hip[leg]) +
                    msg->kd_hip[leg] * (msg->qd_des_hip[leg] - leg_data.qd_hip[leg]) + msg->tau_hip_ff[leg];
                m[leg][1]->setTorque(tau);
            }
            else m[leg][1]->setPosition(msg->q_des_hip[leg] - ang_bias);

            // knee
            if(msg->mode_hip[leg]){
                tau = msg->kp_knee[leg] * (msg->q_des_knee[leg] - leg_data.q_knee[leg]) +
                    msg->kd_knee[leg] * (msg->qd_des_knee[leg] - leg_data.qd_knee[leg]) + msg->tau_knee_ff[leg];
                m[leg][2]->setTorque(-tau);
            }
            else m[leg][2]->setPosition(- msg-> q_des_knee[leg] - ang_bias);
        }
    }

    iter ++;
}

// 控制器运行函数
void webotsController::run(void) {
    // initiate
    initNode();

    // main loop
    while (robot->step(timeStep) != -1) {
        updateLegData();
        updateCheaterState();
        updateImuData();
        ros::spinOnce();
    };
}

3. 编写客户端控制器

主要负责有以下的功能:

  1. 发布控制指令
  2. 监听外设数据
/* 以下是本项目所用控制器的部分代码截取,仅供参考 */

// 发布控制指令
void Control_Node::updateRobotCommand(const RobotCommand* cmd) {
    if (cmd != nullptr) {
            convertToMsg(cmd, &_cmd);
            RobotCmd_Puber.publish(_cmd);
    }
}

// 监听IMU数据
void Control_Node::ImuData_Callback(const common::ImuDataConstPtr& msg) {
    VectorNavData nav;
    nav.quat.w() = msg->quat.w;
    nav.quat.x() = msg->quat.x;
    nav.quat.y() = msg->quat.y;
    nav.quat.z() = msg->quat.z;

    nav.gyro << msg->gyro[0], msg->gyro[1], msg->gyro[2];
    nav.accelerometer << msg->accel[0], msg->accel[1], msg->accel[2];
    _robotRunner->updateVectorNavData(&nav);
}
Logo

开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!

更多推荐