本發(fā)明涉及路徑規(guī)劃技術領域，特別是涉及自動化作業(yè)設備路徑規(guī)劃方法與系統。

背景技術：

隨著人們生活水平的提高，目前越來越多的自動化作業(yè)設備應用到實際生產生活中，給人們帶來了巨大便利。例如目前常見的植保無人機自動進行植保操作、智能掃地機器人自動進行清掃操作等。

自動化作業(yè)設備在進行自動化作業(yè)過程中，如何合理規(guī)劃好作業(yè)路徑是非常重要的環(huán)節(jié)，合理路徑規(guī)劃能夠有效提升自動化作業(yè)設備的作業(yè)效率，帶來更大的經濟效益并給人們帶來更多的生活便利性。

然而，一般自動化作業(yè)設備路徑規(guī)劃一般基于起始點與終止點采用固定路徑規(guī)劃或采用自動檢測故障物的方式，當檢測到故障物時進行轉向(掃地機器人多采用此種方式)等方式進行路徑規(guī)劃。作業(yè)路徑固定規(guī)劃或無序隨意變動都會使自動化作業(yè)設備無目的、無針對性針對當前應用場景進行合理的作業(yè)，降低了自動化作業(yè)設備的作業(yè)效率。

技術實現要素：

基于此，有必要針對一般自動化作業(yè)設備路徑規(guī)劃固定或無序設定，無法針對當前應用場景進行合理規(guī)劃，嚴重降低自動化作業(yè)設備的作業(yè)效率的問題，提供一種自動化作業(yè)設備路徑規(guī)劃方法與系統，針對當前應用場景，合理規(guī)劃自動化作業(yè)設備路徑，有效提升其作業(yè)效率。

一種自動化作業(yè)設備路徑規(guī)劃方法，包括步驟：

獲取自動化作業(yè)設備的未作業(yè)范圍，并確定未作業(yè)范圍的各個頂點；

構建平面坐標系，確定各頂點的坐標；

以任意單個頂點為作業(yè)起點，根據作業(yè)起點坐標、作業(yè)路徑預設寬度以及各頂點中與作業(yè)起點相鄰的兩個頂點坐標，計算路徑規(guī)劃起點坐標，作業(yè)路徑預設寬度不大于自動化作業(yè)設備的作業(yè)寬度的一半；

沿平行于未作業(yè)范圍邊界的方向，與未作業(yè)范圍邊界間隔作業(yè)路徑預設寬度距離，在未作業(yè)范圍內側進行路徑規(guī)劃；

當未作業(yè)范圍為零時，終止路徑規(guī)劃，獲得自動化作業(yè)設備的路徑。

一種自動化作業(yè)設備路徑規(guī)劃系統，包括：

頂點確定模塊，用于獲取自動化作業(yè)設備的未作業(yè)范圍，并確定未作業(yè)范圍的各個頂點；

坐標構建模塊，用于構建平面坐標系，確定各頂點的坐標；

起點計算模塊，用于以任意單個頂點為作業(yè)起點，根據作業(yè)起點坐標、作業(yè)路徑預設寬度以及各頂點中與作業(yè)起點相鄰的兩個頂點坐標，計算路徑規(guī)劃起點坐標，作業(yè)路徑預設寬度不大于自動化作業(yè)設備的作業(yè)寬度的一半；

路徑劃設模塊，用于沿平行于未作業(yè)范圍邊界的方向，與未作業(yè)范圍邊界間隔作業(yè)路徑預設寬度距離，在未作業(yè)范圍內側進行路徑規(guī)劃；

終止模塊，用于當未作業(yè)范圍為零時，終止路徑規(guī)劃，獲得自動化作業(yè)設備的路徑。

本發(fā)明自動化作業(yè)設備路徑規(guī)劃方法與系統，獲取自動化作業(yè)設備的未作業(yè)范圍，并確定未作業(yè)范圍的各個頂點，確定各頂點的坐標，以任意單個頂點為作業(yè)起點，根據作業(yè)起點坐標、作業(yè)路徑預設寬度以及相鄰兩個頂點，計算路徑規(guī)劃起點坐標，沿平行于未作業(yè)范圍邊界的方向，與未作業(yè)范圍邊界間隔作業(yè)路徑預設寬度距離，在未作業(yè)范圍內側進行路徑規(guī)劃，當未作業(yè)范圍為零時，終止路徑規(guī)劃，獲得自動化作業(yè)設備的路徑。整個過程中，規(guī)劃的路徑由外向內逐漸收縮，可較大程度減小整個作業(yè)過程中自動化作業(yè)設備的小半徑轉向，降低轉向過程的能量消耗，提升作業(yè)時間與效率。

