更多创客作品，请关注笔者网站园丁鸟，搜集全球极具创意，且有价值的创客作品 ROS机器人知识请关注，diegorobot 业余时间完成的一款在线统计过程分析工具SPC，及SPC知识分享网站qdo
ros_arduino_bridge的base controller中对两个电机的PID控制是用的一套PID参数，但实际的使用中，由于马达的特性，地形，或者机器人的载重的平衡性的诸多问题使机器人不能按照预定轨迹行驶，最简单的就是机器人是否能够走直线，这就需要对两个电机分别进行PID调速，本文中将介绍如何对ros_arduino_bridge进行修改，以使支持对两路电机用不同的PID参数调速
a.修改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;分别定义左右两个马达的dorightPID()和doleftPID()函数
/* PID routine to compute the next motor commands */ void dorightID(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’s-pid-derivative-kick/ * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner’s-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’s-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’s-pid-derivative-kick/ * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner’s-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’s-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’s-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 */ dorightID(&rightPID);//执行右马达PID doleftPID(&leftPID);//执行左马达PID /* Set the motor speeds accordingly */ setMotorSpeeds(leftPID.output, rightPID.output); }b.修改ROSArduinoBridge.ino文件
修改argv1 和argv2的数组长度
char argv1[32]; char argv2[32];修改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参数调速了
a.修改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: 0b.修改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)c.修改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参数进行调速。
