一種運動控制方法及裝置與流程
2023-05-17 07:13:11 1
本發明涉及伺服系統運動控制技術領域，尤其涉及一種運動控制方法及裝置。
背景技術：
在高精度伺服系統中，光柵傳感器常用於採集待測目標的位置信息。按運動形式分，光柵傳感器可分為直線型和旋轉型。其中，直線型光柵傳感器主要由光柵尺和讀數頭構成，旋轉型光柵傳感器主要由碼盤和讀數頭構成。在實際使用光柵傳感器時，用戶只需按照固定的時鐘頻率向讀數頭髮送脈衝信號，讀數頭就會將封裝好的數據返回給用戶。但是，如果用戶想要得到待測目標的角度信息，則還需要對讀數頭的數據進行採集。
在現有技術中，運動控制器可以針對光柵進行數據採集、並進行高精度的伺服控制。但是，現有的運動控制器價格昂貴，即使便宜的也高達幾萬元。另外，現有的運控控制器因其複雜的結構和複雜的控制算法，基本上很難用於對實時性要求很高的系統。
針對現有的運動控制器存在的結構複雜、實時性差、成本高的缺陷，亟需一種通用性強、結構簡單、成本低、實時性高，能適用於多種絕對式光柵和增量式光柵的運動控制方法和裝置。
技術實現要素：
本發明的目的在於提出一種通用性強、實時性高、精度高的運動控制方法及裝置。
一方面，本發明提供了一種運動控制方法，所述方法包括：
S1、第一信號轉換器將外部編碼器採集的差分數據信號轉換為單端數據信號，並將所述單端數據信號發送至FPGA解碼器；
S2、FPGA解碼器對所述單端數據信號進行解碼，並將解碼得到的數據信號發送至ARM控制器；
S3、ARM控制器通過主定時器模塊讀取所述解碼得到的數據信號以及上位機發送的控制參數，並根據預設的複合控制算法獲取控制輸出量，並將所述控制輸出量輸出至DA轉換器；所述複合控制算法由PID閉環控制、前饋控制構成；
S4、DA轉換器將所述控制輸出量由數字量形式轉變為模擬量形式，然後將其輸出至電機驅動器；
其中，所述ARM控制器裝載有UCOSII作業系統。
優選的，在步驟S3中，所述ARM控制器通過主定時器模塊讀取所述解碼得到的數據信號以及上位機發送的控制參數，具體包括：
讀取上位機發送的控制參數，並判斷ARM控制器中的控制參數是否改變；若控制參數改變，則對ARM控制器重新進行初始化；若控制參數未改變或者在重新初始化之後，讀取FPGA解碼器解碼得到的數據信號、並將其轉換為角度數據。
優選的，在步驟S3中，所述主定時器模塊根據預設的複合控制算法獲取控制輸出量，具體包括：
根據所述角度數據獲取位置反饋數據，並對所述位置反饋數據進行一階濾波獲取速度反饋數據；
根據電機運行模式調用相應的軌跡規劃函數，計算位置期望數據；其中，所述軌跡規劃函數為梯形規劃函數或三角形規劃函數；
根據所述位置反饋數據、位置期望數據計算位置環偏差，並將所述位置環偏差輸入PID位置閉環，以獲取位置環PID控制量；
對所述位置環PID控制量進行二階濾波，獲取速度期望數據；
根據所述速度反饋數據、速度期望數據計算速度環偏差，並將所述速度環偏差輸入PID速度閉環，以獲取速度環PID控制量。
優選的，所述一階濾波具體為：
式中，Δv為速度反饋數據，s1為當前時刻的角度增量，s2為與s1相鄰的前一時刻的角度增量、s3為與s2相鄰的前一時刻的角度增量、s4為與s3相鄰的前一時刻的角度增量。
優選的，所述根據所述角度數據獲取位置反饋數據，具體包括：
判斷當前採集的角度數據是否正確；若當前採集的角度數據正確，則將錯誤計數清零，並將當前採集的角度數據作為當前的位置反饋數據；若當前採集的角度數據錯誤，則將錯誤計數加1，並且，在連續出錯次數小於100次時，用前一時刻的位置反饋數據作為當前的位置反饋數據。
優選的，在當前採集的角度數據錯誤時，所述方法還包括：若連續出錯次數大於等於100次，則令ARM控制器進入緊急停車模式，以使控制輸出量為0。
優選的，在獲取速度反饋數據之後，且在計算位置期望數據之前，所述方法還包括：根據所述速度反饋數據判斷電機是否超速；若電機未超速，則將超速計數清零；若電機超速，則將超速計數加1；並且，在連續超速次數大於100次時，令ARM控制器進入緊急停車模式，以使控制輸出量為0。
優選的，在根據預設的複合控制算法獲取控制輸出量之後，且在將所述控制輸出量輸出至DA轉換器之前，所述方法還包括：
判斷所述控制輸出量是否超出預設的輸出極限值；若所述控制輸出量超出所述輸出極限值，則將所述輸出極限值輸出至DA轉換器；若所述控制輸出量未超出所述輸出極限值，則將控制輸出量輸出至DA轉換器。
優選的，所述方法還包括：所述ARM控制器通過網絡通信模塊接收上位機發送的控制參數，以及向上位機返回ARM控制器的狀態信息。
另一方面，本發明還提供了一種運動控制裝置，所述裝置包括：第一信號轉換器、FGPA解碼器、裝載有UCOSII作業系統的ARM控制器、DA轉換器；所述ARM控制器包括：主定時器模塊、網絡通信模塊；
第一信號轉換器用於將外部編碼器採集的差分數據信號轉換為單端數據信號，並將所述單端數據信號發送至FPGA解碼器；
FPGA解碼器用於對所述單端數據信號進行解碼，並將解碼得到的數據信號發送至ARM控制器；
ARM控制器用於通過所述網絡通信模塊接收上位機發送的控制參數，以及向上位機返回ARM控制器的狀態信息；ARM控制器還用於通過主定時器模塊讀取所述解碼得到的數據信號以及上位機發送的控制參數，並根據預設的複合控制算法獲取控制輸出量，並將所述控制輸出量輸出至DA轉換器；所述複合控制算法由PID閉環控制、前饋控制構成；
DA轉換器用於將所述控制輸出量由數字量形式轉變為模擬量形式，然後將其輸出至電機驅動器；
其中，所述ARM控制器為STM32晶片。
從以上技術方案可以看出，本發明的運動控制方法主要包括以下步驟：第一信號轉換器將外部編碼器採集的差分數據信號轉換為單端數據信號，並將單端數據信號發送至FPGA解碼器；FPGA解碼器對單端數據信號進行解碼，並將解碼得到的數據信號發送至裝載UCOSII作業系統的ARM控制器；ARM控制器通過主定時器模塊讀取所述解碼得到的數據信號以及上位機發送的控制參數，並根據預設的複合控制算法獲取控制輸出量，並將控制輸出量輸出至DA轉換器；DA轉換器對控制輸出量進行數模轉換，然後將其輸出至電機驅動器。在本發明的運動控制方法中，通過採用差分輸入方式採集編碼器信號、並通過第一信號轉換器將差分數據信號轉換為單端數據信號，提高了控制方法的通用性；通過FPGA解碼器進行數據解碼、並通過載有實時作業系統的ARM控制器進行閉環控制，提高了控制方法的實時性。進一步的，本發明通過採用由PID閉環控制、前饋控制構成的複合控制算法，提高了控制方法的精度和動態響應性。
附圖說明
通過以下參照附圖而提供的具體實施方式部分，本發明的特徵和優點將變得更加容易理解，在附圖中：
圖1是示出的本發明實施例的運動控制方法的流程示意圖；
圖2是圖1中讀取解碼得到的數據信號以及控制參數的優選方式；
圖3是圖1中根據複合控制算法獲取控制輸出量的優選方式；
圖4是圖3中根據角度數據獲取位置反饋數據的優選方式；
圖5是根據圖3中的速度反饋數據判斷電機是否超速的步驟示意圖；
圖6是示出的本發明實施例的運動控制裝置的結構組成示意圖；
圖7是圖6所示的ARM控制器的結構組成示意圖；
1、外部編碼器；2、第一信號轉換器；3、FPGA解碼器；4、ARM控制器；5、DA轉換器；6、上位機；7、電機驅動器；8、IO輸入輸出；9、LCD顯示器；10、第二信號轉換器；11、總線；401、網絡通信模塊；402、主定時器模塊；403、初始化模塊。
具體實施方式
下面參照附圖對本發明的示例性實施方式進行詳細描述。對示例性實施方式的描述僅僅是出於示範目的，而絕不是對本發明及其應用或用法的限制。
現有的運動控制器普遍存在以下三個問題：一是由於技術壟斷、生產成本高，導致運動控制器的購買價格昂貴；二是由於控制器的結構設計、控制算法複雜，用戶很難進行二次開發、修改；三是由於結構、控制算法複雜，導致控制器很難用於對實時性要求很高的系統，即不能對控制器的高精度與高實時性進行兼顧。
針對現有的運動控制器的缺陷，本發明提供了一種運動控制方法與裝置，以提高運動控制方法與運動控制裝置的通用性、實時性、控制精度，同時降低運動控制的成本。
下面結合附圖1-5對本發明實施例中的運動控制方法進行詳細說明。圖1示出了本發明實施例的運動控制方法的流程示意圖。從圖1可見，該運動控制方法主要包括步驟S1～S4：
步驟S1、第一信號轉換器將外部編碼器採集的差分數據信號轉換為單端數據信號，並將所述單端數據信號發送至FPGA(現場可編程門陣列)解碼器。
在該步驟中，通過設置第一信號轉換器進行信號變換，便於採用差分輸入方式進行數據採集，極大地提高了運動控制方法的通用性。
步驟S2、FPGA解碼器對所述單端數據信號進行解碼，並將解碼得到的數據信號發送至ARM控制器。
在一個優選實施例中，FPGA解碼器在對單端數據信號進行解碼之後，將解碼後的數據信號分為兩路：一路數據信號發送至ARM控制器，另一路數據信號經第二信號轉換器轉變為差分信號形式後輸出至外部設備。這樣一來，既滿足了運動控制的需求，又滿足了將光柵信息傳到外部設備的需求，極大提高了運動控制方法的通用性。
步驟S3、ARM控制器通過主定時器模塊讀取所述解碼得到的數據信號以及上位機發送的控制參數，並根據預設的複合控制算法獲取控制輸出量，並將所述控制輸出量輸出至DA轉換器。其中，所述ARM控制器裝載有UCOSII作業系統。
在步驟S3中，所述ARM控制器讀取所述解碼得到的數據信號以及上位機發送的控制參數存在多種實施方式。圖2示出了一種讀取解碼得到的數據信號以及控制參數的優選方式。由圖2可見，該優選方式具體包括：首先，讀取上位機發送的控制參數；然後，判斷ARM控制器中的控制參數是否改變；若控制參數改變，則對ARM控制器重新進行初始化；若控制參數未改變或者在重新初始化之後，讀取FPGA解碼器解碼得到的數據信號、並將其轉換為角度數據。在具體實施時，ARM控制器可通過網絡通信模塊接收上位機發送的控制參數，以及通過網絡通信模塊向上位機返回ARM控制器的狀態信息。
在圖2所示的優選方式中，通過在進行運動控制之前判斷ARM控制器中的控制參數是否改變，極大提高了運動控制方法的可靠性。
另外，在步驟S3中，所述主定時器模塊根據預設的複合控制算法獲取控制輸出量也存在多種實施方式。圖3示出了一種根據複合控制算法獲取控制輸出量的優選方式。從圖3可見，該優選方式具體包括：步驟S321～步驟S325：
S321、根據所述角度數據獲取位置反饋數據，並對所述位置反饋數據進行一階濾波獲取速度反饋數據。
在步驟S21中，根據所述角度數據獲取位置反饋數據存在多種實施方式。圖4示出了一種根據角度數據獲取位置反饋數據的優選方式。從圖4可見，該優選方式具體包括：判斷當前採集的角度數據是否正確；若當前採集的角度數據正確，則將錯誤計數清零，並將當前採集的角度數據作為當前的位置反饋數據；若當前採集的角度數據錯誤，則將錯誤計數加1，並且，在連續出錯次數小於100次時，用前一時刻的位置反饋數據作為當前的位置反饋數據。進一步的，若連續出錯次數大於等於100次，則令ARM控制器進入緊急停車模式，以使控制輸出量為0。在圖4所示的優選方式中，通過對當前採集的角度數據進行判斷，並在連續出錯多次時使ARM控制器進入緊急停車模式，極大提高了運動控制的可靠性。
進一步的，在一個優選實施例中，所述一階濾波具體為：
式中，Δv為速度反饋數據，s1為當前時刻的角度增量，s2為與s1相鄰的前一時刻的角度增量、s3為與s2相鄰的前一時刻的角度增量、s4為與s3相鄰的前一時刻的角度增量。與現有的一階濾波方法相比，該一階濾波方法不僅簡單高效、而且極大提高了運動控制方法的精度。
S322、根據電機運行模式調用相應的軌跡規劃函數，計算位置期望數據，其中，所述軌跡規劃函數為梯形規劃函數或三角形規劃函數。
在一個優選實施例中，在步驟S321獲取速度反饋數據之後，且在步驟S322計算位置期望數據之前，所述方法還包括圖5所示的步驟。由圖5可見，該步驟具體為：根據所述速度反饋數據判斷電機是否超速；若電機未超速，則將超速計數清零；若電機超速，則將超速計數加1；並且，在連續超速次數大於100次時，令ARM控制器進入緊急停車模式，以使控制輸出量為0。在圖5所示的步驟中，通過判斷電機是否超速、並在連續超速多次時使ARM控制器進入緊急停車模式，極大提高了運動控制的可靠性、安全性。
S323、根據所述位置反饋數據、位置期望數據計算位置環偏差，並將所述位置環偏差輸入PID位置閉環，以獲取位置環PID控制量。
S324、對所述位置環PID控制量進行二階濾波，獲取速度期望數據。
S325、根據所述速度反饋數據、速度期望數據計算速度環偏差，並將所述速度環偏差、前饋控制輸出值輸入PID速度閉環，以獲取速度環PID控制量。
進一步的，在一個優選實施例中，在根據圖3所示的優選方式獲取控制輸出量以後，且在將所述控制輸出量輸出至DA轉換器之前，該運動控制方法還包括：判斷所述控制輸出量是否超出預設的輸出極限值；若所述控制輸出量超出所述輸出極限值，則將所述輸出極限值輸出至DA轉換器；若所述控制輸出量未超出所述輸出極限值，則將控制輸出量輸出至DA轉換器。通過以上步驟，能夠有效防止由於控制輸出量過大引起電機損壞的故障出現。
步驟S4、DA轉換器將所述控制輸出量由數字量形式轉變為模擬量形式，然後將其輸出至電機驅動器。
在本發明實施例的運動控制方法中，通過採用PID位置閉環、PID速度閉環，並採用路徑規划算法，極大提高了運動控制的精度。進一步的，通過在速度環的輸入中加入前饋控制，極大地提高了系統的動態性能。進一步的，通過在ARM控制器中裝載實時作業系統UCOSII，極大提高了運動控制的實時性。另外，通過設置第一信號轉換器，便於以差分方式進行數據採集，極大提高了運動控制的通用性。
下面結合圖6和圖7對本發明實施例的運動控制裝置進行詳細說明。圖6示出了本發明實施例的運動控制裝置的結構示意圖。圖7示出了本發明實施例中的ARM控制器的結構示意圖。從圖6可見，該運動控制裝置主要包括：第一信號轉換器2、FGPA解碼器3、裝載有UCOSII作業系統的ARM控制器4、DA轉換器5。其中，ARM控制器4主要包括：主定時器模塊402、網絡通信模塊401。另外，從圖6可見，該運動控制裝置還包括：IO輸入輸出8、LCD顯示器9、總線11。其中，IO輸入輸出8包括：IO輸入接口、IO輸出接口、以及用於隔離IO輸入接口與IO輸出接口的光耦。LCD顯示器9與ARM控制器4相連，用於顯示ARM控制器的狀態信息。總線11用於運動控制裝置中多個部件的連接，比如ARM控制器與DA轉換器，ARM控制器與IO輸入輸出8，等等。
第一信號轉換器2用於將外部編碼器1採集的差分數據信號轉換為單端數據信號，並將所述單端數據信號發送至FPGA解碼器3。在具體實施時，第一信號轉換器可優先選用DS26LS31晶片。在本發明實施例中，通過設置第一信號轉換器，便於以差分方式對外部編碼器的數據進行採集，極大提高了運動控制系統的通用性。
FPGA解碼器3用於對所述單端數據信號進行解碼，並將解碼得到的數據信號通過並口發送至ARM控制器4。在具體實施時，FPGA解碼器可優先選用EP2C5T144C8晶片，ARM控制器優先選取STM32晶片。
ARM控制器4用於通過網絡通信模塊401接收上位機發送的控制參數，以及向上位機返回ARM控制器的狀態信息。ARM控制器4用於通過主定時器模塊402讀取所述解碼得到的數據信號以及上位機發送的控制參數，並根據預設的複合控制算法獲取控制輸出量，並將所述控制輸出量輸出至DA轉換器5。其中，所述複合控制算法由PID閉環控制、前饋控制構成。另外，從圖6可見，ARM控制器4還包括：初始化模塊403。初始化模塊403用於對運動控制裝置進行初始化，如對實時作業系統UCOSII進行初始化，對FPGA解碼器進行初始化等等。在本發明實施例中，通過在ARM控制器中裝載UCOSII實時作業系統，並在此基礎上直接利用ARM控制器的32位的高精度定時器，以完成控制算法的周期性調用，從而極大提高了運動控制的實時性。
DA轉換器5用於將所述控制輸出量由數字量形式轉變為模擬量形式，然後將其輸出至電機驅動器7。
在一個優選實施例中，運動控制裝置還包括：第二信號轉換器10。在該優選實施例中，FPGA解碼器3在對單端數據信號進行解碼之後，將解碼後的數據信號分為兩路：一路數據信號發送至ARM控制器4，另一路數據信號經第二信號轉換器10轉變為差分信號形式後輸出至外部設備。這樣一來，既滿足了運動控制的需求，又滿足了將光柵信息傳到外部設備的需求，極大提高了運動控制裝置的通用性。
在本發明實施例的運動控制裝置中，通過設置第一信號轉換器，便於以差分方式進行數據採集，極大提高了運動控制的通用性。通過在ARM控制器中裝載實時作業系統UCOSII，極大提高了運動控制的實時性。另外，通過將本發明的運動控制方法應用到運動控制裝置中，極大提高了運動控制的精度以及系統的動態性能。
雖然參照示例性實施方式對本發明進行了描述，但是應當理解，本發明並不局限於文中詳細描述和示出的具體實施方式，在不偏離權利要求書所限定的範圍的情況下，本領域技術人員可以對所述示例性實施方式做出各種改變。