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 $\omega= 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 \omega$ rather than $\omega$, hence something like $\omega =\omega +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 velocity to the flywheel. So if the error (curr_angle) is positive (i.e. bike tilts CW), then the angular velocity of the flywheel is negative and increasing, i.e. a clockwise angular acceleration, i.e. a clockwise torque, i.e. an anti-CW reaction torque.
This script somewhat manages to balance the bike for the first couple of seconds (it oscillates a bit about the vertical), but then the angular velocity of the flywheel starts to increase/decrease rapidly and eventually the bike spirals out of control.
What changes can I make to the script/my approach to prevent this from happening?
The obvious answer is to tune the PID values properly, but is there a systematic way to do this? I have tried changing around the kp values (since that is largely responsible for the oscillation), but at one value, the oscillations are too violent, and when I decrease it slightly, the reaction torque becomes insufficient to counter the original torque..
edit: Video demonstration: https://www.youtube.com/watch?v=lBRUsTbNaj0
