[發明專利]一種機器人糾偏系統及其控制方法

有效

• 申請號: 201711243449.0
• 申請日: 2017-11-30
• 公開（公告）號: CN108021134B
• 公開（公告）日: 2021-02-02
• 發明（設計）人: 劉昌臻;陳思鑫;許偉堅;梁良飛;池瑞楨;張銀榮
• 申請（專利權）人: 廈門華蔚物聯網科技有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 廈門智慧呈睿知識產權代理事務所(普通合伙) 35222
• 代理人: 郭福利;魏思凡

1.一種機器人糾偏系統，其特征在于，包括供電單元、角度傳感器、濾波單元、主控芯片、機器人主體、電機和若干個輪子；所述角度傳感器、濾波單元、主控芯片、輪子、電機與所述供電單元依次電連接，若干個所述輪子兩兩并列分布于所述機器人主體底部；所述角度傳感器設置于所述機器人主體的幾何中心點，所述角度傳感器以所述幾何中心點為原點，以所述機器人主體的寬度方向及長度方向分別為x軸和y軸建立直角坐標系；以x軸正半軸0°起點線，逆時針方向將坐標系劃分8個等分區，Ⅰ：0°～45°、Ⅱ：45～90°、Ⅲ：90°～135°、Ⅳ：135°～180°、Ⅴ：180°～225°、Ⅵ：225°～270°、Ⅶ：270°～315°、Ⅷ：315°～360°，使所述機器人 主體對稱線落在Ⅰ、Ⅲ、Ⅴ、Ⅶ區內判斷為左傾，Ⅱ、Ⅳ、Ⅵ、Ⅷ區內判斷為右傾；所述角度傳感器用于收集機器人實時角度α₁傳送給所述濾波單元，其中，所述實時角度α₁為所述機器人主體行進方向與x軸的夾角；所述濾波單元用于對所述實時角度數據進行濾波處理；所述主控芯片根據濾波處理后的實時角度數據實現對電機和輪子的糾偏控制。

2.根據權利要求1所述的機器人糾偏系統，其特征在于，所述角度傳感器為九軸傳感器。

3.根據權利要求2所述的機器人糾偏系統，其特征在于，所述濾波單元包括濾波處理模塊，定義α₁經濾波后的數據為α₂，所述主控芯片根據α₂對所述機器人主體進行是否水平的判斷。

4.根據權利要求1所述的機器人糾偏系統，其特征在于，所述主控芯片包括存儲模塊，用于記錄所述機器人主體實施糾偏前的運轉狀態。

5.根據權利要求1所述的機器人糾偏系統，其特征在于，所述主控芯片內設糾偏模塊，所述糾偏模塊包括自動糾偏模式與調整糾偏模式。

6.如權利要求1-5任一項所述機器人糾偏系統的控制方法，其特征在于，包括以下步驟：

S1：角度測量系統校準歸零；

S2：以所述機器人主體的幾何中心點為原點，建立直角坐標系；定義所述機器人主體沿y軸正方向行進為上行狀態，反之為下行狀態；定義所述機器人主體沿x軸正方向行進為上行狀態，反之為下行狀態；當所述機器人主體沿y軸正方向行進時，定義以y軸正半軸與x軸構成上行區，以y軸負半軸與x軸構成下行區；當所述機器人主體沿x軸正方向行進時，定義以x軸正半軸與y軸構成上行區，以x軸負半軸與y軸構成下行區；以x軸正半軸0°起點線，逆時針方向將坐標系劃分8個等分區，Ⅰ：0°～45°、Ⅱ：45～90°、Ⅲ：90°～135°、Ⅳ：135°～180°、Ⅴ：180°～225°、Ⅵ：225°～270°、Ⅶ：270°～315°、Ⅷ：315°～360°，使所述機器人主體對稱線落在Ⅰ、Ⅲ、Ⅴ、Ⅶ區內判斷為左傾，Ⅱ、Ⅳ、Ⅵ、Ⅷ區內判斷為右傾；

S3，獲取x軸與所述行進方向的實時角度α₁；

S4，濾波單元接收α₁，將α₁進行卡爾曼濾波處理得到α₂；

S5，選擇糾偏模式；

S6，主控芯片中的存儲模塊記錄所述機器人主體運轉狀態，進入S7；

S7，所述主控芯片接收所述α₂，擬合所述機器人主體實時姿態，判斷所述機器人主體是否處于水平位置，是，結束糾偏，否，進入S8；

S8：所述主控芯片接收到遙控器沿y軸行進的信號，進入步驟S9-S15；所述主控芯片接收到遙控器沿x軸行進的信號，進入步驟S16-S22；

S9，判斷所述機器人主體是否水平左傾，是，進入步驟S10，否，進入步驟S13；

S10，判斷機器人是否處于上行狀態，是，進入步驟S11，否，進入步驟S12；

S11，所述主控芯片發送向上右校準指令，控制輪子與電機進行糾偏回歸零點，返回步驟S3；

S12，所述主控芯片發送向下右校準指令，控制輪子與電機進行糾偏回歸零點，返回步驟S3；

S13，判斷機器人是否處于上行狀態，是，進入步驟S14，否，進入步驟S15；

S14，所述主控芯片發送向上左校準指令，控制輪子與電機進行糾偏回歸零點，返回步驟S3；

S15，所述主控芯片發送向下左校準指令，控制輪子與電機進行糾偏回歸零點，返回步驟S3；

S16，判斷所述機器人主體是否水平左傾，是，進入步驟S17，否，進入步驟S20；

S17，判斷機器人是否處于上行狀態，是，進入步驟S18，否，進入步驟S19；

S18，所述主控芯片發送向下右校準指令，控制輪子與電機進行糾偏回歸零點，返回步驟S3；

S19，所述主控芯片發送向上右校準指令，控制輪子與電機進行糾偏回歸零點，返回步驟S3；

S20，判斷機器人是否處于上行狀態，是，進入步驟S21，否，進入步驟S22；

S21，所述主控芯片發送向上左校準指令，控制輪子與電機進行糾偏回歸零點，返回步驟S3；

S22，所述主控芯片發送向下左校準指令，控制輪子與電機進行糾偏回歸零點，返回步驟S3。