[發明專利]一種通過插入運動段實現機器人平滑運動的控制方法

有效

• 申請號: 201610994055.8
• 申請日: 2016-11-11
• 公開（公告）號: CN108073162B
• 公開（公告）日: 2021-05-04
• 發明（設計）人: 劉蔭忠;孫維堂;董世斌;韓衛光
• 申請（專利權）人: 中國科學院沈陽計算技術研究所有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 沈陽科苑專利商標代理有限公司 21002
• 代理人: 李巨智

1.一種通過插入運動段實現機器人平滑運動的控制方法，其特征在于，包括以下步驟：

步驟1：判斷機器人實際位置是否偏離理論軌跡，如果是，則根據實際位置及當前規劃的命令位置插入運動段；否則，按照原軌跡正常規劃；

步驟2：按照插入運動段后的軌跡進行規劃；

所述判斷機器人實際位置是否偏離理論軌跡包括以下過程：

步驟11：采集各方向實際位置，并計算實際位置與命令位置之間的實際軌跡方向矢量；

步驟12：根據實際軌跡方向矢量與理論軌跡方向矢量，計算兩者之間夾角；

步驟13：如果實際軌跡方向矢量與理論軌跡方向矢量的夾角小于系統參數配置所確定的允許角度，則正常進行軌跡規劃；否則計算實際位置與命令位置之間的實際軌跡方向矢量的模的大小，并繼續判斷實際位置與命令位置之間的實際軌跡方向矢量的模的大小是否大于系統參數配置所確定的允許長度；

步驟14：如果實際位置與命令位置之間的實際軌跡方向矢量的模的大小大于系統參數配置所確定的允許長度，則根據實際位置及當前規劃的命令位置插入運動段；否則，按照原軌跡正常規劃；

所述實際位置與命令位置之間的實際軌跡方向矢量為：

其中，設命令位置坐標為P(x,y)，實際位置坐標為P'(x',y')；

所述實際軌跡方向矢量與理論軌跡方向矢量的夾角包括以下計算過程：

設運動段的起點坐標為O(x0,y0)，則理論軌跡方向矢量為：

矢量的單位化：

矢量的夾角θ為：

θ＝acos(d)

其中，插入運動段前理論軌跡的命令位置P(x,y)，插入運動段前理論軌跡的實際位置P'(x',y')，為矢量的模，為矢量的模，d為矢量單位化矢量與的數量積，θ為實際軌跡方向矢量與理論軌跡方向矢量間的夾角，acos(d)為計算d的反余弦函數；

所述根據實際位置及當前規劃的命令位置插入運動段包括以下過程：修改插入運動段前的理論軌跡信息，然后初始化所插入的運動段信息，形成插入運動段后的理論軌跡；

所述修改插入運動段前的理論軌跡信息，然后初始化所插入的運動段信息，形成插入運動段后的理論軌跡；

插入運動段前的理論軌跡的修改包括：將插入運動段前的理論軌跡的起點修改為當前的規劃的命令位置；將規劃速度修改為零；將規劃長度修改為從起點到目標點的距離；將已規劃長度修改為零；

所述初始化所插入的運動段信息包括：

設置插入運動段的起點位置為當前反饋位置；設置目標位置為插入運動段前的目標位置；設置當前規劃速度、加速度為插入運動段前的規劃速度、加速度；設置規劃長度為從起點到目標點的距離；設置已規劃長度為零。