[發明專利]一種用于軸運動的軟限位控制方法及基于RTX64的開放式運動控制器

有效

• 申請號: 202110658647.3
• 申請日: 2021-06-15
• 公開（公告）號: CN113359620B
• 公開（公告）日: 2022-03-15
• 發明（設計）人: 東梁;鄺應煒;董志劼;魯吉志;李松;蔡四雄;向龍;韋傲峰
• 申請（專利權）人: 深圳市旗眾智能科技有限公司
• 主分類號: G05B19/416
• 分類號: G05B19/416
• 代理公司: 深圳市中知專利商標代理有限公司 44101
• 代理人: 張學群;顧楠楠

1.一種用于軸運動的軟限位控制方法，其特征在于：包括如下步驟：

步驟S1、設置運動參數值，運動參數值包括：正限位LimitP、負限位LimitN；起始速度f_s、結束速度f_e、目標速度V_target；最大加速度A、最大減速度D；加速階段給定的第一加加速度J_acc、減速階段給定的第二加加速度J_dec；

步驟S2、下發開始運動以及運動方向指令；

步驟S3、基于S型算法，當進行加速階段時，重新計算實際的加速階段下的加加速度值J′_acc和最大加速度A′，并計算加速階段中減加速段所需周期n₃；

加加速段周期n₁與減加速段周期n₃的關系式為：n₃＝n₁-1；

到達最大加速度的所需的加加速段周期n₁為：

經加加速段和減加速段后速度的增加量為：Δv＝n₁×A；

從起始速度f_s到目標速度V_target允許的最大速度增量為Δv_allow＝V_target-f_s，若Δv_allow＞Δv，則說明速度增量在允許范圍內，可以達到最大加速度，否則加速度達不到設定的最大加速度，重新計算加加速度；

步驟S4、當進入減速階段時，重新計算實際的減速階段的參數，包括減速階段的第二加加速度J′_dec和最大減速度D′，并計算加減速段周期n₅，勻減速段周期n₆，減減速段周期n₇；

步驟S5、計算減速階段所需的減速距離L′_dec以及經加速階段中的減加速段后達到的最大速度V_max；

步驟S6、計算軸運動到達的當前位置值CurPos距離軟件限位位置的距離LimitLen；

步驟S7、判斷減速階段運行的總距離L′_dec是否小于或等于步驟S6計算得到的距離LimitLen，當L′_dec≤LimitLen時，進入步驟S16，否則進入步驟S8；

步驟S8、判斷步驟S5計算的V_max是否小于目標速度V_target，當V_max＜V_target，繼續當前段的運動，并進入步驟S9，否則進入步驟S10進入減加速段的運動；

步驟S9、比較當前加速度A_cur和重新計算的允許的最大加速度A'，并根據A_cur和A'的大小繼續加加速段運動或進入勻加速段運動，并在執行結束后返回步驟S3，進入下一周期循環；

步驟S10、進入減加速段的運動；

步驟S11、在減加速段的運動過程中，通過判斷減加速階段周期n₃的大小，執行減加速段運動或減減速段運動；當n₃＞0時，進入步驟S12，進行減加速段運動，當n₃≤0時，進入步驟S13，進行勻速段運動；

步驟S12、當n₃＞0，進入減加速段運動，并計算當前周期加速度A_cur＝A_cur-J'_dec、當前周期速度V_cur＝V_cur+A_cur、當前周期的運動位置值CurPos＝CurPos+V_cur、n₃＝n₃-1，然后返回步驟S11繼續判斷n₃的大小；

步驟S13、進入勻速段運動；

步驟S14，在勻速階段的運動過程中，計算每個周期當前位置值CurPos距離軟件的限位位置的距離LimitLen，然后通過判斷減速距離L'_dec與距離LimitLen的大小，確認繼續執行勻速階段運動或進入加減速段運動；

當L'_dec＞LimitLen，進入步驟S15繼續勻速階段運動，計算當前周期的運動位置值CurPos；然后進入下一周期循環，繼續判斷減速距離L'_dec與距離LimitLen的大小；

CurPos＝CurPos+V_cur

當L'_dec≤LimitLen，進入步驟S16，進入加減速段運動，并在加減速段運動中判斷加減速段周期n₅是否大于0；

當n₅＞0時，進入步驟S17，繼續加減速段運動，并在每個循環周期計算A_cur＝A_cur-J'_dec，V_cur＝V_cur+A_cur，n₅＝n₅-1，返回步驟S16繼續判斷；

當n₅≤0時，進入步驟S18，進入勻減速段運動；

步驟S18，在勻減速段運動中判斷勻減速段周期n₆是否大于0；

當n₆＞0時，進入步驟S20，繼續勻減速段運動，并在每個循環周期計算A_cur＝A_cur，V_cur＝V_cur+A_cur，n₆＝n₆-1的值，返回步驟S18繼續判斷；

當n₆≤0時，進入步驟S20，進入減減速段運動，在減減速段運動中判斷減減速段周期n₇是否大于0，當n₇＞0時，進入步驟S21，繼續減減速段運動，在每個循環周期計算A_cur＝A_cur+J'_dec，V_cur＝V_cur+A_cur，n₇＝n₇-1，并返回步驟S20繼續判斷；

當n₇≤0時，停止當前運動規劃軸的運動。