Lesson 6: 运动控制-两侧驱动马达独立PID调速

 

ros_arduino_bridge的base controller中对两个电机的PID控制是用的一套PID参数,但实际的使用中,由于马达的特性,地形,或者机器人的载重的平衡性的诸多问题使机器人不能按照预定轨迹行驶,最简单的就是机器人是否能够走直线,这就需要对两个电机分别进行PID调速,本节中将对ros_arduino_bridge进行修改,以使支持对两路电机用不同的PID参数调速

1.首先修改arduino代码

1.1.修改diff_controller.h文件
增加左右两个马达的PID控制变量:

/* PID Parameters */
int Kp = 20;
int Kd = 12;
int Ki = 0;
int Ko = 50;

int left_Kp=Kp;
int left_Kd=Kd;
int left_Ki=Ki;
int left_Ko=Ko;

int right_Kp=Kp;
int right_Kd=Kd;
int right_Ki=Ki;
int right_Ko=Ko;

修改resetPID函数

void resetPID(){
 leftPID.TargetTicksPerFrame = 0.0;
 leftPID.Encoder = readEncoder(LEFT);
 leftPID.PrevEnc = leftPID.Encoder;
 leftPID.output = 0;
 leftPID.PrevInput = 0;
 leftPID.ITerm = 0;

 rightPID.TargetTicksPerFrame = 0.0;
 rightPID.Encoder = readEncoder(RIGHT);
 rightPID.PrevEnc = rightPID.Encoder;
 rightPID.output = 0;
 rightPID.PrevInput = 0;
 rightPID.ITerm = 0;
}

分别定义左右两个马达的dorightPID()和doleftPID()函数

