[發(fā)明專利]一種六自由度平臺的定位控制方法

在審

• 申請?zhí)枺?/td>: 201811285907.1
• 申請日: 2018-10-31
• 公開（公告）號: CN109613819A
• 公開（公告）日: 2019-04-12
• 發(fā)明（設(shè)計）人: 陳良洲;宋暢;宋永鋒;金磊;陳有林
• 申請（專利權(quán)）人: 華中科技大學(xué)
• 主分類號: G05B11/42
• 分類號: G05B11/42;G05B17/02
• 代理公司: 華中科技大學(xué)專利中心 42201
• 代理人: 尚威;李智

1.一種六自由度平臺定位控制方法，該六自由度平臺包括上平臺、下平臺以及連接上平臺和下平臺的六個支腿，各支腿兩端均通過鉸鏈與上平臺、下平臺連接，各支腿均具有支腿促動器，支腿促動器分為上、下兩個部分，用于控制支腿伸縮，其特征在于，該控制方法包括以下步驟：

(1)根據(jù)六自由度平臺的結(jié)構(gòu)及結(jié)構(gòu)參數(shù)，建立其六自由度仿真模型；

(2)對步驟(1)的六自由度仿真模型添加約束和輸入、輸出變量；

(3)在步驟(2)的基礎(chǔ)上配置輸入、輸出變量，獲得機(jī)械系統(tǒng)動力學(xué)自動分析模型，并建立該機(jī)械系統(tǒng)動力學(xué)自動分析模型的simulink控制模塊；

(4)通過simulink控制模塊建立六自由度平臺基于正運(yùn)動學(xué)的PID控制算法，該基于正運(yùn)動學(xué)的PID控制算法包括根據(jù)給定的空間位姿坐標(biāo)計算當(dāng)前上平臺的空間坐標(biāo)差，通過運(yùn)動學(xué)反解將該空間坐標(biāo)差轉(zhuǎn)化為六個支腿促動器的長度變化，然后通過正運(yùn)動學(xué)計算，得到當(dāng)前上平臺的坐標(biāo)，實(shí)現(xiàn)六自由度平臺的定位控制。

2.根據(jù)權(quán)利要求1所述的一種六自由度平臺定位控制方法，其特征在于，步驟(1)中的結(jié)構(gòu)參數(shù)包括上平臺半徑、下平臺半徑、相鄰鉸點(diǎn)相對于所屬平臺的中心點(diǎn)的夾角以及上平臺的中位高度；其中，鉸點(diǎn)為六自由度仿真模型中鉸鏈的位置點(diǎn)。

3.根據(jù)權(quán)利要求1或2所述的一種六自由度平臺定位控制方法，其特征在于，步驟(2)中，先將六自由度平臺的各零件根據(jù)六自由度上平臺、六自由度下平臺、六個支腿促動器的上部分、下部分以及鉸鏈部分進(jìn)行整合以簡化六自由度仿真模型，并且計算出整合后各部分的質(zhì)量和轉(zhuǎn)動慣量。

4.根據(jù)權(quán)利要求3所述的一種六自由度平臺定位控制方法，其特征在于，步驟(2)中，將整合后各部分的質(zhì)量和轉(zhuǎn)動慣量添加至簡化后的六自由度仿真模型，并將上平臺與下平臺設(shè)為固定約束，六個支腿促動器上下部分之間設(shè)為圓柱副約束，支腿與上、下平臺之間設(shè)為萬向鉸鏈約束，添加六個支腿促動器沿支腿軸線方向的力驅(qū)動；將六個支腿促動器沿支腿軸線方向的力設(shè)為輸入變量，六個支腿的長度、伸縮速度以及上平臺在空間六個自由度方向上的位姿設(shè)為輸出變量。

5.根據(jù)權(quán)利要求1所述的一種六自由度平臺定位控制方法，其特征在于，步驟(4)中，基于正運(yùn)動學(xué)的PID控制算法包括如下子步驟：

(4.1)以六自由度平臺的上平臺在中位時的空間位姿坐標(biāo)q₀、q₁為初始坐標(biāo)，并給定目標(biāo)位置的空間位姿坐標(biāo)Q；

(4.2)給定六自由度平臺的上平臺定位精度ε及總迭代次數(shù)N，從當(dāng)前迭代次數(shù)k＝1開始，比較當(dāng)前上平臺的坐標(biāo)q_k與給定的空間位姿坐標(biāo)Q之間的空間坐標(biāo)差Δq_k與上平臺定位精度ε的大小，如果Δq_k≤ε，或者Δq_k＞ε且K＝N則停止迭代，輸出q_k作為所求正解；否則，進(jìn)入步驟(4.3)；

(4.3)通過運(yùn)動學(xué)反解將上平臺的空間坐標(biāo)之差轉(zhuǎn)化為六個支腿促動器的長度變化；

(4.4)將六個支腿促動器的長度變化進(jìn)行六自由度平臺的正運(yùn)動學(xué)計算，得到當(dāng)前上平臺的坐標(biāo)q_k+1；

(4.5)令k＝k+1，并返回步驟(2)。

6.根據(jù)權(quán)利要求5所述的一種六自由度平臺定位控制方法，其特征在于，步驟(4.4)中，六自由度平臺的正運(yùn)動學(xué)求解計算采用離散的牛頓迭代算法，方法如下：根據(jù)給定的坐標(biāo)初始值q₀、q₁，構(gòu)造一個關(guān)于上平臺坐標(biāo)變量q的方程F(q)＝0，在q₀處作泰勒一階展開得到用相鄰函數(shù)值與相鄰自變量之差商代替，得到：

7.一種計算機(jī)可讀存儲介質(zhì)，其特征在于，該計算機(jī)可讀存儲介質(zhì)上存儲有計算機(jī)程序，該計算機(jī)程序被處理器執(zhí)行時實(shí)現(xiàn)如權(quán)利要求1～6任一項(xiàng)所述的方法。

8.一種六自由度平臺定位控制設(shè)備，其特征在于，包括如權(quán)利要求7所述的計算機(jī)可讀存儲介質(zhì)以及處理器，處理器用于調(diào)用和處理計算機(jī)可讀存儲介質(zhì)中存儲的計算機(jī)程序。