[發(fā)明專利]一種機(jī)器人控制方法與裝置

在審

• 申請?zhí)枺?/td>: 201910549019.4
• 申請日: 2019-06-24
• 公開（公告）號: CN110154035A
• 公開（公告）日: 2019-08-23
• 發(fā)明（設(shè)計(jì)）人: 宇曉龍
• 申請（專利權(quán)）人: 安徽紅蝠智能科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J13/08
• 代理公司: 合肥方舟知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 34158
• 代理人: 劉躍

1.一種機(jī)器人控制方法，其特征在于，具體包括以下步驟：

S101：設(shè)定采樣點(diǎn)t為0，設(shè)定機(jī)器人期望的位置和方向η_d＝[x_d，y_d，ψ_d]^T，設(shè)定機(jī)器人期望的速度θ_d＝[u_d，v_d，r_d]^T。η＝[x,y，ψ]^T表示位置和方向組合的向量，(x，y)表示位置，x表示東向坐標(biāo)，y表示北向坐標(biāo)，ψ表示方向，θ＝[u,v,r]^T表示速度組合的向量，u、v、r分別表示前進(jìn)速度、側(cè)向速度與偏航速度；下標(biāo)d表示該下標(biāo)對應(yīng)的變量的期望，上標(biāo)T表示轉(zhuǎn)置；

S102：采樣點(diǎn)t自增1，采集機(jī)器人位置和方向的數(shù)據(jù)η_m＝[x_m，y_m，ψ_m]^T，η_m中的三個(gè)元素分別對應(yīng)東向坐標(biāo)、北向坐標(biāo)與方向的檢測值；采集機(jī)器人速度的數(shù)據(jù)θ_m＝[u_m,v_m,r_m]^T，θ_m中的三個(gè)元素分別表示前進(jìn)速度、側(cè)向速度與偏航速度的檢測值；下標(biāo)m表示該下標(biāo)對應(yīng)的變量的檢測值；

S103：計(jì)算轉(zhuǎn)移矩陣R(ψ_m)，其中R(ψ)定義如下

其中，R(ψ)具有如下性質(zhì)：R^T(ψ)R(ψ)＝I，I為單位矩陣；S(r)的定義如下：

S104：計(jì)算位置和方向跟蹤誤差η_e＝η_m-η_d與速度跟蹤誤差θ_e＝θ_m-θ_d，下標(biāo)e表示該下標(biāo)對應(yīng)的變量的跟蹤誤差值；

S105：施加控制量Υ中元素分別表示前向、側(cè)向、偏航速度的控制量，具體如下：

其中，M為對稱的慣性矩陣，x₁＝η_e，k₁，k₂皆為3×1控制參數(shù)向量，其每個(gè)元素都大于0，l₁(x₂)為3×1向量，其中的元素k_1,i,x_2，i分別為k₁，x₂中的元素；l₂(x₂)為3×1向量，其中的元素ρ∈(0，1)，某變量上方一點(diǎn)表示該變量的微分，例如為η_e的微分；「k₂」表示該向量擴(kuò)展成的對角矩陣，即diag{k_2，1，k_2，2，k_2，3}；f(x₁，x₂)具體如下：

其中，C(θ_m)是科里奧利向心矩陣，D(θ_m)為阻尼矩陣；

S106：重復(fù)步驟S102至S105，實(shí)現(xiàn)每個(gè)采樣點(diǎn)的控制量輸出。

2.根據(jù)權(quán)利要求1所述的一種機(jī)器人控制方法，其特征在于，步驟S102涉及的位置的檢測方法基于衛(wèi)星定位系統(tǒng)，方向的檢測方法基于羅盤，前進(jìn)與側(cè)向速度的檢測基于流速計(jì)，偏航速度r的檢測基于陀螺儀。

3.一種機(jī)器人控制裝置，其特征在于，包括：機(jī)器人位置檢測單元，機(jī)器人方向檢測單元，機(jī)器人前進(jìn)與側(cè)向速度檢測單元，機(jī)器人偏航速度檢測單元，以及數(shù)據(jù)處理單元；數(shù)據(jù)處理單元用于執(zhí)行機(jī)器人控制程序，所述機(jī)器人控制程序在處理器執(zhí)行時(shí)實(shí)現(xiàn)權(quán)利要求1-2所述的任一項(xiàng)方法的步驟。

4.根據(jù)權(quán)利要求3所述的一種機(jī)器人控制裝置，其特征在于，位置檢測單元是衛(wèi)星定位系統(tǒng)，方向檢測單元是羅盤，前進(jìn)與側(cè)向速度檢測單元是流速計(jì)，偏航速率檢測單元是陀螺儀，所述機(jī)器人控制程序在處理器執(zhí)行時(shí)實(shí)現(xiàn)權(quán)利要求2所述方法的步驟。