调试小车平衡模式下直立环已经基本稳定,可以保持小车30s左右直立

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -27,19 +27,24 @@ base_frame: base_footprint
 odom_name: odom
 
 # Robot drivetrain parameters
-wheel_diameter: 0.067
-wheel_track: 0.21      #L value
-encoder_resolution: 11 #12V DC motors
-gear_reduction: 30     #empty payload rpm 330 r/m,if rpm=530,gear_reduction=19
+wheel_diameter: 0.067   #车轮的直径
+wheel_track: 0.205      #两个车轮之间距离
+encoder_resolution: 11  #12V DC motors
+gear_reduction: 30      #空载转速330rpm电机的减速比
 linear_scale_correction: 1.00
 angular_scale_correction: 1.00
 
 #是否开启平衡车模式,直立pd控制
-balance_mode: False  #开启时设置为True
-middle_val: 4.28     #小车物理平衡时的俯仰角度
+balance_mode: True   #开启时设置为True
+middle_val: 4.26     #小车物理平衡时的俯仰角度
 motor_stop_delay: 11 #单位ms
-balance_kp: 35    #35
-balance_kd: 2.3   #2.3
+check_down_pitch: 18 #当倾斜角超过该度数,认为小车已经倒下,停止电机转动
+pitch_tolerance: 0.2 #当平衡模式时,该角度倾斜范围内不进行调平
+
+balance_kp: 10.0   #直立环kp参数,25最大值,调试值17,17*0.6=10.2为正常值
+balance_kd: 0.64   #直立环kd参数,2.0最大值,调试值1.2,1.2*0.6=0.65
+speed_kp: 15.0     #速度环的kp参数
+speed_ki: 0.075    #速度环的ki参数,一般情况ki=kp/200
 
 # PID parameters
 debugPID: False

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/balance_mode.py