/* PID routine to compute the next motor commands */
void dorightPID(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input = p->Encoder - p->PrevEnc;
  Perror = p->TargetTicksPerFrame - input;

  /*
  * Avoid derivative kick and allow tuning changes,
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
  // p->PrevErr = Perror;
  output = (right_Kp * Perror - right_Kd * (input - p->PrevInput) + p->ITerm) / right_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;
  // Accumulate Integral error *or* Limit output.
  // Stop accumulating when output saturates
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM)
    output = -MAX_PWM;
  else
  /*
  * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
    p->ITerm += Ki * Perror;

  p->output = output;
  p->PrevInput = input;
}

/* PID routine to compute the next motor commands */
void doleftPID(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input = p->Encoder - p->PrevEnc;
  Perror =p->TargetTicksPerFrame + input;

  /*
  * Avoid derivative kick and allow tuning changes,
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
  // p->PrevErr = Perror;
  output = (left_Kp * Perror - left_Kd * (input - p->PrevInput) + p->ITerm) / left_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;
  // Accumulate Integral error *or* Limit output.
  // Stop accumulating when output saturates
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM)
    output = -MAX_PWM;
  else
  /*
  * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
    p->ITerm += Ki * Perror;

  p->output = output;
  p->PrevInput = input;
}

修改updatePID()函数

void updatePID() {
  /* Read the encoders */
  leftPID.Encoder =readEncoder(LEFT);
  rightPID.Encoder = readEncoder(RIGHT);

  /* If we're not moving there is nothing more to do */
  if (!moving){
    /*
    * Reset PIDs once, to prevent startup spikes,
    * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
    * PrevInput is considered a good proxy to detect
    * whether reset has already happened
    */
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
    return;
  }

  /* Compute PID update for each motor */
  dorightPID(&rightPID);//执行右马达PID
  doleftPID(&leftPID);//执行左马达PID

  /* Set the motor speeds accordingly */
  setMotorSpeeds(leftPID.output, rightPID.output);

}

1.2. 修改encoder_driver.ino文件
由于左右两个马达的旋转方向是相反的,所以会有一个编码器的读数反向增加,但我们实际控制的时候,两个马达的行进方向是相同的,这会影响到PID的计算,所以需要对左侧马达的编码器读书取反,修改readEncoder函数。

 /* Wrap the encoder reading function */
 long readEncoder(int i) {
 if (i == LEFT) return 0-left_enc_pos;
 else return right_enc_pos;
 }

1.3.修改ROSArduinoBridge.ino文件

修改pid_args的声明为如下代码

int pid_args[8];

修改runCommand()函数,修改case UPDATE_PID,把原来的Kp,Kd,Ki,Ko赋值的代码注释掉,修改为如下的代码

    case UPDATE_PID:
      while ((str = strtok_r(p, ":", &p)) != '\0') {
        pid_args[i] = atoi(str);
        i++;
      }
//      Kp = pid_args[0];
//      Kd = pid_args[1];
//      Ki = pid_args[2];
//      Ko = pid_args[3];

      left_Kp = pid_args[0];
      left_Kd = pid_args[1];
      left_Ki = pid_args[2];
      left_Ko = pid_args[3];

      right_Kp = pid_args[4];
      right_Kd = pid_args[5];
      right_Ki = pid_args[6];
      right_Ko = pid_args[7];
      Serial.println("OK");
      break;

现在arduino端已经支持对两个电机使用不同的PID参数调速了

1.4修改ROSArduinoBridge.ino文件

修改此文件中argv1 argv2两个参数的定义,将其数组长度扩大,原因是这两个参数用于接收上位机发送的命令,我们增加了pid参数,使得命令长度超过了原来这两个参数的长度。

// Character arrays to hold the first and second arguments
char argv1[32];
char argv2[32];

2.修改上位机ROS包的代码

2.1.修改my_arduino_params.yaml
在PID参数部分增加左右两个马达的PID参数

# === PID parameters
Kp: 10
Kd: 12
Ki: 0
Ko: 50
accel_limit: 1.0

left_Kp: 10
left_Kd: 12
left_Ki: 0
left_Ko: 50

right_Kp: 8
right_Kd: 12
right_Ki: 0

2.2.修改arduino_driver.py文件
修改update_pid()函数

def update_pid(self, left_Kp, left_Kd, left_Ki, left_Ko, right_Kp, right_Kd, right_Ki, right_Ko):
        ''' Set the PID parameters on the Arduino
        '''
        print "Updating PID parameters"
        cmd = 'u ' + str(left_Kp) + ':' + str(left_Kd) + ':' + str(left_Ki) + ':' + str(left_Ko) + ':' + str(right_Kp) + ':' + str(right_Kd) + ':' + str(right_Ki) + ':' + str(right_Ko)
        self.execute_ack(cmd)  

2.3.修改base_controller.py文件
修改def init(self, arduino, base_frame):函数,增加左右两个马达PID参数的初始化代码

def __init__(self, arduino, base_frame):
        self.arduino = arduino
        self.base_frame = base_frame
        self.rate = float(rospy.get_param("~base_controller_rate", 10))
        self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
        self.stopped = False

        pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)

        #modify by william 增加左右两个马达的PID参数的初始化代码
        pid_params['left_Kp'] = rospy.get_param("~left_Kp", 20)
        pid_params['left_Kd'] = rospy.get_param("~left_Kd", 12)
        pid_params['left_Ki'] = rospy.get_param("~left_Ki", 0)
        pid_params['left_Ko'] = rospy.get_param("~left_Ko", 50)
        pid_params['right_Kp'] = rospy.get_param("~right_Kp", 20)
        pid_params['right_Kd'] = rospy.get_param("~right_Kd", 12)
        pid_params['right_Ki'] = rospy.get_param("~right_Ki", 0)
        pid_params['right_Ko'] = rospy.get_param("~right_Ko", 50)

        self.accel_limit = rospy.get_param('~accel_limit', 0.1)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)

        # Set up PID parameters and check for missing values
        self.setup_pid(pid_params)

        # How many encoder ticks are there per meter?
        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction  / (self.wheel_diameter * pi)

        # What is the maximum acceleration we will tolerate when changing wheel speeds?
        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate

        # Track how often we get a bad encoder count (if any)
        self.bad_encoder_count = 0

        now = rospy.Time.now()    
        self.then = now # time for determining dx/dy
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = now + self.t_delta

        # internal data        
        self.enc_left = None            # encoder readings
        self.enc_right = None
        self.x = 0                      # position in xy plane
        self.y = 0
        self.th = 0                     # rotation in radians
        self.v_left = 0
        self.v_right = 0
        self.v_des_left = 0             # cmd_vel setpoint
        self.v_des_right = 0
        self.last_cmd_vel = now

        # subscriptions
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)

        # Clear any old odometry info
        self.arduino.reset_encoders()

        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()

        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")

