[發明專利]一種基于PID控制算法的掃地機器人沿線行走控制系統及其控制方法

有效

• 申請號: 201910940966.6
• 申請日: 2019-09-30
• 公開（公告）號: CN110840344B
• 公開（公告）日: 2021-08-06
• 發明（設計）人: 陳振兵;張馳洲
• 申請（專利權）人: 湖南格蘭博智能科技有限責任公司
• 主分類號: A47L11/40
• 分類號: A47L11/40;A47L11/24
• 代理公司: 北京眾合誠成知識產權代理有限公司 11246
• 代理人: 李寶碩

1.一種基于PID控制算法的掃地機器人沿線行走控制系統，包括掃地機機體、左轉動輪、右轉動輪，其特征在于，包括底層驅動模塊、控制模塊以及電源模塊，所述底層驅動模塊包括掃地機機體姿態測量單元以及驅動單元，所述機體姿態測量單元設置于所述掃地機機體內，所述驅動單元用于驅動左、右轉動輪帶動所述掃地機機體移動，所述左、右轉動輪上均對應設置有用于測量左、右轉動輪行進過程中脈沖數的左、右編碼單元，所述控制模塊為PID控制模塊，所述機體姿態測量單元、驅動單元、左編碼單元、右編碼單元均電性連接所述PID控制模塊，所述機體姿態測量單元、驅動單元、左編碼單元、右編碼單元、PID控制模塊均電性連接所述電源模塊；

所述PID控制模塊包括主控單元、姿態環PID控制單元、速度環PID控制單元和偏距環PID控制單元，所述機體姿態測量單元電性連接所述姿態環PID控制單元，所述左、右編碼單元均電性連接所述速度環PID控制單元，所述姿態環PID控制單元與所述速度環PID控制單元電性連接所述偏距環PID控制單元，所述姿態環PID控制單元、所述速度環PID控制單元、所述偏距環PID控制單元均設置于所述主控單元上；

所述姿態環PID控制單元以及所述偏距環PID控制單元采用位置式PID控制算法，所述速度環PID采用增量式PID控制算法；

所述機體姿態測量單元為陀螺儀，所述左、右編碼單元為左、右光電編碼器，所述驅動單元包括左直流有刷電機、右直流有刷電機，所述左、右光電編碼器對應設置于左、右直流有刷電機上。

2.一種基于PID控制算法的掃地機器人沿線行走的控制方法，其特征在于，包括如權利要求1所述的一種基于PID控制算法的掃地機器人沿線行走控制系統，所述掃地機機體內還設置有定時單元，所述定時單元電性連接所述左光電編碼器、右光電編碼器以及PID控制模塊：

步驟1：開啟掃地機器人；

步驟2：所述掃地機器人系統初始化，清除所述陀螺儀、左光電編碼器、右光電編碼器內存儲的采集數據；

步驟3：所述掃地機器人發送起始信號至定時單元，若定時單元未向主控單元發送中斷信號，則所述姿態環PID控制單元掃描陀螺儀采集的數據，且所述速度環PID控制單元同步掃描左、右光電編碼器掃描的數據，否則執行步驟2；

步驟4：所述姿態環PID控制單元和所述速度環PID控制單元分別將掃描數據與設定值分析對比，并同步將對比值發送至主控單元和偏距環PID控制單元；

步驟5：所述偏距環PID控制單元對接收自姿態環PID控制單元和速度環PID控制單元的對比值進行計算并與設定值進行對比分析后將對比值發送至主控單元，所述主控單元對接收到的對比值進行綜合分析，并根據分析值控制左、右轉動輪運動。

3.根據權利要求2所述的一種基于PID控制算法的掃地機器人沿線行走的控制方法，其特征在于，所述姿態環PID控制單元將掃描自陀螺儀的實時機體姿態角與設定期望姿態角進行對比分析，計算出姿態環PWM占空比值，所述速度環PID控制單元根據掃描自左、右光電編碼器的單位采樣時間內的實時脈沖計數得出實時左、右轉動輪速度值，并將實時左、右轉動輪速度值與設定期望速度值進行對比分析，計算出左、右轉動輪速度環PWM占空比值，所述姿態環PID控制單元與所述速度環PID控制單元均同步將計算值發送至偏距環PID控制單元和主控單元；

所述偏距環PID控制單元根據接收的姿態環PWM占空比值和左、右轉動輪速度環PWM占空比值計算出當前掃地機器人的實時偏距，且根據實時偏距與設定期望偏距進行對比分析，得出偏距環PWM占空比值，并將偏距環PWM占空比值發送至主控單元；

所述主控單元對接收到的姿態環PWM占空比值、速度環PWM占空比值以及偏距環PWM占空比值進行綜合計算分析，得出左轉動輪PWM占空比和右轉動輪PWM占空比，并根據左、右轉動輪PWM占空比對應發送至所述左、右直流有刷電機控制掃地機器人的行進軌跡。

4.根據權利要求3所述的一種基于PID控制算法的掃地機器人沿線行走的控制方法，其特征在于，所述主控單元綜合計算公式為

左轉動輪PWM占空比＝左轉動輪速度環PWM占空比+偏距環PWM占空比+0.5姿態環PWM占空比；

右轉動輪PWM占空比＝右轉動輪速度環PWM占空比-偏距環PWM占空比+0.5姿態環PWM占空比。