[發(fā)明專利]一種三維地形避障算法的適應(yīng)性評(píng)價(jià)方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201410519726.6
• 申請(qǐng)日: 2014-09-30
• 公開（公告）號(hào): CN105528774A
• 公開（公告）日: 2016-04-27
• 發(fā)明（設(shè)計(jì)）人: 孫權(quán)森;王文;王思文;沈肖波;周雨薇;季鴻坤
• 申請(qǐng)（專利權(quán)）人: 南京理工大學(xué)
• 主分類號(hào): G06T7/00
• 分類號(hào): G06T7/00
• 代理公司: 南京理工大學(xué)專利中心 32203
• 代理人: 朱顯國(guó);王培松

1.一種三維地形避障算法的適應(yīng)性評(píng)價(jià)方法，其特征在于，包括以下步驟：

步驟1、判斷采用某一避障算法計(jì)算得到的后驗(yàn)安全區(qū)域與給定的所有先驗(yàn)安全區(qū)域的 相容性：如果后驗(yàn)安全區(qū)域不在任意一個(gè)先驗(yàn)安全區(qū)域中，則判定前述避障算法評(píng)價(jià)失敗； 否則，進(jìn)入下述步驟2；以及

步驟2、判斷前述后驗(yàn)安全區(qū)域與給定的所有先驗(yàn)障礙區(qū)域的互斥性：如果后驗(yàn)安全區(qū) 域與任意一個(gè)先驗(yàn)障礙區(qū)域相交，則判定前述避障算法評(píng)價(jià)失敗；否則，判定前述避障算法 評(píng)價(jià)成功。

2.根據(jù)權(quán)利要求1所述的三維地形避障算法的適應(yīng)性評(píng)價(jià)方法，其特征在于，前述步驟 1中，所述后驗(yàn)安全區(qū)域與給定的所有先驗(yàn)安全區(qū)域的相容性判斷包括以下步驟：

步驟1-1、將前述先驗(yàn)安全區(qū)域等效為第一方形(A)，將前述后驗(yàn)安全區(qū)域等效為第二 方形(B)，其中，第一方形(A)的中心坐標(biāo)為P(x₁,y₁)，第二方形(B)的中心坐標(biāo)為Q(x₂,y₂)， 第一方形(A)邊長(zhǎng)的一半為r，第二方形(B)邊長(zhǎng)的一半為R；

步驟1-2、判斷下述式(1)是否同時(shí)成立：

(x₁-r≤x₂-R)&&(x₂+R≤x₁+r)&&(y₁-r≤y₂-R)&&(y₂+R≤y₁+r)(1)

其中，&&為邏輯運(yùn)算符；

如果前述式(1)不成立，則第一方形(A)與第二方形(B)不相容，判定前述避障算 法評(píng)價(jià)失敗；如果前述式(1)成立，則第一方形(A)與第二方形(B)相容。

3.根據(jù)權(quán)利要求2所述的三維地形避障算法的適應(yīng)性評(píng)價(jià)方法，其特征在于，前述步驟 2中，所述后驗(yàn)安全區(qū)域與給定的所有先驗(yàn)障礙區(qū)域的互斥性判斷包括以下步驟：

步驟2-1、將前述后驗(yàn)安全區(qū)域等效為前述第二方形(B)，將前述先驗(yàn)障礙區(qū)域等效為 第一圓形(C)，其中，第二方形(B)的中心坐標(biāo)為Q(x₂,y₂)，第一圓形(C)的圓心坐標(biāo)為 O(x₃,y₃)，第二方形(B)邊長(zhǎng)的一半為R；第一圓形(C)的半徑為D；

步驟2-2、計(jì)算第二方形(B)中心點(diǎn)與第一圓形(C)圓心的歐式距離d：

d=(x2-x3)2+(y2-y3)2---(2)]]>

步驟2-3、判斷下述式(3)-(8)是否成立：

d＜D(3)

d＜R(4)

(x₃＞x₂)&&(x₃＜x₂+R)&&(y₃＜y₂)&&(y₃＞y₂-R)(5)

(x₃＞x₂)&&(x₃＜x₂+R)&&(y₃＞y₂)&&(y₃＜y₂+R)(6)

(x₃＜x₂)&&(x₃＞x₂-R)&&(y₃＜y₂)&&(y₃＞y₂-R)(7)

(x₃＜x₂)&&(x₃＞x₂-R)&&(y₃＞y₂)&&(y₃＜y₂+R)(8)

其中，&&為邏輯運(yùn)算符；

如果前述(3)-(8)中的任意一個(gè)成立，則第二方形(B)與第一圓形(C)發(fā)生碰撞， 判定前述避障算法評(píng)價(jià)失敗；如果前述(3)-(8)均不成立，則進(jìn)入步驟2-4繼續(xù)判斷第二 方形(B)與第一圓形(C)是否相交；

步驟2-4、將下述四個(gè)式(9)-(12)分別與式(13)聯(lián)列，形成四個(gè)方程組：

x=x2-Ry2-R≤y≤y2+R---(9)]]>

x=x2+Ry2-R≤y≤y2+R---(10)]]>

y=y2-Rx2-R≤x≤x2+R---(11)]]>

y=y2+Rx2-R≤x≤x2+R---(12)]]>

(x-x₃)²+(y-y₃)²＝D²(13)

然后，分別求取四個(gè)方程組的根的總數(shù)，如果有任意一個(gè)方程組的根的總數(shù)≥2，則第二 方形(B)與第一圓形(C)發(fā)生碰撞，判定前述避障算法評(píng)價(jià)失敗；否則，判定前述避障算 法評(píng)價(jià)成功。

4.根據(jù)權(quán)利要求2所述的三維地形避障算法的適應(yīng)性評(píng)價(jià)方法，其特征在于，前述步驟 2-4中，采用求根公式法計(jì)算方程組的根的總數(shù)。