[發明專利]一種自平衡機器人抗持續性外力作用的姿態控制方法

有效

• 申請號: 202010435039.1
• 申請日: 2020-05-21
• 公開（公告）號: CN111665838B
• 公開（公告）日: 2023-08-29
• 發明（設計）人: 董輝;吳宇航;田叮;吳祥;袁登鵬;董浩;陳重;周俊陽
• 申請（專利權）人: 浙江工業大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 杭州君度專利代理事務所(特殊普通合伙) 33240
• 代理人: 楊天嬌

1.一種自平衡機器人抗持續性外力作用的姿態控制方法，其特征在于，所述自平衡機器人為兩輪自平衡機器人且左右輪軸上分別安裝有編碼器，所述自平衡機器人抗持續性外力作用的姿態控制方法，包括：

步驟S1、定時讀取左右輪軸分別對應的編碼器的數值，將兩編碼器的數值相加作為狀態參數，所述狀態參數具有正負；

步驟S2、根據所述狀態參數判斷當前自平衡機器人的運行狀態，包括：

若所述狀態參數的絕對值小于第一臨界值，則當前自平衡機器人的運行狀態為正常工作狀態；

若所述狀態參數的絕對值大于第二臨界值，則當前自平衡機器人的運行狀態為失控偏移狀態；

若所述狀態參數的絕對值位于第一臨界值和第二臨界值之間，則當前自平衡機器人的運行狀態為可調偏移狀態；

步驟S3、根據所述運行狀態，采用PID控制器調節自平衡機器人的姿態，包括：

若運行狀態為正常工作狀態，則不需要重心調節；

若運行狀態為可調偏移狀態，則通過速度環PI控制器抵消自平衡機器人受的持續性水平外力維持原地平衡；

若運行狀態為失控偏移狀態，則通過角度補償的角度環PD控制器改變自平衡機器人的重心，抵消受到的持續性水平外力維持原地平衡；

其中，所述通過角度補償的角度環PD控制器改變自平衡機器人的重心，抵消受到的持續性水平外力維持原地平衡，包括：

建立角度環PD控制器的輸出公式為：

Balance_PWM＝Balance_Kp*Angle+Balance_Kd*Gyro

式中，Balance_PWM為角度環PD控制器最終輸出的PWM波大小；Balance_Kp為PD環的比例參數；Angle是陀螺儀采集到的自平衡機器人當前傾角；Balance_Kd為PD環的微分參數；Gyro是陀螺儀采集到的自平衡機器人當前傾斜方向的角速度；

則角度補償的角度環PD控制器的輸出公式為：

Balance_PWM＝Balance_Kp*(Angle+Central_Point)+Balance_Kd*Gyro

式中，Central_Point為設置機器人重心點的值，當Central_Point的值取正數時，則調整機器人的重心點向后改變，當Central_Point的值取負數時，則調整機器人的重心點向前改變，并且Central_Point的絕對值越大，機器人的重心點離機器人的物理重心零點越遠，即機器人的傾斜角度越大；

其中，設置自平衡機器人重心點的值Central_Point的調整公式如下：

Central_Point＝Mechanical_Zero+Angle_Bias

式中，Mechanical_Zero為機器人機體的物理重心零點，且取值為0，Angle_Bias為每隔預設間隔改變自平衡機器人的角度值，如果需要自平衡機器人重心點往后偏移，則Angle_Bias值取正數；如果需要自平衡機器人重心點往前偏移，則Angle_Bias值取負數；

因此，更新角度補償的角度環PD控制器的輸出公式為：

Balance_PWM＝Balance_Kp*(Angle+Mechanical_Zero+Angle_Bias)+Balance_Kd*Gyro

根據角度補償的角度環PD控制器的輸出Balance_PWM，改變自平衡機器人的重心，抵消受到的持續性水平外力維持原地平衡。

2.如權利要求1所述的自平衡機器人抗持續性外力作用的姿態控制方法，其特征在于，若所述自平衡機器人為向前運動，則狀態參數為正數；若所述自平衡機器人為向后運動，則狀態參數為負數。

3.如權利要求1所述的自平衡機器人抗持續性外力作用的姿態控制方法，其特征在于，所述每隔預設間隔改變自平衡機器人的角度值Angle_Bias的取值范圍為[-0.05，-0.01]∪[0.01，0.05]。