附圖說明

圖1為本發(fā)明自動化作業(yè)設備路徑規(guī)劃方法其中一個實施例的流程示意圖；

圖2為植保作業(yè)示意圖；

圖3為本發(fā)明自動化作業(yè)設備路徑規(guī)劃方法應用于植保作業(yè)的路徑規(guī)劃示意圖；

圖4為本發(fā)明自動化作業(yè)設備路徑規(guī)劃系統其中一個實施例的結構示意圖。

具體實施方式

為了便于說明本發(fā)明自動化作業(yè)設備路徑規(guī)劃方法的技術方案及其帶來的效果，下面將以自動化作業(yè)設備為植保無人機的應用實例，詳細介紹整個方案。

如圖1所示，一種自動化作業(yè)設備路徑規(guī)劃方法，包括步驟：

S100：獲取自動化作業(yè)設備的未作業(yè)范圍，并確定未作業(yè)范圍的各個頂點。

自動化作業(yè)設備的未作業(yè)范圍可以根據自動作業(yè)設備任務指示等數據來獲得。例如當前需要對1千米*1千米范圍內的植物進行植保操作，那么此時植保無人機的未作業(yè)范圍即為1千米*1千米的區(qū)域范圍。具體來說，未作業(yè)范圍的邊界可以是各種多邊形，例如常見的長方形、五邊形、六邊形等，還可以是不規(guī)則的多邊形。在這個未作業(yè)范圍中有多個頂點，將這些頂點依次連接起來即可圍成未作業(yè)范圍，如圖2所示，在圖2中，未作業(yè)范圍的頂點包括G₁、G₂、G₃以及G₄。簡單來說，頂點即發(fā)生轉向的點，相鄰兩個頂點處于相同直線上，該直線構成未作業(yè)范圍的邊界線，例如頂點G₁和G₂所處直線構成未作業(yè)范圍的邊界線G₁G₂，植保無人機在達到頂點時將會發(fā)生轉向，不會繼續(xù)沿直線飛行。

S200：構建平面坐標系，確定各頂點的坐標。

構建平面坐標系，以便準確表征各個頂點之間相對位置。平面坐標可以選擇以自動化作業(yè)設備?？奎c為原點構建平面坐標系，以向東的方向為X軸，以向北的方向為Y軸，建立二維直角坐標系。在具體應用實例中，以植保無人機?？奎c為原點，構建平面坐標系，采用全球定位系統或者北斗系統獲取植保作業(yè)范圍的頂點地理位置以及植保無人機?？奎c(原點)的地理位置，在構建的平面坐標系中，將頂點地理位置以及植保無人機?？奎c的地理位置表征出來，獲得各個頂點的坐標，即在圖2中獲得G₁、G₂、G₃以及G₄的坐標。

S300：以任意單個頂點為作業(yè)起點，根據作業(yè)起點坐標、作業(yè)路徑預設寬度以及各頂點中與作業(yè)起點相鄰的兩個頂點坐標，計算路徑規(guī)劃起點坐標，作業(yè)路徑預設寬度不大于自動化作業(yè)設備的作業(yè)寬度的一半。

任意選擇單個頂點作為作業(yè)起點，選擇與該作業(yè)起點所處單個邊界平行的方向開始作業(yè)。假定選擇圖2中頂點為G₁為作業(yè)起點，選擇與G₁所處單個邊界G₁G₂或G₁G₄平行方向開始作業(yè)，如選擇G₁G₂方向開始作業(yè)。自動化作業(yè)設備一般都有固定有作業(yè)寬度，作業(yè)寬度值可以從自動化作業(yè)設備屬性參數介紹或銘牌中獲得。自動化作業(yè)設備是沿路徑進行作業(yè)的，那么單次能夠作業(yè)寬度最大值為自動化作業(yè)設備的作業(yè)寬度的一半，即在規(guī)劃自動作業(yè)設備路徑時，作業(yè)路徑預設寬度應小于或等于自動化作業(yè)設備的作業(yè)寬度的一半。在圖2中，植保無人機作業(yè)路徑預設寬度為W/2，其中W為植保無人機的植保作業(yè)寬度。作業(yè)起點與其相鄰的兩個頂點連線構成未作業(yè)范圍的兩條邊界線，在圖2中，作業(yè)起點相鄰的兩個頂點為G₂和G₄，獲取G₂和G₄的坐標?；谧鳂I(yè)起點G₁、其相鄰兩個頂點G₂和G₄的坐標、以及作業(yè)路徑預設寬度W/2，即可以通過直線方程求解出路徑規(guī)劃起點坐標P₁的坐標。

