[发明专利]一种基于低通滤波的倒立摆控制方法、装置及系统

权利要求书

1.一种基于低通滤波的倒立摆控制方法，其特征在于，包括：

获取倒立摆摆杆角度信息w_r(k)和旋转臂位置信息m_r(k)；

根据倒立摆摆杆角度信息w_r(k)，判断当前运动状态；

基于当前运动状态，根据得到的倒立摆摆杆角度信息w_r(k)和旋转臂位置信息m_r(k)，结合预设的旋摆杆目标角度w_i(k)、旋转臂目标位置M_i(k)，通过双环低通滤波PID算法计算电机PWM输出值；包括：

倒立状态，采用双环低通滤波PID算法，包括角度环负反馈PID和位置环正反馈PID，角度环负反馈PID所得值U_a，位置环正反馈PID所得值U_d，计算电机PWM输出值U＝U_a+U_d；

角度环负反馈PID：

U_a＝U_aP(k)+U_aI(k)+U_aD(k)

w_i(k)为预设的摆杆目标角度，摆杆角度偏差e_a(k)＝w_i(k)-w_r(k)；

其中U_aP(k)为角度环比例控制器，U_aP(k)＝K_aP•e_a(k)，K_aP为比例系数；

U_aI(k)为角度环积分控制器，K_aI为积分系数；

U_aD(k)为微分控制器，U_aD(k)＝K_aD(1-α)[e_a(k)-e_a(k-1)]+αU_aD(k-1),K_aD为微分控制系数，T为角度传感器采样周期，T_f为低通滤波系数；

位置环正反馈PID：U_d＝U_dP(k)+U_dI(k)+U_dD(k)，

M_i(k)为预设的旋转臂目标位置，旋转臂位置偏差e_d(k)＝M_i(k)-M_r(k)；

其中，U_dP(k)为角度环比例控制器，U_dP(k)＝K_dP·e(k)，K_dP为比例系数；

U_dI(k)为角度环积分控制器，K_dI为积分系数；

U_dD(k)为微分控制器，U_dD(k)＝K_dD(1-β)[e(k)-e(k-1)]+βU_dD(k-1),K_dD为微分控制系数，T’为编码器采样周期，T_f’为低通滤波系数；

根据计算得到的电机PWM输出值，发出指令控制电机转速。

2.根据权利要求1所述的基于低通滤波的倒立摆控制方法，其特征在于，倒立摆运动状态包括：预起摆、起摆、倒立状态。

3.根据权利要求1所述的基于低通滤波的倒立摆控制方法，其特征在于，根据倒立摆摆杆角度信息w_r(k)，判断当前运动状态；包括：

当倒立摆摆杆角度信息w_r(k)＜Ws，Ws为预设的倒立摆摆杆角度阈值，为预起摆运动状态；

当倒立摆摆杆角度信息w_r(k)≥Ws，Ws为预设的倒立摆摆杆角度阈值，则进入起摆运动状态；

当倒立摆摆杆角度值w_r(k)∈{Wc-d,Wc+d}，Wc为预设的倒立摆摆杆倒立角度，d为允许角度偏差，则进入倒立状态。