ros_arduino_bridge的base controller中对两个电机的PID控制是用的一套PID参数，但实际的使用中，由于马达的特性，地形，或者机器人的载重的平衡性的诸多问题使机器人不能按照预定轨迹行驶，最简单的就是机器人是否能够走直线，这就需要对两个电机分别进行PID调速，本文中将介绍如何对ros_arduino_bridge进行修改，以使支持对两路电机用不同的PID参数调速
1.首先修改arduino代码
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%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 */
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参数调速了
2.修改上位机ROS包的代码
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: 0
b.修改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参数进行调速。
原创文章，作者：奋斗，如若转载，请注明出处：https://blog.ytso.com/6907.html