修改def setup_pid(self, pid_params):

def setup_pid(self, pid_params):
        # Check to see if any PID parameters are missing
        missing_params = False
        for param in pid_params:
            if pid_params[param] == "":
                print("*** PID Parameter " + param + " is missing. ***")
                missing_params = True

        if missing_params:
            os._exit(1)

        self.wheel_diameter = pid_params['wheel_diameter']
        self.wheel_track = pid_params['wheel_track']
        self.encoder_resolution = pid_params['encoder_resolution']
        self.gear_reduction = pid_params['gear_reduction']

        #self.Kp = pid_params['Kp']
        #self.Kd = pid_params['Kd']
        #self.Ki = pid_params['Ki']
        #self.Ko = pid_params['Ko']

        #self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)

        #modify by william
        self.left_Kp = pid_params['left_Kp']
        self.left_Kd = pid_params['left_Kd']
        self.left_Ki = pid_params['left_Ki']
        self.left_Ko = pid_params['left_Ko']

        self.right_Kp = pid_params['right_Kp']
        self.right_Kd = pid_params['right_Kd']
        self.right_Ki = pid_params['right_Ki']
        self.right_Ko = pid_params['right_Ko']

        #传递8个参数给update_pid函数
        self.arduino.update_pid(self.left_Kp, self.left_Kd, self.left_Ki, self.left_Ko, self.right_Kp, self.right_Kd, self.right_Ki, self.right_Ko)

修改完上述代码后ros_arduino_bridge可以分别对两个马达设置不同的PID参数进行调速。

3.PID调速

两个电机的PID调速是比较麻烦,需要耐心的不断修改相应参数,使两个电机的转速能够一致,笔者的经验是先调试好一侧的马达,在以这个马达作为标的物,调整另外一个马达的PID参数,当然控制精度还受到电机性能和编码器精度的限制,为了方便PID调速这里我们通过rqt_plot图形曲线把两个电机的速度,PID的输入输出显示出来。

3.1. 修改上位机base_controller.py文件

修改def init(self, arduino, base_frame):函数,在开头处增加self.debugPID参数,debugPID=true,启动调试,debugPID=false,关闭调试

class BaseController:
     def __init__(self, arduino, base_frame):
     self.arduino = arduino
     self.base_frame = base_frame
     self.rate = float(rospy.get_param("~base_controller_rate", 10))
     self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
     self.stopped = False
     self.debugPID=False

修改def init(self, arduino, base_frame):函数,在函数尾部增加如下代码

if self.debugPID:
     self.lEncoderPub = rospy.Publisher('Lencoder', Int32)
     self.rEncoderPub = rospy.Publisher('Rencoder', Int32)
     self.lPidoutPub = rospy.Publisher('Lpidout', Int32)
     self.rPidoutPub = rospy.Publisher('Rpidout', Int32)
     self.lVelPub = rospy.Publisher('Lvel', Int32)
     self.rVelPub = rospy.Publisher('Rvel', Int32)

修改def poll(self):函数,在函数头部增加如下代码

