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

在審

• 申請號: 201811285907.1
• 申請日: 2018-10-31
• 公開（公告）號: CN109613819A
• 公開（公告）日: 2019-04-12
• 發明（設計）人: 陳良洲;宋暢;宋永鋒;金磊;陳有林
• 申請（專利權）人: 華中科技大學
• 主分類號: G05B11/42
• 分類號: G05B11/42;G05B17/02
• 代理公司: 華中科技大學專利中心 42201
• 代理人: 尚威;李智

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

(1)根據六自由度平臺的結構及結構參數，建立其六自由度仿真模型；

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

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

(4)通過simulink控制模塊建立六自由度平臺基于正運動學的PID控制算法，該基于正運動學的PID控制算法包括根據給定的空間位姿坐標計算當前上平臺的空間坐標差，通過運動學反解將該空間坐標差轉化為六個支腿促動器的長度變化，然后通過正運動學計算，得到當前上平臺的坐標，實現六自由度平臺的定位控制。

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

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

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

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

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

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

(4.3)通過運動學反解將上平臺的空間坐標之差轉化為六個支腿促動器的長度變化；

(4.4)將六個支腿促動器的長度變化進行六自由度平臺的正運動學計算，得到當前上平臺的坐標q_k+1；

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

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

7.一種計算機可讀存儲介質，其特征在于，該計算機可讀存儲介質上存儲有計算機程序，該計算機程序被處理器執行時實現如權利要求1～6任一項所述的方法。

8.一種六自由度平臺定位控制設備，其特征在于，包括如權利要求7所述的計算機可讀存儲介質以及處理器，處理器用于調用和處理計算機可讀存儲介質中存儲的計算機程序。