[發(fā)明專利]一種六軸機(jī)器人空間運(yùn)動(dòng)規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201811573810.0
• 申請(qǐng)日: 2018-12-21
• 公開（公告）號(hào): CN109648557B
• 公開（公告）日: 2021-11-02
• 發(fā)明（設(shè)計(jì)）人: 梁泓;陳浩然;王永和;李飛燕
• 申請(qǐng)（專利權(quán)）人: 上海信耀電子有限公司
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 上海申新律師事務(wù)所 31272
• 代理人: 俞滌炯

1.一種機(jī)器人空間運(yùn)動(dòng)規(guī)劃方法，適用于六軸機(jī)器人，其特征在于，所述六軸機(jī)器人的機(jī)器人控制器連接一示教器，于所述示教器中預(yù)先設(shè)定了多個(gè)空間示教點(diǎn)以及各個(gè)所述空間示教點(diǎn)所對(duì)應(yīng)的運(yùn)動(dòng)命令，還包括以下步驟：

步驟S1：所述機(jī)器人控制器讀取所述示教器中的所述空間示教點(diǎn)的坐標(biāo)與對(duì)應(yīng)的運(yùn)動(dòng)命令；

步驟S2：所述機(jī)器人控制器根據(jù)所述運(yùn)動(dòng)命令中的參數(shù)，計(jì)算出對(duì)應(yīng)的所述空間示教點(diǎn)之間的空間運(yùn)動(dòng)路徑；

步驟S3，所述機(jī)器人控制器根據(jù)各個(gè)所述空間示教點(diǎn)以及對(duì)應(yīng)的所述空間運(yùn)動(dòng)路徑計(jì)算得到各個(gè)所述空間運(yùn)動(dòng)路徑所對(duì)應(yīng)的示教中間點(diǎn)；

步驟S4：所述機(jī)器人控制器計(jì)算各個(gè)所述空間運(yùn)動(dòng)路徑在所述示教中間點(diǎn)處的空間切線方向的夾角及空間姿態(tài)矩陣，隨后計(jì)算出相鄰的所述空間運(yùn)動(dòng)路徑在所述示教中間點(diǎn)處的空間切線方向的夾角之間的第一角度差和空間姿態(tài)角之間的第二角度差；

步驟S5：分別將所述第一角度差和所述第二角度差與預(yù)設(shè)定的閾值進(jìn)行比較，并根據(jù)比較結(jié)果計(jì)算出經(jīng)過所述示教中間點(diǎn)的速度和加速度；

步驟S6：將所述運(yùn)動(dòng)運(yùn)命令中的參數(shù)及所述示教中間點(diǎn)的速度和加速度作為輸入?yún)?shù)輸入到所述機(jī)器人控制器中，以采用預(yù)設(shè)的運(yùn)動(dòng)規(guī)劃函數(shù)對(duì)所述六軸機(jī)器人的運(yùn)動(dòng)進(jìn)行規(guī)劃，并得到所述六軸機(jī)器人的規(guī)劃運(yùn)動(dòng)參數(shù)；

步驟S7：所述機(jī)器人控制器根據(jù)所述規(guī)劃運(yùn)動(dòng)參數(shù)形成的機(jī)器人運(yùn)動(dòng)軌跡，控制所述六軸機(jī)器人在經(jīng)過所述示教中間點(diǎn)的示教路徑上進(jìn)行空間運(yùn)動(dòng)；

所述示教中間點(diǎn)的速度和加速度計(jì)算方法包括以下步驟：

步驟S51：判斷所述第一角度差或第二角度差是否超出預(yù)設(shè)定閾值；

若是則轉(zhuǎn)入步驟S52；

若否則轉(zhuǎn)入步驟S53；

步驟S52：將所述示教中間點(diǎn)設(shè)為終止點(diǎn)，并將所述六軸機(jī)器人的末端經(jīng)過所述終止的速度與加速度設(shè)定為零，隨后轉(zhuǎn)到步驟S6；

步驟S53：將所述示教中間點(diǎn)設(shè)為過程點(diǎn)，根據(jù)所述示教中間點(diǎn)處的空間切線方向的夾角及所述角度差所對(duì)應(yīng)的比例系數(shù)和前后兩段所述空間運(yùn)動(dòng)路徑的額定速度，通過預(yù)設(shè)的計(jì)算公式進(jìn)行計(jì)算，得到經(jīng)過該點(diǎn)的速度和加速度，隨后轉(zhuǎn)到步驟S6；

所述預(yù)設(shè)的計(jì)算公式具體為：

經(jīng)過所述示教中間點(diǎn)的速度：

經(jīng)過所述示教中間點(diǎn)的加速度：

其中θ為所述第一角度差，β所述第二角度差，θ_th、β_th為對(duì)應(yīng)的所述限定閾值，v_p、v_n，為所述中間示教點(diǎn)前后的路徑期望速度，a_p、a_n為所述中間示教點(diǎn)前后的路徑期望加速度。

2.如權(quán)利要求1所述的機(jī)器人空間運(yùn)動(dòng)規(guī)劃方法，其特征在于，所述運(yùn)動(dòng)命令中的參數(shù)包括所述空間運(yùn)動(dòng)路徑的路徑長(zhǎng)度、所述六軸機(jī)器人依據(jù)所述空間運(yùn)動(dòng)路徑進(jìn)行運(yùn)動(dòng)的額定速度和所述六軸機(jī)器人的末端姿態(tài)。

3.如權(quán)利要求1所述的機(jī)器人空間運(yùn)動(dòng)規(guī)劃方法，其特征在于，所述步驟S3還包括以下步驟：

步驟S31：判斷所述空間運(yùn)動(dòng)路徑是否是由圓弧過渡連接的直線路徑；

若是則轉(zhuǎn)到步驟S32；

若否則轉(zhuǎn)到步驟S33；

步驟S32：求出所述空間運(yùn)動(dòng)路徑的中段點(diǎn)的坐標(biāo)和姿態(tài)，作為所述示教中間點(diǎn)，隨后轉(zhuǎn)向所述步驟S4；

步驟S33：判斷所述示教中間點(diǎn)是否都在所述空間運(yùn)動(dòng)路徑上；

若是，則直接記錄所述示教中間點(diǎn)，隨后轉(zhuǎn)向所述步驟S4；

若否則轉(zhuǎn)到步驟S34；

步驟S34：根據(jù)等比例劃分原則，求出所述示教中間點(diǎn)所對(duì)應(yīng)的所述空間運(yùn)動(dòng)路徑上的點(diǎn)的坐標(biāo)及姿態(tài)。

4.如權(quán)利要求1所述的機(jī)器人空間運(yùn)動(dòng)規(guī)劃方法，其特征在于，所述步驟S6中還包括以下步驟：

步驟S61：判斷所述輸入?yún)?shù)是否可以應(yīng)用于S型曲線加減速方法對(duì)所述六軸機(jī)器人進(jìn)行運(yùn)動(dòng)規(guī)劃；

若是則轉(zhuǎn)向步驟S62；

若否則轉(zhuǎn)向步驟S63；

步驟S62：應(yīng)用S型加減速方法對(duì)所述六軸機(jī)器人進(jìn)行運(yùn)動(dòng)規(guī)劃，并輸出所述規(guī)劃運(yùn)動(dòng)參數(shù)，隨后轉(zhuǎn)到步驟S7；

步驟S63：應(yīng)用梯形加減速方法對(duì)所述六軸機(jī)器人進(jìn)行運(yùn)動(dòng)規(guī)劃,并輸出所述規(guī)劃運(yùn)動(dòng)參數(shù)，隨后轉(zhuǎn)到步驟S7。