S400：沿平行于未作業(yè)范圍邊界的方向，與未作業(yè)范圍邊界間隔作業(yè)路徑預設寬度距離，在未作業(yè)范圍內側進行路徑規(guī)劃。

未作業(yè)范圍的邊界是一個變化的范圍，即未作業(yè)范圍＝初始未作業(yè)范圍-已作業(yè)范圍。如在圖2中，當植保無人機還在P₁點準備進行植保操作時，未作業(yè)范圍為G₁、G₂、G₃以及G₄合圍的區(qū)域，當植保無人機植保作業(yè)至P₂點時，未作業(yè)范圍為G₅、G₆、G₂、G₃以及G₄合圍的區(qū)域。在步驟S300中計算獲得P₁的坐標，以P₁為起始點，沿平行于G₁G₂的方向，與未作業(yè)范圍邊界間隔作業(yè)路徑預設寬度距離，計算P₂的坐標，依次順序迭代確定各個路徑規(guī)劃中轉向點的坐標。

S500：當未作業(yè)范圍為零時，終止路徑規(guī)劃，獲得自動化作業(yè)設備的路徑。

當未作業(yè)范圍為零時，表明自動化作業(yè)設備按照當前規(guī)劃的路徑可以完成本次作業(yè)任務，終止路徑規(guī)劃，獲得自動化作業(yè)設備的路徑。未作業(yè)范圍為零的判定條件可以通過仿真模擬計算、仿真圖像處理以及基于坐標點的預測分析計算來獲得。

本發(fā)明自動化作業(yè)設備路徑規(guī)劃方法，獲取自動化作業(yè)設備的未作業(yè)范圍，并確定未作業(yè)范圍的各個頂點，確定各頂點的坐標，以任意單個頂點為作業(yè)起點，根據作業(yè)起點坐標、作業(yè)路徑預設寬度以及相鄰兩個頂點，計算路徑規(guī)劃起點坐標，沿平行于未作業(yè)范圍邊界的方向，與未作業(yè)范圍邊界間隔作業(yè)路徑預設寬度距離，在未作業(yè)范圍內側進行路徑規(guī)劃，當未作業(yè)范圍為零時，終止路徑規(guī)劃，獲得自動化作業(yè)設備的路徑。整個過程中，規(guī)劃的路徑由外向內逐漸收縮，可較大程度減小整個作業(yè)過程中自動化作業(yè)設備的小半徑轉向，降低轉向過程的能量消耗，提升作業(yè)時間與效率。

在其中一個實施例中，以任意單個頂點為作業(yè)起點，根據作業(yè)起點坐標、作業(yè)路徑預設寬度以及各頂點中與作業(yè)起點相鄰的兩個頂點坐標，計算路徑規(guī)劃起點坐標的步驟包括：

步驟一：在各頂點中任意選取作業(yè)起點。

下面將采用數學公式以及圖3詳細解釋，計算路徑規(guī)劃起點坐標的過程。如圖3所示，選取G₁、G₂、G₃以及G₄4個頂點中G₁為作業(yè)起點。

步驟二：確定各頂點中與作業(yè)起點相鄰的第一頂點和第二頂點，獲取第一頂點的坐標與第二頂點的坐標。

確定頂點G₂、G₃以及G₄3個頂點中與作業(yè)起點G₁相鄰的第一頂點G₂以及第二頂點G₄，獲取第一頂點G₂以及第二頂點G₄的坐標分別為(X₂,Y₂)和(X₄,Y₄)，作業(yè)起點G₁的坐標為(X₁,Y₁)。

步驟三：根據作業(yè)起點的坐標以及第一頂點的坐標，獲取未作業(yè)范圍中作業(yè)起點與第一頂點所處邊界直線的直線方程，記為第一邊界的直線方程。

根據作業(yè)起點G₁的坐標為(X₁,Y₁)和第一頂點G₂的坐標(X₂,Y₂)，獲得未作業(yè)范圍中作業(yè)起點G₁與第一頂點G₂所處邊界直線G₁G₂的直線方程：

