更多創客作品，請關注筆者網站園丁鳥，搜集全球極具創意，且有價值的創客作品
ROS機器人知識請關注，diegorobot
業餘時間完成的一款線上統計過程分析工具SPC，及SPC知識分享網站qdo
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參數進行調速。
![linux-svn解除安裝與安裝[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)