[發(fā)明專利]一種掛裝車外掛物位姿調(diào)整方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110380532.2
• 申請(qǐng)日: 2021-04-08
• 公開（公告）號(hào): CN113268089B
• 公開（公告）日: 2023-03-24
• 發(fā)明（設(shè)計(jì)）人: 羅迪;韓召輝;李軍;曾慶遠(yuǎn)
• 申請(qǐng)（專利權(quán)）人: 成都立航科技股份有限公司
• 主分類號(hào): G05D3/12
• 分類號(hào): G05D3/12
• 代理公司: 成都金英專利代理事務(wù)所(普通合伙) 51218
• 代理人: 袁英

1.一種掛裝車外掛物位姿調(diào)整方法，其特征在于，所述掛裝車外掛物位姿調(diào)整方法包括以下步驟：

S1：利用智能識(shí)別技術(shù)和慣性數(shù)據(jù)計(jì)算得到目標(biāo)點(diǎn)位姿坐標(biāo)，且還包括以下子步驟：

S11：建立位置坐標(biāo)系{A}，設(shè)空間中任一點(diǎn)P的位置可用3×1的列矢量^AP表示為：

式中，上角標(biāo)A代表位置坐標(biāo)系{A}，^AP為位置矢量；

S12：建立姿態(tài)坐標(biāo)系{B}，設(shè)坐標(biāo)系{B}的三個(gè)相互垂直的單位矢量為x_B，y_B，z_B，對(duì)于空間任意矢量V，其方向由該矢量與三個(gè)坐標(biāo)軸夾角的方向余弦表示為：

即坐標(biāo)系{B}的三個(gè)單位矢量x_B，y_B，z_B相對(duì)于坐標(biāo)系{A}的方向余弦組成3×1矩陣^AR_B表示外掛物姿態(tài)：

式中：n_x——坐標(biāo)系{B}的X軸對(duì)坐標(biāo)系{A}X軸的余弦；

n_y——坐標(biāo)系{B}的X軸對(duì)坐標(biāo)系{A}Y軸的余弦；

n_Z——坐標(biāo)系{B}的X軸對(duì)坐標(biāo)系{A}Z軸的余弦；

o_x——坐標(biāo)系{B}的Y軸對(duì)坐標(biāo)系{A}X軸的余弦；

o_y——坐標(biāo)系{B}的Y軸對(duì)坐標(biāo)系{A}Y軸的余弦；

o_Z——坐標(biāo)系{B}的Y軸對(duì)坐標(biāo)系{A}Z軸的余弦；

a_x——坐標(biāo)系{B}的Z軸對(duì)坐標(biāo)系{A}X軸的余弦；

a_y——坐標(biāo)系{B}的Z軸對(duì)坐標(biāo)系{A}Y軸的余弦；

a_Z——坐標(biāo)系{B}的Z軸對(duì)坐標(biāo)系{A}Z軸的余弦；

該矩陣即表示外掛物相對(duì)于坐標(biāo)系{A}的姿態(tài)，^AR_B被稱為B的旋轉(zhuǎn)矩陣；

S13：將外掛物與坐標(biāo)系{B}固接，將外掛物的幾何中心設(shè)置為{B}的坐標(biāo)原點(diǎn)；

S14：用坐標(biāo)系{B}表示外掛物的位姿為：

{B}＝{^AR_B,^AP_B0} (4)；

S2：利用掛裝裝置調(diào)姿平臺(tái)上的運(yùn)動(dòng)傳感器，得到運(yùn)動(dòng)關(guān)節(jié)位姿坐標(biāo)；

S3：利用DH算法對(duì)運(yùn)動(dòng)方程逆解求取運(yùn)動(dòng)關(guān)節(jié)轉(zhuǎn)角和位移

S4：判斷目標(biāo)位姿是否可達(dá)，若目標(biāo)位姿可達(dá)，則對(duì)運(yùn)動(dòng)關(guān)節(jié)進(jìn)行運(yùn)動(dòng)規(guī)劃，得到每個(gè)關(guān)節(jié)運(yùn)動(dòng)的先后順序、運(yùn)動(dòng)時(shí)間，并輸出到控制器；若目標(biāo)位姿不可達(dá)，則發(fā)出警示標(biāo)志，對(duì)目標(biāo)位姿進(jìn)行修正；

S5：通過控制器控制調(diào)姿平臺(tái)運(yùn)動(dòng)以達(dá)到目標(biāo)位姿。

2.如權(quán)利要求1所述的一種掛裝車外掛物位姿調(diào)整方法，其特征在于，所述掛裝車外掛物位姿調(diào)整方法還包括：

S6：進(jìn)行多次微調(diào)，以達(dá)到要求的調(diào)姿精度。

3.如權(quán)利要求1所述的一種掛裝車外掛物位姿調(diào)整方法，其特征在于，所述步驟S2還包括以下子步驟：

S21：制定關(guān)節(jié)連桿命名規(guī)則：指定第一個(gè)關(guān)節(jié)為關(guān)節(jié)n，第二個(gè)關(guān)節(jié)為關(guān)節(jié)n+1，其余關(guān)節(jié)以此類推；指定第一個(gè)連桿為連桿n，第二個(gè)連桿為連桿n+1，其余連桿以此類推；

S22：確定Z軸方向：若關(guān)節(jié)是旋轉(zhuǎn)的，則Z軸方向?yàn)榘从沂忠?guī)則旋轉(zhuǎn)的方向；若關(guān)節(jié)是滑動(dòng)的，則Z軸方向?yàn)檠刂本€運(yùn)動(dòng)的方向，關(guān)節(jié)n處Z軸下標(biāo)為n-1；

S23：確定X軸方向：若兩關(guān)節(jié)的Z軸既不平行也不相交，則取兩關(guān)節(jié)Z軸公垂線的方向作為X軸方向，命名規(guī)則同Z軸；若兩關(guān)節(jié)的Z軸平行，則挑選兩Z軸之間與前一關(guān)節(jié)的公垂線共線的一條公垂線為X軸方向；若兩關(guān)節(jié)的Z軸相交，則取兩關(guān)節(jié)Z軸的叉積方向?yàn)閄軸方向；

S24：確定Y軸方向：取X軸、Y軸差積方向，按右手規(guī)則確定Y軸的方向；

S25：根據(jù)X、Y、Z軸方向建立坐標(biāo)系。

4.如權(quán)利要求1所述的一種掛裝車外掛物位姿調(diào)整方法，其特征在于，所述步驟S3還包括以下子步驟：

S31：將已知姿態(tài)矩陣的左右兩側(cè)依次乘以各機(jī)構(gòu)齊次變換的逆矩陣；

S32：選擇矩陣中具有常數(shù)項(xiàng)的元素，用左右兩邊同時(shí)相乘的方法得到轉(zhuǎn)角和位移。