Self balancing bike using a flywheel

edit

(ROS melodic, Ubuntu-18.04)

I have a bike with two stands and a flywheel like this:

The stands can be adjusted so that they don't touch the ground. Now, the task is to implement a self balancing mechanism: when I run the script, (with the stands not touching the ground), the bike should simply stay at one place without tilting appreciably.

Approach: I suppose the tilt of the bike (i.e. roll angle) can be used an "error" term, (the tilt is zero when the bike is upright) to control the torque on the flywheel. If the bike tilts towards the right (i.e clockwise), then the torque on the flywheel should be clockwise, which exerts a reaction torque on the bike in the anti-clockwise direction, which counters the original torque.

The bike comes with an IMU, so we can use it to extract the position quaternion, which can be converted to Euler angles using the tf library. Then, we simply need to use PID to control the angular velocity of the flywheel.

Another point worth mentioning is that I don't think something like W= P+I+D is appropriate for this problem. What is important for balancing is the torque on the flywheel, which can be better expressed using delta W rather than W, hence something like W =W +P +I +D seems to be better for controlling the torque on the flywheel. With that in mind, I wrote this script:

#!/usr/bin/env python

from std_msgs.msg import Float32
from sensor_msgs.msg import LaserScan, Imu
from geometry_msgs.msg import Quaternion,Twist
import rospy
import tf

class sbb():
    def _init_(self):

        rospy.init_node('controller')

        self.sbb_orientation_quaternion = [0.0, 0.0, 0.0, 0.0]
        self.sbb_orientation_euler = [0.0, 0.0, 0.0]


        self.sample_rate = 50.0



        self.curr_angle = 0.0
        self.prev_angle = 0.0
        self.w=0.0
        self.I=0

        self.data_cmd = Float32()
        self.data_cmd.data = 0.0
        self.data_pub = rospy.Publisher('/flywheel/command', Float32, queue_size=10)

        rospy.Subscriber('/sbb/imu', Imu, self.imu_callback)


    def imu_callback(self, msg):

        self.sbb_orientation_quaternion[0] = msg.orientation.x
        self.sbb_orientation_quaternion[1] = msg.orientation.y
        self.sbb_orientation_quaternion[2] = msg.orientation.z
        self.sbb_orientation_quaternion[3] = msg.orientation.w



    def pid(self):

       (self.sbb_orientation_euler[1], self.sbb_orientation_euler[0], self.sbb_orientation_euler[2]) = tf.transformations.euler_from_quaternion([self.sbb_orientation_quaternion[0], self.sbb_orientation_quaternion[1], self.sbb_orientation_quaternion[2], self.sbb_orientation_quaternion[3]])
           #self.sbb_orientation_euler=[pitch,roll,yaw]
           self.curr_angle = self.sbb_orientation_euler[1]
           rospy.loginfo("angle: " + str(self.curr_angle))

           kp=3.5
           kd=0.02
           dt=0.02
           ki=0.001
           self.I=self.I + self.curr_angle*dt

           self.w=self.w - (kp*self.curr_angle)-(kd*(self.curr_angle-self.prev_angle)/dt)-(ki*self.I)     

           self.data_cmd.data= self.w
           rospy.loginfo("flywheel w: "+str(self.w))
           self.data_pub.publish(self.data_cmd.data)

           self.prev_angle=self.curr_angle




if _name_ == '_main_':

    sbb = sbb()
    r = rospy.Rate(sbb.sample_rate)  
    while not rospy.is_shutdown():

        try:    
            sbb.pid()
            r.sleep()

The - signs in the PID function have directional significance. A negative value publishes a clockwise angular ... (more)