1
$\begingroup$

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)


    rospy.loginfo(
        "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
$\endgroup$
  • $\begingroup$ Take a look at this post $\endgroup$ – Benyamin Jafari May 17 at 7:02
  • $\begingroup$ 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? $\endgroup$ – Ilyas May 17 at 8:58
  • $\begingroup$ Also my error as it is in pixel is then converted in velocity in the PID $\endgroup$ – Ilyas May 17 at 9:05
  • $\begingroup$ 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 $\endgroup$ – Ilyas May 17 at 9:17
  • $\begingroup$ I don't know exactly about that problem. $\endgroup$ – Benyamin Jafari May 17 at 9:24
0
$\begingroup$
    #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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.