@@ -9,42 +9,73 @@
     20211005:初始化代码.
 """
 
-import os
-import rospy
+import os,rospy
 import roslib; roslib.load_manifest('ros_arduino_python')
 from std_msgs.msg import Float32
 from sensor_msgs.msg import Imu
 
+
 class BalanceMode:
     def __init__(self):
-        self.angularVal = 57.2957
+        self.angularVal = 57.296     #1弧度对应的角度
+        self.encoderFilterPer = 0.7  #低通滤波系数
 
         self.pitchData = 0
         self.angularPitch = 0
+        self.encoder_integral = 0
+        self.encoder_lowout = 0  #低通滤波上次的输出值
+        self.downPitchData = 0   #pitch角度超过该值,认为小车倒下,停止电机转动
 
-        self.middle_val = rospy.get_param("~middle_val", 4.3)
-        self.balance_kp = rospy.get_param("~balance_kp", 1)
-        self.balance_kd = rospy.get_param("~balance_kd", 1)
+        self.middle_val  = rospy.get_param("~middle_val", 4.3)
+        self.downPitchData = rospy.get_param("~check_down_pitch", 18)
+        self.pitch_tolerance = rospy.get_param("~pitch_tolerance", 0.2)
+        self.balance_kp  = rospy.get_param("~balance_kp", 17)
+        self.balance_kd  = rospy.get_param("~balance_kd", 1)
+        self.speed_kp    = rospy.get_param("~speed_kp", 1)
+        self.speed_ki    = rospy.get_param("~speed_ki", 1)
 
         rospy.Subscriber("/pitch_data", Float32, self.pitchDataProc)
         rospy.Subscriber("/imu_data", Imu, self.angularProc)
 
     def pitchDataProc(self, msg):
-        self.pitchData = msg.data*self.angularVal
+        self.pitchData = msg.data * self.angularVal
 
     def angularProc(self, msg):
-        self.angularPitch = msg.angular_velocity.y*self.angularVal
-
-    def keepBalance(self):
-        pitch_err = self.pitchData - self.middle_val
-        motorPWM = int(self.balance_kp*pitch_err + self.balance_kd*self.angularPitch)
-                          
-        #将计算得到的pwm值进行限幅,最大pwm值为255
-        if motorPWM > 255:
-            motorPWM = 255
-        elif motorPWM < -255:
-            motorPWM = -255
-
-        rospy.loginfo("pitch=" + str(self.pitchData) + \
-                      " angular=" + str(self.angularPitch) + " PWM=" + str(motorPWM))
+        self.angularPitch = msg.angular_velocity.y * self.angularVal
+
+    #平衡小车直立环,PD控制器,这里需要提前确定小车的机械中值,物理平衡值
+    def keepBalance(self, pitch_err):
+        if abs(pitch_err) > self.pitch_tolerance:
+            motorPWM = int(self.balance_kp*pitch_err + self.balance_kd*self.angularPitch)
+            return motorPWM
+        else:
+            return 0
+
+    #平衡小车速度环,PI控制器
+    def keepVelocity(self, encoder_A, encoder_B, target_speed):
+        encoder_err = encoder_A + encoder_B
+
+        #电机编码器计数进行一阶低通滤波
+        self.encoder_lowOut = (1-self.encoderFilterPer)*encoder_err + \
+                            self.encoderFilterPer*self.encoder_lowout
+
+        self.encoder_integral += self.encoder_lowOut    #速度偏差进行积分
+
+        self.encoder_integral -= target_speed
+        #积分限幅
+        if self.encoder_integral > 1000:
+            self.encoder_integral = 1000
+        elif self.encoder_integral < -1000:
+            self.encoder_integral = -1000
+        
+        #计算控制量pwm
+        motorPWM = self.speed_kp*self.encoder_lowOut + self.speed_ki*self.encoder_integral
+        #rospy.loginfo("encoder A:B="+str(encoder_A)+":"+str(encoder_B)+" speed pwm=" + str(motorPWM))
         return motorPWM
+
+    #检查小车是否已经倒下,倒下后停止电机转动
+    def checkIfDown(self, pitch_err, pwm_A, pwm_B):
+        if pitch_err < self.downPitchData and pitch_err > (-self.downPitchData):
+            return pwm_A, pwm_B
+        else:
+            return 0, 0   

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -32,7 +32,8 @@ class BaseController:
         self.isBalance  = balance_mode
         self.cmd_topic  = cmd_topic
         self.PID_rate   = 100  #下位机中pid固定频率100hz
-
+        self.target_speed = 0
+        
         self.odom_name  = rospy.get_param("~odom_name", "odom")
         self.debugPID   = rospy.get_param('/dynamic_tutorials/debugPID', False)
         
@@ -146,26 +147,30 @@ class BaseController:
             self.arduino.i2c_update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
                                         self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,)
 
+    def pwm_ctrl(self, pwmA, pwmB):
+        if pwmA > 255:
+            pwmA = 255
+        elif pwmA < -255:
+            pwmA = -255
+
+        if pwmB > 255:
+            pwmB = 255
+        elif pwmB < -255:
+            pwmB = -255
+
+        return int(pwmA), int(pwmB)
+
     #底盘运动控制的主循环,执行频率由arduino_node中的频率决定,默认50hz
     def poll(self):
         #发送调试pid参数的数据
         self.send_debug_pid()
 
-        #是否开启平衡车模式,计算控制周期内控制电机的pwm值
-        if self.isBalance:
-            pwm = self.balanceControl.keepBalance()
-            if self.use_serial:
-                self.arduino.pwm_drive(-pwm, pwm)
-            else:
-                self.arduino.i2c_pwm_drive(-pwm, pwm)
-
         #读取编码器计数值,用于计算里程计信息
         try:
             if self.use_serial:
                 aWheel_enc, bWheel_enc = self.arduino.get_encoder_counts()
             else:
                 aWheel_enc, bWheel_enc = self.arduino.i2c_get_encoder_counts()
-                rospy.loginfo("get encoder counts: "+str(self.arduino.i2c_get_encoder_counts()))
         except:
             try:
                 #rospy.logwarn("Failed to get encoder counts, try again. ")
@@ -176,16 +181,51 @@ class BaseController:
             except:
                 rospy.logerr("Encoder exception count !")
                 return
+        #rospy.loginfo("Encoder A:B=" + str(aWheel_enc) + ":" + str(bWheel_enc))
+
+        #计算编码器计数差值,当前值减去上次的计数值
+        encoder_diff_A = aWheel_enc - self.enc_A
+        encoder_diff_B = bWheel_enc - self.enc_B
 
         #当读取到最新的编码器计数为0,说明下位机已经将计数清零,那上位机保存的上一次编码器计数也得清零
-        if aWheel_enc == 0:
+        if aWheel_enc == 0 and bWheel_enc == 0:
             self.enc_A = 0
-        if bWheel_enc == 0:
             self.enc_B = 0
+            if self.isBalance:
+                self.balanceControl.encoder_integral = 0
+                self.balanceControl.encoder_lowout = 0
+
+        #是否开启平衡车模式,在控制周期内计算控制电机的pwm值
+        if self.isBalance:
+            #计算小车当前的倾角,用于直立环和判断小车是否倒下
+            pitch_err = self.balanceControl.pitchData - self.balanceControl.middle_val
+
+            #启动直立环控制,PD控制器,控制pitch角度和角速度
+            stand_pwm = self.balanceControl.keepBalance(pitch_err)
+
+            #启动速度环控制,PI控制器
+            #speed_pwm = self.balanceControl.keepVelocity(encoder_diff_A, encoder_diff_B, self.target_speed)
+            speed_pwm = 0
+
+            #启动转向环控制
+            turn_pwm = 0
+
+            #计算最终发送给电机的pwm
+            pwm_A_tmp = -(stand_pwm + speed_pwm - turn_pwm)
+            pwm_B_tmp = stand_pwm + speed_pwm + turn_pwm
+            
+            #将计算得到的pwm值进行限幅,最大pwm值为255,保持正负号
+            pwm_A, pwm_B = self.pwm_ctrl(pwm_A_tmp, pwm_B_tmp)
+            pwm_A, pwm_B = self.balanceControl.checkIfDown(pitch_err, pwm_A, pwm_B)
+            #rospy.loginfo("pwm_A:pwm_B= " + str(pwm_A) + " : " + str(pwm_B))
+            if self.use_serial:
+                self.arduino.pwm_drive(pwm_A, pwm_B)
+            else:
+                self.arduino.i2c_pwm_drive(pwm_A, pwm_B)
 
         #根据编码器两次反馈的差值来计算车轮移动的距离
-        dist_A = (aWheel_enc - self.enc_A)/self.ticks_per_meter
-        dist_B = (bWheel_enc - self.enc_B)/self.ticks_per_meter
+        dist_A = encoder_diff_A/self.ticks_per_meter
+        dist_B = encoder_diff_B/self.ticks_per_meter
 
         #保存当前编码器计数值,用于下次计算
         self.enc_A = aWheel_enc