[發明專利]于弧形路徑中控制端效器追蹤對象的方法

在審

• 申請號: 202010745423.1
• 申請日: 2020-07-29
• 公開（公告）號: CN114063557A
• 公開（公告）日: 2022-02-18
• 發明（設計）人: 袁偉翔
• 申請（專利權）人: 創博股份有限公司
• 主分類號: G05B19/19
• 分類號: G05B19/19;B65G47/24;B65G47/90
• 代理公司: 北京匯澤知識產權代理有限公司 11228
• 代理人: 關宇辰

1.一種于弧形路徑中控制端效器追蹤對象的方法，運用于具有一輸送帶及一端效器的一自動化作業系統，該輸送帶用以輸送一對象并具有一弧形路徑，其特征在于，該方法包括以下步驟：

步驟a：取得進入該弧形路徑的該對象的一對象進入姿態及一對象進入位置；

步驟b：取得一對象移動速度及一對象旋轉速度；

步驟c：規畫自一端效器起始位置到該對象進入位置的一直線軌跡；

步驟d：依據該對象移動速度計算用以使該端效器追上該對象的一移動速度曲線及用于該直線軌跡的一位置補償量；

步驟e：依據該對象進入姿態及一端效器起始姿態規畫一姿態插補軌跡；

步驟f：依據該對象旋轉速度計算用以使該端效器旋轉至與該對象的姿態對應的一旋轉速度曲線及用于該姿態插補軌跡的一姿態補償量；

步驟g：依據該直線軌跡、該移動速度曲線、該位置補償量、該姿態插補軌跡及該姿態補償量產生一控制命令；及

步驟h：執行該控制命令來控制該端效器朝該對象移動并旋轉姿態，其中于該端效器追上該對象后，該端效器與該對象的移動速度相同，且該端效器與該對象的姿態對應變化。

2.如權利要求1所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，該自動化作業系統還包括一觸發裝置，該方法于該步驟a之前還包括一步驟i：于通過該觸發裝置偵測到該對象進入該弧形路徑時，執行該步驟a至該步驟h。

3.如權利要求1所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，該對象的姿態是以一坐標系表示，步驟b是計算該輸送帶的該弧形路徑的線速度作為該坐標系的該對象移動速度，依據該對象移動速度計算一對象移動角度，并依據該對象移動角度計算該坐標系的該對象旋轉速度。

4.如權利要求1所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，還包括一步驟j ：于該端效器追上該對象后，控制該端效器對該對象執行一自動化作業。

5.如權利要求4所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，該自動化作業包括對該對象執行抓取、放置、加工或檢測的至少其中之一操作。

6.如權利要求1所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，還包括一步驟k：于偵測到該對象移動速度改變時，再次執行該步驟a至該步驟h。

7.如權利要求1所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，該步驟d包括以下步驟：

步驟d1：依據該對象移動速度計算該移動速度曲線，其中該移動速度曲線是用以指示該端效器的一端效器移動速度的變化；及

步驟d2：基于該移動速度曲線計算該端效器于各周期的一弧形移動距離及一弧形移動角度，依據各該弧形移動距離及各該弧形移動角度執行一弧形插補以獲得各該周期的一弧形補償位置，并依據該對象進入位置及各該周期的該弧形補償位置計算各該周期的該位置補償量。

8.如權利要求7所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，該步驟g包括一步驟g1：依據該端效器于該直線軌跡的各該周期的直線偏移量及各該周期的該位置補償量計算該端效器于各該周期的一端效器位置，并依據各該周期的該端效器位置計算該端效器于各該周期的一移動控制命令。

9.如權利要求1所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，該步驟f包括：

步驟f1：依據該對象旋轉速度計算該旋轉速度曲線；及

步驟f2：依據該旋轉速度曲線計算各周期的一對象旋轉角度，依據該對象進入姿態與各該周期的該對象旋轉角度執行一四元素插補以獲得該對象于各該周期的一四元素旋轉坐標作為各該周期的該姿態補償量。

10.如權利要求9所述的于弧形路徑中控制端效器追蹤對象的方法，其特征在于，該步驟g包括一步驟g2：依據各該周期的該四元素旋轉坐標執行一姿態融合以獲得該對象于各該周期的一旋轉矩陣，并依據各該旋轉矩陣產生各該周期的一旋轉控制命令。