[發(fā)明專利]一種無(wú)人叉車的自動(dòng)叉取方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202111430280.6
• 申請(qǐng)日: 2021-11-29
• 公開(kāi)（公告）號(hào): CN114195045A
• 公開(kāi)（公告）日: 2022-03-18
• 發(fā)明（設(shè)計(jì)）人: 田豐;楊澤鋒;葉國(guó)云;泮振宇;張堯堯;謝武達(dá)
• 申請(qǐng)（專利權(quán)）人: 寧波如意股份有限公司
• 主分類號(hào): B66F9/06
• 分類號(hào): B66F9/06;B66F9/075;G05D1/02
• 代理公司: 寧波市鄞州盛飛專利代理事務(wù)所(特殊普通合伙) 33243
• 代理人: 鮑英彬

1.一種無(wú)人叉車的自動(dòng)叉取方法，其特征在于，以目標(biāo)托盤的慣性原點(diǎn)為目標(biāo)原點(diǎn)構(gòu)建目標(biāo)坐標(biāo)系，以叉車輪軸中心點(diǎn)為叉車原點(diǎn)構(gòu)建叉車坐標(biāo)系，包括步驟：

S1：基于目標(biāo)坐標(biāo)系和叉車坐標(biāo)系，通過(guò)視覺(jué)傳感器獲取目標(biāo)托盤相對(duì)于叉車的叉取位姿；

S2：根據(jù)叉取位姿，基于位姿鎮(zhèn)定算法控制叉車移動(dòng)至目標(biāo)托盤正前方的調(diào)校點(diǎn)；

S3：根據(jù)叉車實(shí)時(shí)的速度信息構(gòu)建叉車的運(yùn)動(dòng)學(xué)模型；

S4：根據(jù)運(yùn)動(dòng)學(xué)模型構(gòu)建叉車行進(jìn)并叉取過(guò)程中，當(dāng)前時(shí)刻叉車位姿與叉取位姿的誤差方程；

S5：獲取誤差方程微分后的微分方程并帶入軌跡跟蹤控制律公式，獲取下一時(shí)刻的速度信息；

S6：根據(jù)下一時(shí)刻的速度信息控制叉車進(jìn)行速度狀態(tài)調(diào)整；

S7：判斷叉車位姿是否到達(dá)叉取位姿，若否，返回S3步驟，若是，完成叉取動(dòng)作；

所述目標(biāo)托盤的叉取方向?yàn)槟繕?biāo)坐標(biāo)系的第一軸向，所述叉車輪軸的中垂線為叉車坐標(biāo)系的第一軸向。

2.如權(quán)利要求1所述的一種無(wú)人叉車的自動(dòng)叉取方法，其特征在于，所述S2步驟中，位姿鎮(zhèn)定算法可表述為如下公式，

式中，r為目標(biāo)原點(diǎn)與叉車原點(diǎn)之間的直線距離，α為目標(biāo)原點(diǎn)與叉車原點(diǎn)連線與叉車坐標(biāo)系第一軸向的角度，β為目標(biāo)原點(diǎn)與叉車原點(diǎn)連線與目標(biāo)坐標(biāo)系第一軸向的角度，θ為β與α之間的夾角，v為叉車的線速度，ω為叉車的角速度。

3.如權(quán)利要求2所述的一種無(wú)人叉車的自動(dòng)叉取方法，其特征在于，所述位姿鎮(zhèn)定算法，需滿足如下控制律公式，

v＝k_ρrcosθ

ω＝k_αα+k_ββ

式中，k_ρ、k_α和k_β為根據(jù)電機(jī)響應(yīng)速度設(shè)置的速度比例控制系數(shù)。

4.如權(quán)利要求1所述的一種無(wú)人叉車的自動(dòng)叉取方法，其特征在于，所述S3步驟中，運(yùn)動(dòng)學(xué)模型可表示為如下公式，

式中，q＝(x，y，θ)^T為叉車移動(dòng)過(guò)程中的廣義坐標(biāo)向量，T為時(shí)間，v為時(shí)刻T時(shí)叉車的線速度，ω為時(shí)刻T時(shí)叉車的角速度；叉車以線速度v和角速度ω的速度信息運(yùn)行，且叉車以調(diào)校點(diǎn)處的叉車原點(diǎn)構(gòu)建叉車坐標(biāo)系的情況下，x為時(shí)刻T時(shí)叉車在第一軸向上的水平偏移距離，y為時(shí)刻T時(shí)叉車與第一軸向的垂直偏移距離，θ為時(shí)刻T時(shí)叉車原點(diǎn)與調(diào)校點(diǎn)連線與第一軸向的夾角。

5.如權(quán)利要求4所述的一種無(wú)人叉車的自動(dòng)叉取方法，其特征在于，所述S4步驟中，誤差方程可表示為如下公式，

式中，q_e＝(x_e，y_e，θ_e)為時(shí)刻T時(shí)，在叉車以T時(shí)刻的叉車原點(diǎn)構(gòu)建叉車坐標(biāo)系的情況下，假使叉車以當(dāng)前角速度和線速度繼續(xù)運(yùn)行，叉車移動(dòng)至目標(biāo)原點(diǎn)后的坐標(biāo)偏移量；(x_r，y_r，θ_r)為目標(biāo)原點(diǎn)在叉車以調(diào)校點(diǎn)處的叉車原點(diǎn)構(gòu)建叉車坐標(biāo)系下的坐標(biāo)量。

6.如權(quán)利要求5所述的一種無(wú)人叉車的自動(dòng)叉取方法，其特征在于，所述S5步驟中，微分方程可表示為如下公式，

式中，ω_c為下一時(shí)刻的角速度，v_c為下一時(shí)刻的線速度，v_r為叉車的預(yù)設(shè)線速度，ω_r為叉車的預(yù)設(shè)角速度。

7.如權(quán)利要求6所述的一種無(wú)人叉車的自動(dòng)叉取方法，其特征在于，所述S5步驟中，軌跡跟蹤控制律公式可表示為如下公式，

式中，k1、k2、k3為速度比例控制項(xiàng)，a，b為防止除0運(yùn)算的約束量。

8.如權(quán)利要求7所述的一種無(wú)人叉車的自動(dòng)叉取方法，其特征在于，所述約束量需滿足如下約束公式，

k₂＜(v_max-v_rmax)

k₁v_rmax+k₃＜(ω_max-ω_rmax)

k₁，k₂，k₃，a，b＞0

式中，v_max為叉車的最大線速度，v_rmax為叉車的預(yù)設(shè)最大線速度，ω_max為叉車的最大角速度，ω_rmax為叉車的預(yù)設(shè)最大角速度。