步驟四：根據第一邊界的直線方程與作業(yè)路徑預設寬度，獲取路徑規(guī)劃第一直線的直線方程。

根據上述G₁G₂的直線方程以及作業(yè)路徑預設寬度，獲得路徑規(guī)劃第一直線P₁P₂的直線方程：

式中，W'為作業(yè)路徑預設寬度，一般來說W'＝W/2，W為自動化作業(yè)設備的作業(yè)寬度。

步驟五：根據作業(yè)起點的坐標以及第二頂點的坐標，獲取未作業(yè)范圍中作業(yè)起點與第二頂點所處邊界直線的直線方程，記為第二邊界的直線方程。

根據作業(yè)起點G₁的坐標(X₁,Y₁)和第二頂點G₄的坐標(X₄,Y₄)，獲取作業(yè)起點G₁與第二頂點G₄所處邊界直線G₁G₄的直線方程：

步驟六：根據路徑規(guī)劃第一直線的直線方程與第二邊界的直線方程，確定路徑規(guī)劃起點坐標。

計算直線P₁P₂和直線G₄G₁的交點坐標(x₁,y₁)，即為P₁點的坐標。

式中，b₁＝B₁+W'/cosθ、B₁＝(Y₁X₂-Y₂X₁)/(X₂-X₁)、B₄＝(Y₄X₁-Y₁X₄)/(X₁-X₄)。

在計算P₁點的坐標之后，需要確定自動化作業(yè)設備路徑規(guī)劃中下一轉向點P₂的坐標，采用上述計算P₁點的坐標相同的方式計算P₂，下面將參考上述公式(1)、(2)、(3)介紹下一轉向點P₂的坐標的計算過程。

1、根據G₂、G₃的坐標，可以參考公式(1)或者(3)，得到直線G₂G₃的方程。

2、參考公式(2)，可以得到直線P₂P₃的方程。

3、參考公式(4)，可以得到直線P₁P₂和直線P₂P₃的交點坐標，即P₂的坐標(x₂,y₂)。

相應的針對計算得到轉向點P₂的再下一轉向點P₃，可以采用下述方式計算：

1、參考公式(1)或(3)，可以得到邊界G5G6所在直線的方程。

2、用P₂點迭代P₁，可計算出軌跡P₂P₃的直線方程和P₃點的坐標。

重復上述迭代過程，即可計算在未作業(yè)范圍內側迭代計算路徑規(guī)劃下一轉向點坐標。針對后續(xù)下一轉向點P₄、P₅……P_n的過程與上述相似，在此不再贅述。

在其中一個實施例中，當未作業(yè)范圍為零時，終止路徑規(guī)劃的步驟包括：

當迭代計算出的路徑規(guī)劃下一轉向點坐標與相鄰已路徑規(guī)劃的路徑的垂直距離小于2倍作業(yè)路徑預設寬度時，判定當前轉折點為路徑規(guī)劃終點，終止路徑規(guī)劃。

如圖3所示，自動化作業(yè)設備的路徑規(guī)劃由外向內逐漸收縮，最終未作業(yè)范圍為零時，表明自動化作業(yè)設備按照當前規(guī)劃的路徑可以完成本次作業(yè)任務，終止路徑規(guī)劃。在這里，采用基于坐標點的計算的方式來判定未作業(yè)范圍為零。具體來說，當采用上述計算下一轉向點坐標方式計算路徑規(guī)劃中各個下一轉向點的坐標時，當某一個轉向點坐標與相鄰已路徑規(guī)劃的路徑的垂直距離小于2倍作業(yè)路徑預設寬度時，表明當前路徑規(guī)劃線路兩側已經被作業(yè)路徑預設寬度(自動化作業(yè)設備的作業(yè)寬度)所覆蓋，即當前當未作業(yè)范圍為零，終止路徑規(guī)劃。

同樣的，下面將采用實例，結合附圖3進行展開說明。在實例中，作業(yè)路徑預設寬度為自動化作業(yè)設備的作業(yè)寬度的一半。

重復上述迭代計算下一轉向點坐標，當計算得到P₁₂的坐標時，發(fā)現其與相鄰已路徑規(guī)劃的路徑P₉P₁₀的垂直距離小于自動化作業(yè)設備的作業(yè)寬度W，故植保作業(yè)結束。整個無人機的飛行路徑即為P₁→P₂→P₃→……→P₁₂。

如圖4所示，一種自動化作業(yè)設備路徑規(guī)劃系統，包括：

頂點確定模塊100，用于獲取自動化作業(yè)設備的未作業(yè)范圍，并確定未作業(yè)范圍的各個頂點。

坐標構建模塊200，用于構建平面坐標系，確定各頂點的坐標。

起點計算模塊300，用于以任意單個頂點為作業(yè)起點，根據作業(yè)起點坐標、作業(yè)路徑預設寬度以及各頂點中與作業(yè)起點相鄰的兩個頂點坐標，計算路徑規(guī)劃起點坐標，作業(yè)路徑預設寬度不大于自動化作業(yè)設備的作業(yè)寬度的一半。

路徑劃設模塊400，用于沿平行于未作業(yè)范圍邊界的方向，與未作業(yè)范圍邊界間隔作業(yè)路徑預設寬度距離，在未作業(yè)范圍內側進行路徑規(guī)劃。

終止模塊500，用于當未作業(yè)范圍為零時，終止路徑規(guī)劃，獲得自動化作業(yè)設備的路徑。

本發(fā)明自動化作業(yè)設備路徑規(guī)劃系統，頂點確定模塊100獲取自動化作業(yè)設備的未作業(yè)范圍，并確定未作業(yè)范圍的各個頂點，坐標構建模塊200確定各頂點的坐標，起點計算模塊300以任意單個頂點為作業(yè)起點，根據作業(yè)起點坐標、作業(yè)路徑預設寬度以及相鄰兩個頂點，計算路徑規(guī)劃起點坐標，路徑劃設模塊400沿平行于未作業(yè)范圍邊界的方向，與未作業(yè)范圍邊界間隔作業(yè)路徑預設寬度距離，在未作業(yè)范圍內側進行路徑規(guī)劃，終止模塊500當未作業(yè)范圍為零時，終止路徑規(guī)劃，獲得自動化作業(yè)設備的路徑。整個過程中，規(guī)劃的路徑由外向內逐漸收縮，可較大程度減小整個作業(yè)過程中自動化作業(yè)設備的小半徑轉向，降低轉向過程的能量消耗，提升作業(yè)時間與效率。

在其中一個實施例中，起點計算模塊300包括：

選取單元，用于在各頂點中任意選取作業(yè)起點。

坐標獲取單元，用于確定各頂點中與作業(yè)起點相鄰的第一頂點和第二頂點，獲取第一頂點的坐標與第二頂點的坐標。

第一計算單元，用于根據作業(yè)起點的坐標以及第一頂點的坐標，獲取未作業(yè)范圍中作業(yè)起點與第一頂點所處邊界直線的直線方程，記為第一邊界的直線方程。

第二計算單元，用于根據第一邊界的直線方程與作業(yè)路徑預設寬度，獲取路徑規(guī)劃第一直線的直線方程。

第三計算單元，用于根據作業(yè)起點的坐標以及第二頂點的坐標，獲取未作業(yè)范圍中作業(yè)起點與第二頂點所處邊界直線的直線方程，記為第二邊界的直線方程。

坐標計算單元，用于根據路徑規(guī)劃第一直線的直線方程與第二邊界的直線方程，確定路徑規(guī)劃起點坐標。

在其中一個實施例中，路徑劃設模塊400用于控制起點計算模塊300采用計算路徑規(guī)劃起點坐標相同方式，在未作業(yè)范圍內側迭代計算路徑規(guī)劃下一轉向點坐標。

在其中一個實施例中，終止模塊500用于當迭代計算出的路徑規(guī)劃下一轉向點坐標與相鄰已路徑規(guī)劃的路徑的垂直距離小于2倍作業(yè)路徑預設寬度時，判定當前轉折點為路徑規(guī)劃終點，終止路徑規(guī)劃。

在其中一個實施例中，坐標構建模塊200包括：

位置確定單元，用于確定自動化作業(yè)設備停放位置。

坐標構建單元，用于以自動化作業(yè)設備停放位置為坐標原點，構建平面坐標系，確定各頂點的坐標。

以上實施例僅表達了本發(fā)明的幾種實施方式，其描述較為具體和詳細，但并不能因此而理解為對發(fā)明專利范圍的限制。應當指出的是，對于本領域的普通技術人員來說，在不脫離本發(fā)明構思的前提下，還可以做出若干變形和改進，這些都屬于本發(fā)明的保護范圍。因此，本發(fā)明專利的保護范圍應以所附權利要求為準。