[發(fā)明專利]一種無人艇一字型隊到倒V字型隊隊形變換方法

有效

• 申請?zhí)枺?/td>: 201710004063.8
• 申請日: 2017-01-04
• 公開（公告）號: CN106774336B
• 公開（公告）日: 2020-04-07
• 發(fā)明（設(shè)計）人: 蘇厚勝;孫亞平;陳志強(qiáng);王磊;耿濤;鐘鳳娟;劉淑君
• 申請（專利權(quán)）人: 廣東華中科技大學(xué)工業(yè)技術(shù)研究院;華中科技大學(xué);廣東省智能機(jī)器人研究院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 廣州粵高專利商標(biāo)代理有限公司 44102
• 代理人: 羅曉林;楊桂洋

1.一種無人艇一字型隊到倒V字型隊隊形變換方法，包括以下步驟：

S1，為在水面上成一字型隊形中的無人艇按照從小到大的順序編號，并從中選取出一艘無人艇作為領(lǐng)導(dǎo)無人艇；

S2，計算獲取下一個時間間隔內(nèi)所有無人艇的實際速度方向；

S3，計算獲取下一個時間間隔內(nèi)所有無人艇的速度值；

S4，計算獲取所有無人艇下一個時間間隔初的位置點；

S5，判斷隊形是否變換完成，若變換完成，結(jié)束；若未完成隊形的變換，則返回步驟S2；

除領(lǐng)導(dǎo)無人艇之外的其他所有無人艇按照獲取得到的實際速度方向、速度值和位置點進(jìn)行運動，實現(xiàn)從一字型隊到倒V字型隊隊形的變換，其中領(lǐng)導(dǎo)無人艇的運動由該領(lǐng)導(dǎo)無人艇的預(yù)設(shè)運動方程確定；

所述步驟S2包括求取無人艇期望速度方向和無人艇實際速度方向，其中：

求取無人艇期望速度方向具體包括：

S201，以編號為i的無人艇為圓心，半徑為r畫圓，若該圓內(nèi)有其他無人艇，則稱該其他無人艇為鄰居無人艇，設(shè)該鄰居無人艇個數(shù)為n_ir；

S202，當(dāng)n_ir不為0，則下一個時間間隔內(nèi)編號為i的無人艇的期望速度方向為鄰居無人艇的速度方向向量和的反方向，該期望速度方向為：

當(dāng)n_ir為0，則下一個時間間隔內(nèi)編號為i的無人艇的期望速度方向為當(dāng)前時刻編號為i的無人艇實際位置指向編號為i的無人艇期望位置，該期望速度方向為：

dir_i[k+1]＝(dir_i[k+1](1，1)，dir_i[k+1](1，2))，

dir_i[k+1](1，1)＝x_i0[k]-x_i[k]，

dir_i[k+1](1，2)＝y(tǒng)_i0[k]-y_i[k]，

式中：i＝1，2......N；k≥0；dir_i[k+1]是編號為i的無人艇在第k+1個時間間隔內(nèi)的期望速度方向；dit_i[k]是編號為i的無人艇的j鄰居無人艇在第k個時間間隔內(nèi)的實際速度方向；dir_i[k+1](1，1)為dir_i[k+1]的x軸分量；dir_i[k+1](1，2)為dir_i[k+1]的y軸分量；x_i0[k]為第k個時間間隔初，編號為i的無人艇相對于領(lǐng)導(dǎo)無人艇期望位置的x軸分量；x_i[k]為第k個時間間隔初，編號為i的無人艇實際位置的x軸分量；y_i0[k]為第k個時間間隔初，編號為i的無人艇相對于領(lǐng)導(dǎo)無人艇期望位置的y軸分量；y_i[k]為第k個時間間隔初，編號為i的無人艇實際位置的y軸分量，從而得到編號為i的無人艇在下一個時間間隔內(nèi)的期望速度方向；

判斷無人艇轉(zhuǎn)動角度是否超過無人艇的最大轉(zhuǎn)動角度，并求取無人艇實際速度方向，具體包括：

S203，設(shè)定第k個時間間隔內(nèi)編號為i的無人艇的最大轉(zhuǎn)動角度為θ_i[k]；

S204，當(dāng)dir_i[k+1]＝0，則當(dāng)前時刻編號為i的無人艇實際位置與期望位置重合，則當(dāng)前時刻編號為i的無人艇期望速度方向與領(lǐng)導(dǎo)無人艇同向，則無人艇實際速度方向為：

當(dāng)<dir_i[k+1]，dit_i[k]>小于等于θ_i[k]時，則編號為i的無人艇能夠在一個時間間隔內(nèi)能由dit_i[k]轉(zhuǎn)向dir_i[k+1]，則下一個時間間隔內(nèi)編號為i的無人艇的實際速度方向為：

dit_i[k+1]＝dir_i[k+1]；

當(dāng)<dir_i[k+1]，dit_i[k]>大于θ_i[k]時，則編號為i的無人艇無法在一個時間間隔內(nèi)由dit_i[k]轉(zhuǎn)向dir_i[k+1]，則下一個時間間隔內(nèi)編號為i的無人艇的實際速度方向dit_i[k+1]為dit_i[k]向dir_i[k+1]轉(zhuǎn)動θ_i[k]角后的方向；

上式中：i＝1，2……N；k≥0；<dir_i[k+1]，dit_i[k]>為dir_i[k+1]與dit_i[k]之間的夾角；為領(lǐng)導(dǎo)無人艇在第k個時間間隔內(nèi)的速度方向；dit_i[k+1]表示編號為i的無人艇在第k+1個時間間隔內(nèi)的實際速度方向；θ_i[k]表示在第k個單位時間間隔內(nèi)，編號為i的無人艇的最大轉(zhuǎn)動角度；

