在webots中实现pid控制

RobotFreak

已于 2023-03-04 11:32:22 修改

阅读量824

点赞数 2

分类专栏： webots C/C++ 文章标签：机器人 c语言自动化

于 2023-01-29 21:20:36 首次发布

本文链接：https://blog.csdn.net/svfsvadfv/article/details/128794308

版权

C/C++ 同时被 2 个专栏收录

17 篇文章 9 订阅

订阅专栏

webots

3 篇文章 1 订阅

订阅专栏

在webots中实现pid控制

webots是一款开源机器人仿真软件，拥有强大的仿真能力，在机器人开发中也经常被使用。这篇文章向大家介绍如何在webots中实现自己的pid控制

前置知识

有关webots和PID的知识这里不再介绍，有需要者可移步以下链接：

webots:

PID:

图文详解PID调参_Top嵌入式的博客-CSDN博客_pid调参

开发环境

webots：2023a

VScode

搭建仿真环境

新建project，命名为pid_controller

在这里插入图片描述

生成这样一个世界：

在这里插入图片描述

然后我们搭建用于演示pid的物体，它是由一个大方块和一个小长方体组成的：

在这里插入图片描述

大方块我们设为robot节点，尺寸为0.1,0.1,0.12(x,y,z)，appearance选PBRAppearance，颜色设为红色

在这里插入图片描述

小长方体是一个solid节点，尺寸为0.025,0.025,0.1(x,y,z)，在大方块的一侧并与大方块有一定空隙

在这里插入图片描述

大方块和小长方体通过一个HingeJoint连接，关节的原点设在endPoint的solid的中心，绕Y轴旋转；关节的device有motor和position sensor

在这里插入图片描述

！需要注意的是为了体现一个比较好的旋转效果，并尽量贴合实际，我将大方块的physics字段中的density设为了5000，而小长方体的density为1000。这样大方块就会更重，体现出来是小长方体转动而大方块不动，小长方体旋转的惯性不会影响大方块，类似于把一个杆固定在墙上旋转。

编写控制器

控制器实现了一个简单的pid控制速度

/*
 * File:          pid_controller.c
 * Date:
 * Description:
 * Author:        RobotFreak
 * 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>
//#include "pid.h"
/*
 * You may want to add macros here.
 */
#define TIME_STEP 64
#define SPD_KP 0.001
#define SPD_KI 0
#define SPD_KD 0
#define POS_KP 0
#define POS_KI 0
#define POS_KD 0
#define SPD_OUT_MAX 10
#define SPD_I_MAX 0

typedef struct
{   
    double kp;           //比例系数
    double ki;           //积分系数
    double kd;           //微分系数
    double err_all;      //误差和
    double err_now;      //当前误差
    double err_last;     //上次误差
    double err_llast;    //上上次误差
    double p_out;        //比例输出
    double i_out;        //积分输出
    double d_out;        //微分输出
    double output;       //输出量
    double i_out_max;    //积分限幅
    double out_max;      //输出限幅
    double delta_output; //输出量增量
}PID;

void PID_Init(PID* pid_ptr, double kp, double ki, double kd, double output_max, double i_max)
{
    pid_ptr->kp = kp;
    pid_ptr->ki = ki;
    pid_ptr->kd = kd;
    pid_ptr->err_now = 0;
    pid_ptr->err_last = 0;
    pid_ptr->err_llast = 0;
    pid_ptr->err_all = 0;
    pid_ptr->p_out = 0;
    pid_ptr->i_out = 0;
    pid_ptr->d_out = 0;
    pid_ptr->delta_output = 0;
    pid_ptr->output = 0;
    pid_ptr->i_out_max = i_max;
    pid_ptr->out_max = output_max;
}

void PID_ErrUpdate(PID* pid_ptr, double target, double real)
{
    double err = target - real;  //期望值-实际值得到误差
    pid_ptr->err_llast = pid_ptr->err_last;
    pid_ptr->err_last = pid_ptr->err_now;
    pid_ptr->err_now = err;
    pid_ptr->err_all += err;
}

double PID_PositionalCalcOutput(PID* pid_ptr)
{
    pid_ptr->p_out = pid_ptr->kp * pid_ptr->err_now;
    pid_ptr->i_out = pid_ptr->ki * pid_ptr->err_all;
    // if(pid_ptr->i_out > pid_ptr->i_out_max)  //必要时可对积分限幅
    //     pid_ptr->i_out = pid_ptr->i_out_max;
    pid_ptr->d_out = pid_ptr->kd * (pid_ptr->err_now - pid_ptr->err_last);
    pid_ptr->output = pid_ptr->p_out + pid_ptr->i_out + pid_ptr->d_out;
    if(pid_ptr->output > pid_ptr->out_max)   //必要时可对输出限幅
      pid_ptr->output = pid_ptr->out_max;
    if(pid_ptr->output < -1 * pid_ptr->out_max)
      pid_ptr->output = -1 * pid_ptr->out_max;
    return pid_ptr->output;
}

double PID_IncrementalCalcOutput(PID* pid_ptr)
{
    pid_ptr->delta_output = pid_ptr->kp * (pid_ptr->err_now - pid_ptr->err_last) + pid_ptr->ki * pid_ptr->err_now + pid_ptr->kd * (pid_ptr->err_now - 2*pid_ptr->err_last + pid_ptr->err_llast);
    return pid_ptr->delta_output;
}

/*
 * 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");
   */
    //获取deviceTag
  WbDeviceTag motor = wb_robot_get_device("motor");
  WbDeviceTag positionSensor = wb_robot_get_device("positionSensor");
  wb_position_sensor_enable(positionSensor, TIME_STEP); //位置传感器使能
  //PID结构体
  PID speedPD; //p,i,d看作三个独立的单词(缩写)
  PID_Init(&speedPD, SPD_KP, SPD_KI, SPD_KD, SPD_OUT_MAX, SPD_I_MAX);
  //位置(rad)和速度(rad/s)
  double position = 0;
  double positionLast = 0;
  double speed = 0;
  double speedTarget = 5.0;
  /* 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);
     */
    positionLast = position;
    position = wb_position_sensor_get_value(positionSensor);
    speed = (position - positionLast) / TIME_STEP * 1000; //TIME_STEP是毫秒

    /* Process sensor data here */
    PID_ErrUpdate(&speedPD, speedTarget, speed);
    /*
     * Enter here functions to send actuator commands, like:
     * wb_motor_set_position(my_actuator, 10.0);
     */
    printf("position: %f speed: %f force: %f\n", position, speed, PID_PositionalCalcOutput(&speedPD));
    wb_motor_set_force(motor, PID_PositionalCalcOutput(&speedPD));
  };

  /* Enter your cleanup code here */

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

  return 0;
}