1249022 九、發明說明： 【發明所屬之技術領域】 本發明係與導航技術有關，特別是指一種自走裝置循 跡導航系統。 5【先前技術】 按，自走車是一種集合環境感知、規劃決策及無人化自 動操控等多功能為一體的綜合體，應用範圍甚為廣泛，除 了自動駕駛外，亦可使用於物料的自動運輸、倉庫的監視巡 邏、危險有害場所的作業等，均有高度發展潛力。 10 傳統的自走車通常是使用地面指示帶來導引，或是利 用GPS(全球衛星定位系統)來做定位之技術，依預定之路 徑來前進，並沿途不斷進行位置及航向的比對，使其確保 依照預之路徑來前進。然而，傳制技術中，大多數的路 徑規劃係為網格式點對點間相連接的線所組成的路徑，此 15種規劃方式會在轉彎處產生路徑不連續的現象，即所規割 自走車在行進的過程會有較不平‘ 柃4,其貝際仃走路徑較不易符合規劃路徑。 l發、叨pg容j 20 餅主要目的在於提供—種自走裝置循跡& 較平滑提，線與實際位置、航向的校對，並可: 浐曰、、1，使行走路線更接近於設定路線。 研从糸統，主要包含有：一自走裝置； -4- 1249022 羅盤；一速度感應器；以及一 預定路線至該自找㈣ 感應$…中係先輪入 5 航點，並驅動該自走裝置行:預藉:=義有= :器=該位置感應器來感測該自走裝置“ :該i=L並自=:導航點的平均座標點， r_ 一走進裝方 :於向裝置轉動車輪洲 【實施方式】 為了詳細說明本發明之構造及特點所在，兹舉以下之 較佳實施例並配合圖式說明如后，其中·· ，-圖係本發[較佳實施例之系贿構示意圖。 15 系本發明—較佳實施例之敢行進路線圖。 圖第三圖係本發明-較佳實施例之實際行進路線比較 第四圖係本發明一較佳實施例之C_R曲線示意圖。 第五圖係本發明一較佳實施例之操作示意圖，顯示自 20走裝置行進之狀態。 如第一圖所示，本發明第一較佳實施例所提供之一種 自走裝置循跡導航系統(10)，主要包含有： 一自走裝置（11)，本身具有複數車輪(21)而可移動以 及轉向，且可供使用者輸入預定路線，該自走裝置（11)具有 -5- 1249022 一轉向裝置(23)控制車輪(21)之轉向角； 一電子羅盤（12)，設置於該自走裝置（11)上，用以偵 測方向； 一速度感應器（14)，設置於該自走裝置（11)上，用以 5偵測該自走裝置(11)之行進速度； 一位置感應器（16)，設置於該自走裝置（11)上，用以 偵測該自走裝置(11)之位置，本實施例中係為GPS全球定 位系統之感應端； 15取得位置點，將該等位置點以四點一組的方式配合C_R曲 線(Catmull-Rom Splines)來求得一連續曲線即可形成預定 如第一圖至第四圖所示，係先輸入預定路線(如第二圖 1〇所示）至該自走裝置(11)内，該預定路線上定義有多數的導 航點，其中該預定路線可由兩種方式形成：一、由人員直接 將路徑電子擋輸入至該自走裝置(11)内；二、由人員驅動該 自走裝置(11)先行進一次，依預定時間間隔(例如2秒，此間 隔可視環境來調整其時間長短)沿途藉由該位置感應器（16) 路線。 接著5再驅動該自走裝置⑴)行進，藉由該雷子羅盤1249022 IX. Description of the invention: [Technical field to which the invention pertains] The present invention relates to navigation technology, and more particularly to a self-propelled device tracking navigation system. 5 [Previous technology] According to the self-propelled vehicle, it is a kind of complex that integrates environmental awareness, planning decision-making and unmanned automatic control. It has a wide range of applications. In addition to automatic driving, it can also be used for automatic material. Monitoring and patrolling of transportation, warehouses, and operations in hazardous and hazardous areas have high development potential. 10 Traditional self-propelled vehicles usually use the ground indication to guide, or use GPS (Global Positioning System) to make the positioning technology, proceed according to the predetermined path, and continuously compare the position and heading along the way. Make sure it moves forward according to the path ahead. However, in the transmission technology, most of the path planning is a path composed of line-to-point connected lines in the network format. The 15 kinds of planning methods will produce a path discontinuity at the turning point, that is, the self-propelled car In the process of traveling, there will be a relatively uneven '柃4', and the path of the Becker is less likely to conform to the planned path. l hair, 叨pgrong j 20 cake main purpose is to provide a kind of self-propelled device tracking & smoother, line and actual position, heading proofreading, and: 浐曰, 1, 1, make the walking route closer to Set the route. Research from SiS, mainly includes: a self-propelled device; -4- 1249022 compass; a speed sensor; and a predetermined route to the self-seeking (four) induction $... in the first round of 5 waypoints, and drive the self Walk device line: pre-borrow: = sense = = device = the position sensor to sense the self-propelled device ": the i = L and from =: the average coordinate point of the navigation point, r_ one into the installer: Turning the Wheels to the Device [Embodiment] In order to explain the structure and features of the present invention in detail, the following preferred embodiments are described with reference to the accompanying drawings, in which: BRIEF DESCRIPTION OF THE DRAWINGS The present invention is a preferred embodiment of the preferred embodiment of the present invention. 5 is a schematic diagram of the operation of a preferred embodiment of the present invention, showing the state of travel from the 20-way device. As shown in the first figure, a self-propelled device provided by the first preferred embodiment of the present invention The tracking navigation system (10) mainly includes: a self-propelled device (11), The vehicle itself has a plurality of wheels (21) movable and steerable, and the user can input a predetermined route. The self-propelling device (11) has a steering gear (11) of -5 - 1249022 (23) to control the steering angle of the wheel (21); An electronic compass (12) is disposed on the self-propelled device (11) for detecting a direction; a speed sensor (14) is disposed on the self-propelled device (11) for detecting the self-propelled a traveling speed of the device (11); a position sensor (16) disposed on the self-propelling device (11) for detecting the position of the self-propelled device (11), in this embodiment, GPS global positioning The sensing end of the system; 15 obtains the position point, and the C_R curve (Catmull-Rom Splines) is used in a four-point set to obtain a continuous curve to form a predetermined one as shown in the first to fourth figures. Preferably, the predetermined route (as shown in FIG. 1A) is first input into the self-propelled device (11), and the predetermined route defines a plurality of navigation points, wherein the predetermined route can be formed in two ways: The person directly inputs the path electronic gear into the self-propelled device (11); The self-propelled device (11) first travels once, and the route sensor (16) is routed along the way according to a predetermined time interval (for example, 2 seconds, the interval is adjusted according to the environment). Then 5 drives the self-propelled device (1) Travel through the thunder compass
至當 -6- 1249022 了相對於自走裝置行進方向之轉向角，該自走裝置（η)繼 續行進時，即可更為接近預定路徑，藉以達到使該自走裝 · 置(11)循跡導航的效果。 於本實施例中更包含有：一外部物體感應器（18)，設 置於4自走裝置（11)上，用以感測外部之障礙物之大小及 位置，該自走裝置（11)即可藉以執行預定的迴避措施，藉以 避開障礙物。 本貝知例於使用C-R曲線時，係主要根據FergUS〇n，s 鲁 函數(piecewise cubic functions)利用四個點來描述一曲線， 10該曲線具有内插、連續性等優點，可改進習用者之不連續性 的問題。其中，為了使C-R曲線符合Ferguson，s三次曲線 的特性，在端點定義斜率為（(P^-Pm)。，（p,>2-P/)/2)取代， 並配合第四圖推導出（式之矩陣方程式（〇n丄ine Geometric Modeling Notes CATMULL-ROM SPLINES j 15 2000) 〇When the self-propelling device (η) continues to travel, the self-propelled device (η) can be closer to the predetermined path, so as to achieve the self-propelled device (11) The effect of the trace navigation. In this embodiment, an external object sensor (18) is disposed on the 4 self-propelling device (11) for sensing the size and position of the external obstacle, and the self-propelling device (11) The predetermined avoidance measures can be implemented to avoid obstacles. Benbee knows that when using the CR curve, it uses four points to describe a curve based on the FergUS〇n, piecewise cubic functions. 10 This curve has the advantages of interpolation, continuity, etc., which can improve the practitioners. The problem of discontinuity. In order to make the CR curve conform to the characteristics of Ferguson, s cubic curve, the slope defined at the endpoint is ((P^-Pm)., (p, > 2-P/)/2), and the fourth figure Derived (the matrix equation of the formula (〇n丄ine Geometric Modeling Notes CATMULL-ROM SPLINES j 15 2000) 〇
2 0 0 1 一 5 4 3 — 3 0 0 一 1 (式l)2 0 0 1 a 5 4 3 — 3 0 0 a 1 (formula l)
其中among them
藉此，可在每組位置點的中問區段取得曲線，將各個 20曲線相連接繪出，即可完成整條路徑曲線；再將此曲線與 -7- 1249022 預定路線比對，即可決定出實際位置、方向與預定位置、方 向的差異，藉以進行修正。 由上可知，本發明在利用人員驅動自走裝置（11)來建 立預定路徑時，可獲得較為平滑的路徑，並沿途不斷的比 5對前進方向與期望航向之誤差，利用該轉向裝置(23)來轉 動車輪(21)轉動適當的轉向角度，藉以儘量符合於預定路 線0 1249022 【圖式簡單說明】 第一圖係本發明一較佳實施例之系統架構示意圖。 第二圖係本發明一較佳實施例之預定行進路線圖。 第三圖係本發明一較佳實施例之實際行進路線比較 5圖。 第四圖係本發明一較佳實施例之C-R曲線示意圖。 第五圖係本發明一較佳實施例之操作示意圖，顯示自 走裝置行進之狀態。 _ 1〇【主要元件符號說明】 (10)自走裝置循跡導航系統 （11)自走裝置 (12)電子羅盤 （14)速度感應器（16)位置感應器 (18)外部物體感應器 （21)車輪 (23)轉向裝置 •In this way, the curve can be obtained in the middle section of each group of points, and the 20 curves can be connected and drawn to complete the entire path curve; then the curve can be compared with the predetermined route of -7-1249022. Determine the difference between the actual position, direction and the predetermined position and direction, so as to make corrections. As can be seen from the above, the present invention can obtain a relatively smooth path when the person drives the self-propelled device (11) to establish a predetermined path, and continuously uses an error of 5 pairs of the forward direction and the desired heading direction, and utilizes the steering device (23). To rotate the wheel (21) to rotate the appropriate steering angle, so as to conform to the predetermined route 0 1249022 as much as possible. [First Description of the Drawings] The first drawing is a schematic diagram of the system architecture of a preferred embodiment of the present invention. The second drawing is a predetermined travel route diagram of a preferred embodiment of the present invention. The third figure is a comparison of the actual travel route of a preferred embodiment of the present invention. The fourth figure is a schematic view of a C-R curve of a preferred embodiment of the present invention. Fig. 5 is a schematic view showing the operation of a preferred embodiment of the present invention, showing the state in which the self-propelled device travels. _ 1〇 [Main component symbol description] (10) Self-propelled device tracking navigation system (11) Self-propelled device (12) Electronic compass (14) Speed sensor (16) Position sensor (18) External object sensor ( 21) Wheel (23) steering device •