最終，得到編號為i的無人艇在下一時間間隔內(nèi)的實際速度方向dit_i[k+1]，將該實際速度方向單位化為：

式中：i＝1，2……N；k≥0；|dir_i[k+1]|表示dir_i[k+1]的模；

上述各式子求取除了領(lǐng)導(dǎo)無人艇外的其他所有無人艇的實際速度方向；

所述步驟S3對無人艇的速度值的計算獲取具體包括：

編號為i的無人艇在第k+1個時間間隔內(nèi)的期望速度方向與領(lǐng)導(dǎo)無人艇在第k個時間間隔內(nèi)的實際速度方向夾角為：

當(dāng)θ_i0＜θ_i3[k]且(|x_i0[k]-x_i[k]|＜μ)且(y_i0[k]-y_i[k])＞0時，下一個時間間隔內(nèi)編號為i的無人艇的速度值為：

當(dāng)(θ_i0≠0或(y_i0[k]-y_i[k])≤0)且(θ_i0＜θ_i1[k])且(v_i1[k]＜v_i[k])時，下一個時間間隔內(nèi)編號為i的無人艇的速度值為：

當(dāng)(θ_i0≠0或(y_i0[k]-y_i[k])≤0)且(θ_i0＜θ_i1[k])且(v_i1[k]＝v_i[k])時，下一個時間間隔內(nèi)編號為i的無人艇的速度值為：

v_i1[k+1]＝v_i[k]；

當(dāng)(θ_i0≠0或(y_i0[k]-y_i[k])≤0)且(θ_i0＜θ_i1[k])且(v_i1[k]＞v_i[k])時，下一個時間間隔內(nèi)編號為i的無人艇的速度值為：

當(dāng)[(θ_i1[k]≤θ_i0＜θ_i2[k])或((θ_i0＞θ_i2[k])且|x_i0[k]-x_i[k]|≥μ)]且(v_i1[k]≤v_i0[k])時，下一個時間間隔內(nèi)編號為i的無人艇的速度值為：

v_i1[k+1]＝v_i0[k]；

當(dāng)[(θ_i1[k]≤θ_i0＜θ_i2[k])或((θ_i0＞θ_i2[k])且|x_i0[k]-x_i[k]|≥μ)]且(v_i1[k]＞v_i0[k])時，下一個時間間隔內(nèi)編號為i的無人艇的速度值為：

當(dāng)(θ_i0＞θ_i2[k])且(|x_i0[k]-x_i[k]|＜μ)且(v_i1[k]≤v_i0[k])，下一個時間間隔內(nèi)編號為i的無人艇的速度值為：

v_i1[k+1]＝v_i0[k]；

當(dāng)(θ_i0＞θ_i2[k])且(|x_i0[k]-x_i[k]|＜μ)且(v_i1[k]＞v_i0[k])時，下一個時間間隔內(nèi)編號為i的無人艇的速度值和實際速度方向分別為：

dit_i[k+1]＝dit_i[k]；

上式中：i＝1，2……N；k≥0；表示領(lǐng)導(dǎo)無人艇第k個時間間隔內(nèi)的實際速度方向；表示的模；v_i1[k+1]表示編號為i的無人艇在第k+1個時間間隔內(nèi)的速度大小；v_i1[k]為編號為i的無人艇在第k個時間間隔內(nèi)的速度大小；v_i[k]表示編號為i的無人艇在第k個時間間隔內(nèi)的期望速度；v_i0[k]表示編號為i的無人艇在第k個時間間隔內(nèi)的最小速度；a_i1[k]、a_i2[k]、a_i3[k]、a_i4[k]、a_i5[k]表示編號為i的無人艇第k個時間間隔內(nèi)的加速度大小；θ_i1[k]、θ_i2[k]、θ_i3[k]表示第k個時間間隔內(nèi)的判斷角度值；μ表示無人艇倒V字型隊中橫向間距的最大誤差值；τ表示1個單位時間間隔；

從而得到編號為i的無人艇在第k+1個時間間隔內(nèi)的速度大小；

上述各式子求取除了領(lǐng)導(dǎo)無人艇外的其他所有無人艇的速度大小；

所述步驟S4具體包括：

根據(jù)下式計算得到：

x_i[k+1]＝x_i[k]+v_i1[k+1]·τ·dit_i[k+1](1，1)，

y_i[k+1]＝y(tǒng)_i[k]+v_i1[k+1]·τ·dit_i[k+1](1，2)，

上式中：i＝1，2……N；k≥0；x_i[k+1]表示編號為i的無人艇在第k+1個時間間隔初實際位置的x軸分量；y_i[k+1]表示編號為i的無人艇在第k+1個時間間隔初實際位置的y軸分量；dit_i[k+1](1，1)表示編號為i的無人艇在第k+1個時間間隔內(nèi)實際速度方向的x軸分量；dit_i[k+1](1，2)表示編號為i的無人艇在第k+1個時間間隔內(nèi)實際速度方向的y軸分量；

根據(jù)上式可計算得到編號為i的無人艇在第k+1個時間間隔初的位置點；

上述各式子求取除了領(lǐng)導(dǎo)無人艇外的其他所有無人艇的位置點；

所述步驟S5中判斷隊形是否完成還具體包括：

若(x_i[k+1]-x_i0[k+1]＝0)且(y_i[k+1]-y_i0[k+1]＝0)且則隊形變換完成，終止計算；否則，k＝k+1并返回步驟S2。