# Python PID tuning - Parrot Bebop 2 - Target follower - ROS

I'm developing a PID controller for making the drone follow a detected object.

I used as reference these topics:

I wrote a Python code that interacts with ROS bebop_autonomy package.

For the detection, I used YOLO.

The problem is that I'm not sure if the PID that I wrote is correct and I don't know how to tune the gain parameters. I have read about the theory of PID tuning like What are good strategies for tuning PID loops?

I also know that with an input step you can graph the response and then based on it tune the gains until you obtain the desired characteristics but here I don't know how to do the graphs as the input is a pixel value of the bounding box.

Do you know how should I tune the gain parameters? Also, let me know if how I developed the PID is correct or if there is a better way to do it.

Code:

import rospy
from darknet_ros_msgs.msg import BoundingBoxes
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist

def set_state(input):   #possible states: "pd", "np " person detected, no person
global state
state = input
def get_state():
return(state)

def callback(data):

Cy_center = 340 #x center position of the image
Cz_center = 240 #y center position of the image
Area_id = 640 * 480 * 0.30 # 30% of pixel area (for x)
K_x = 1 #vel_x paramete
sum_error_x = 0
sum_error_y = 0
sum_error_z = 0

#parameters to tune
Kp_fb = 0.2 #proportional gain FORWARD/BACKWARD
Kd_fb = 1.5 #derivative gain FORWARD/BACKWARD
Ki_fb = 1 #integral gain FORWARD/BACKWARD
Kp_lr = 0.2 #proportional gain LEFT/RIGHT
Kd_lr = 1.5 #derivative gain LEFT/RIGHT
Ki_lr = 1 #integral gain LEFT/RIGHT
Kp_ud= 0.2 #proportional gain UP/DOWN
Kd_ud = 1.5 #derivative gain UP/DOWN
Ki_ud = 1 #integral gain UP/DOWN

state = get_state()

pub = rospy.Publisher('bebop/cmd_vel', Twist, queue_size = 1)
pub_land = rospy.Publisher('bebop/land', Empty, queue_size = 1)
pub_takeoff = rospy.Publisher('bebop/takeoff', Empty, queue_size = 1)
empty_msg = Empty()
twist = Twist()

if state == "np":
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0;
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = -0.3;
pub.publish(twist)
print("Looking for person")

for box in data.bounding_boxes:

if box.Class == "person":

set_state("pd")
Cy = box.xmin+(box.xmax-box.xmin)/2;
Cz = box.ymin+(box.ymax-box.ymin)/2;

Area_ref = ((box.xmax - box.xmin) * (box.ymax - box.ymin))
error_y = Cy_center - Cy # LEFT/RIGHT
#error_z = Cz_center - Cz # UP/DOWN
error_z = Cz_center - box.ymin #so drone will fly higher
error_x = Area_id - Area_ref #FORWARD/BACKWARD
vel_y = error_y/320
if vel_y >= 1:
vel_y = 1
elif vel_y <= -1:
vel_y = -1
vel_z = error_z/320
if vel_z >= 1:
vel_z = 1
elif vel_z <= -1:
vel_z = -1
vel_x = (error_x*K_x)/Area_id
if vel_x >= 1:
vel_x = 1
elif vel_x <= -1:
vel_x = -1

#PID X AXES
sum_error_x = sum_error_x + vel_x
last_error_x = vel_x
output_x = Kp_fb * vel_x + Kd_fb * (vel_x - last_error_x) + Ki_fb * sum_error_x

#PID Y AXES
sum_error_y = sum_error_y + vel_y
last_error_y = vel_y
output_y = Kp_lr * vel_y + Kd_lr * (vel_y - last_error_y) + Ki_lr * sum_error_y

#PID Z AXES
sum_error_z = sum_error_z + vel_z
last_error_z = vel_z
output_z = Kp_lr * vel_z + Kd_lr * (vel_z - last_error_z) + Ki_lr * sum_error_z

#SEND COMMANDS
twist.linear.x = 0.5*output_x; twist.linear.y = 0.5*output_y; twist.linear.z = 0.5*output_z;
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0;
pub.publish(twist)

"vel_x: {}, vel_y: {}, vel_z: {}, Classe: {}".format(round(output_x,2), round(output_y,2), round(output_z,2), box.Class)
)

def main():
set_state("np")
while not rospy.is_shutdown():

rospy.init_node('listener', anonymous=True)
rate = rospy.Rate(1)
rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes , callback )
set_state("np") #reset state to no person in case detector fails to detect
rospy.spin() #blocks until ros node is shutdown

if __name__ == '__main__':
try :
main()
except rospy.ROSInterruptException:
pass

• Take a look at this post May 17 '19 at 7:02
• How do I find the FOPDT model of my system and then how do I find the parameters from the step response? The problem is that I don't know how to apply it in my case because my input is the center of the ideal bounding box + ideal area. How can I graph it for viewing the response of the system? May 17 '19 at 8:58
• Also my error as it is in pixel is then converted in velocity in the PID May 17 '19 at 9:05
• Maybe I could add in the code 3 graphs with the value of centre of the detected bounding box + area in the time and then from there see how's the response when the detected person is far away from the centre. What do you think? @Benyamin Jafari May 17 '19 at 9:17
• I don't know exactly about that problem. May 17 '19 at 9:24

    #PID X AXES
sum_error_x = sum_error_x + vel_x
last_error_x = vel_x
output_x = Kp_fb * vel_x + Kd_fb * (vel_x - last_error_x) + Ki_fb * sum_error_x

#PID Y AXES
sum_error_y = sum_error_y + vel_y
last_error_y = vel_y
output_y = Kp_lr * vel_y + Kd_lr * (vel_y - last_error_y) + Ki_lr * sum_error_y

#PID Z AXES
sum_error_z = sum_error_z + vel_z
last_error_z = vel_z
output_z = Kp_lr * vel_z + Kd_lr * (vel_z - last_error_z) + Ki_lr * sum_error_z


Your error for the derivative terms are all wrong. vel_x - last_error_x is always zero because you assign the last error variables as vel variable.