if self.debugPID:
     rospy.logdebug("poll start-------------------------------: ")
     try:
         left_pidin, right_pidin = self.arduino.get_pidin()
         self.lEncoderPub.publish(left_pidin)
         self.rEncoderPub.publish(right_pidin)
         rospy.logdebug("left_pidin: "+str(left_pidin))
         rospy.logdebug("right_pidin: "+str(right_pidin))
     except:
         rospy.logerr("getpidin exception count: ")
         return

     try:
         left_pidout, right_pidout = self.arduino.get_pidout()
         self.lPidoutPub.publish(left_pidout)
         self.rPidoutPub.publish(right_pidout)
         rospy.logdebug("left_pidout: "+str(left_pidout))
         rospy.logdebug("right_pidout: "+str(right_pidout))
     except:
         rospy.logerr("getpidout exception count: ")
         return

修改def poll(self):函数,在if not self.stopped:后面增加如下代码

if self.debugPID:
     self.lVelPub.publish(self.v_left)
     self.rVelPub.publish(self.v_right)

3.2. 修改上位机arduino_driver.py文件

增加get_pidin和get_pidout两个函数

def get_pidin(self):
    values = self.execute_array('i')
    if len(values) != 2:
         print "pidin was not 2"
         raise SerialException
         return None
    else:
         return values

def get_pidout(self):
    values = self.execute_array('f')
    if len(values) != 2:
         print "pidout was not 2"
         raise SerialException
         return None
    else:
         return values

3.3. 修改Arduino 的commands.h

增加读取pidin,和pidout的命令

#define READ_PIDOUT    'f'
#define READ_PIDIN     'i'

 

3.4. 修改Arduino 的diff_controller.h

在尾部增加readPidIn和readPidOut两个函数

long readPidIn(int i) {
  long pidin=0;
    if (i == LEFT){
    pidin = leftPID.PrevInput;
  }else {
    pidin = rightPID.PrevInput;
  }
  return pidin;
}

long readPidOut(int i) {
  long pidout=0;
    if (i == LEFT){
    pidout = leftPID.output;
  }else {
    pidout = rightPID.output;
  }
  return pidout;
}

 

3.5. 修改Arduino 的ROSArduinoBridge.ino

修改runcammand()函数,在switch部分增加case READ_PIDIN:和case READ_PIDOUT:

  switch (cmd) {
    case GET_BAUDRATE:
      Serial.println(BAUDRATE);
      break;
    case READ_PIDIN:
      Serial.print(readPidIn(LEFT));
      Serial.print(" ");
      Serial.println(readPidIn(RIGHT));
      break;
    case READ_PIDOUT:
      Serial.print(readPidOut(LEFT));
      Serial.print(" ");
      Serial.println(readPidOut(RIGHT));
      break;
     ...

3.6. 执行PID调速

在不同的终端执行下面的命令
启动roscore

roscore

增加路径到bash

. ~/catkin_ws/devel/setup.bash

启动节点

roslaunch ros_arduino_python arduino.launch

这时候我们可以发布Twist消息来控制机器人的运行,如:

rostopic pub /cmd_vel geometry_msgs/Twist -r 1 -- '[0.5, 0.0, 0.0]' '[0.0, 0.0, 0.0]'

启动rqt_plot

rqt_plot

rqt_plot的使用方法,请见官方文档http://wiki.ros.org/rqt_plot
这时候就可以通过rqt_plot所提供的图形化界面,按照如下PID调速口诀进行调试了,

参数整定找最佳,从小到大顺序查
先是比例后积分,最后再把微分加
曲线振荡很频繁,比例度盘要放大
曲线漂浮绕大湾,比例度盘往小扳
曲线偏离回复慢,积分时间往下降
曲线波动周期长,积分时间再加长
曲线振荡频率快,先把微分降下来
动差大来波动慢。微分时间应加长
理想曲线两个波,前高后低4比1
一看二调多分析,调节质量不会低。

发表评论

Scroll to top
%d 博主赞过: