[發(fā)明專利]路徑軌跡追蹤系統(tǒng)

在審

• 申請?zhí)枺?/td>: 201911118962.6
• 申請日: 2019-11-15
• 公開（公告）號: CN112815952A
• 公開（公告）日: 2021-05-18
• 發(fā)明（設(shè)計）人: 林家仁;林詠翔;張家喜;賴俊吉
• 申請（專利權(quán)）人: 東元電機股份有限公司
• 主分類號: G01C21/34
• 分類號: G01C21/34
• 代理公司: 北京同立鈞成知識產(chǎn)權(quán)代理有限公司 11205
• 代理人: 羅英;臧建明

1.一種路徑軌跡追蹤系統(tǒng)，設(shè)置于移動平臺，其特征在于，包含：

定位模塊，用以定位所述移動平臺而獲得目前坐標；

路徑分段模塊，用以利用多個路徑點將所述移動平臺所應(yīng)遵行的規(guī)劃路徑劃分成多個路段，且每一所述多個路徑點具有路徑點坐標；

位置向量運算模塊，電性連接所述定位模塊與所述路徑分段模塊，用以接收所述目前坐標與所述多個路徑點的路徑點坐標，并利用所述目前坐標分別與每一所述多個路徑點的路徑點坐標運算出位置向量，藉以運算出多個上述的位置向量；

路徑向量運算模塊，電性連接所述路徑分段模塊，用以利用所述多個路徑點中的一者與次一相鄰者定義出與所述位置向量相對應(yīng)的路徑向量；

路徑重建模塊，電性連接所述位置向量運算模塊與所述路徑向量運算模塊，藉以利用所述多個位置向量與所述多個路徑向量中的彼此對應(yīng)者計算出向量夾角，將所述向量夾角大于270度與小于90度所對應(yīng)的路徑點刪除，以將所述規(guī)劃路徑剩余的部分重建成殘余路徑；

軌跡追蹤向量運算模塊，電性連接所述路徑重建模塊與所述定位模塊，用以運算出所述目前坐標與所述殘余路徑之間的最短距離，據(jù)以產(chǎn)生軌跡追蹤向量；以及

控制模塊，電性連接所述軌跡追蹤向量運算模塊，藉以依據(jù)所述軌跡追蹤向量產(chǎn)生控制命令，并利用所述控制命令控制所述移動平臺沿所述軌跡追蹤向量移動并追跡至所述殘余路徑。

2.根據(jù)權(quán)利要求1所述的路徑軌跡追蹤系統(tǒng)，還包含路徑產(chǎn)生模塊，且所述路徑產(chǎn)生模塊電性連接所述路徑分段模塊，用以產(chǎn)生所述規(guī)劃路徑。

3.根據(jù)權(quán)利要求1所述的路徑軌跡追蹤系統(tǒng)，其中，所述位置向量運算模塊包含位置向量運算單元，且所述位置向量運算單元將所述目前坐標分別減去每一所述多個路徑點的路徑點坐標以運算出所述多個位置向量。

4.根據(jù)權(quán)利要求1所述的路徑軌跡追蹤系統(tǒng)，其中，所述路徑向量運算模塊包含路徑向量運算單元，且所述路徑向量運算單元將上述所述多個路徑點中的次一相鄰者減去所述多個路徑點中的該者以定義出所述路徑向量。

5.根據(jù)權(quán)利要求1所述的路徑軌跡追蹤系統(tǒng)，其中，所述路徑重建模塊包含：

長度運算單元，用以運算出每一所述多個位置向量與每一所述多個路徑向量的長度；

余弦運算單元，電性連接所述長度運算單元，并利用所述多個位置向量與所述多個路徑向量中的彼此對應(yīng)者及其長度計算出所述向量夾角的余弦值；以及

判斷單元，電性連接所述余弦運算單元，用以接收所述余弦值，并在確認所述余弦值大于0時，判斷出所述向量夾角大于270度或小于90度。

6.根據(jù)權(quán)利要求5所述的路徑軌跡追蹤系統(tǒng)，其中，所述軌跡追蹤向量運算模塊包含：

向量運算單元，利用被刪除的路徑點的最后一者所對應(yīng)的位置向量與路徑向量運算出面積值；以及

距離運算單元，電性連接所述向量運算單元，并利用所述面積值與上述路徑向量的長度運算出所述最短距離。

7.根據(jù)權(quán)利要求6所述的路徑軌跡追蹤系統(tǒng)，其中，所述軌跡追蹤向量運算模塊還包含：

方向判斷單元，判斷出自目標位置朝向所述殘余路徑的追跡方向；以及

向量產(chǎn)生單元，利用所述最短距離與所述追跡方向產(chǎn)生所述軌跡追蹤向量。