TW201015457A

TW201015457A - Robot state inspecting system

Info

Publication number: TW201015457A
Application number: TW97138159A
Authority: TW
Inventors: Wen Shu
Original assignee: Hon Hai Prec Ind Co Ltd
Priority date: 2008-10-03
Filing date: 2008-10-03
Publication date: 2010-04-16

Abstract

A robot state inspecting system includes a measuring device, a data receiving device, a data processor, a state judgment device, an implement device, a locating device and an error correction device. The measuring device measures the movement of robot and then sends an acceleration signal and a velocity signal to the data receiving device. The data processor calculates out a robot movement parameter according to the acceleration signal and the velocity signal filtered by the data receiving device. The state judgment device judges the robot's state according to the robot movement parameter. The locating device is arranged along the movement route of the robot with equal interval. The locating device also provides adjustment information referring to its location. The error correction device receives the adjustment information provided by the locating device and corrects the robot's movement parameter calculated by the data processor.

Description

201015457 九、發明說明： * 【發明所屬之技術領域】 * 本發明涉及一種機器人狀態感知系統。【先前技術】隨著機械製造技術和自動控制技術之飛速發展，各種各樣之機器人已經進入人類生活和生產之相關領域，以代替或模仿人類執行各種任務。在控制機器人活動之各項技 ❿術中，最為關鍵之技術之一便係機器人之狀態感知技術。現有機器人之狀態感知主要係通過全球定位系統 (Global Positioning System，GPS )、紅外線導航系統或慣性導航系統對機器人之相關運動參量進行測量和計算來獲取機器人運動狀態資訊。但GPS系統於室内或有強電磁干擾之環境下會無法鎖定目標從而出現訊號中斷之情況，且使用成本較高，而紅外線導航系統對障礙物敏感，只能在 ❹較小範圍内滿足感測精度之要求。由加速度計和陀螺儀等運動感測器件所構成之慣性導航系統具有短時間内較高之精度且價格便宜但因其需要採用積分運算來推測被感測物之位置資訊，所產生之誤差會隨時間增加而累積，無法長時間使用。【發明内容】有鑒於此，有必要提供一種可長時間準確感知機器人狀態之機器人狀態感知系統。 7 201015457 一種機器人狀態感知系統，其包括：感測裝置’用於測量機器人運動之加速度訊號和角速度訊號。資料接收裝置’與該感測裝置連接，用於對該感測裝置測得之加速度訊號和角速度訊號進行濾波和數位化處理。資料處理器，與該資料接收裝置連接，用於根據該資料接收裝置濾波和數位化處理後之加速度訊號和角速度訊號计异機器人運動之加速度、角速度和位移。狀態判斷装置，與該資料接收裝置和資料處理器連接，用於根據該資料接收裝置濾波和數位化處理後之加速度訊號和該資料處理器計算出之加速度判斷機器人所處之運動狀態。執行裝置，與該狀態判斷裝置連接，用於根據機器人所處之運動狀態發出相對應之警報聲。 G 定位裝置，其沿機器人運動路徑按均等間距分佈。該定位裝置上設置有數位標籤，該數位標鐵上之内容為與該定位裝置所處位置相關之校正資訊。誤差校正裝置，其與該資料處理器相連接，用於讀取該數位標籤上之校正資訊並根據該校正資訊對該資料處理器所計算出來之機器人運動之位移進行校正。相對於Μ技術，本發明所提供之機器人狀態感知系統利用感測裝置測量機器人運動過程中之各種運動參量，並通過計算和分析來判斷機器人所處之運動狀態，以簡單 201015457 之結構和較低之成本實現了機器人之狀態感知。其次，在 * 機器人運動路徑上設置間距相等之定位裝置以提供與該定 ’ 位裝置所處位置相關之校正資訊，並通過誤差校正裝置來感測該定位裝置上之校正資訊從而對由資料處理器計算出來之機器人運動位移進行校正以克服機器人狀態感知系統於長時間使用過程中所產生之累積誤差。【實施方式】 ® 如圖1所示，本發明提供之機器人狀態感知系統2包括一感測裝置22、一與該感測裝置22相連接之資料接收裝置24、一與該資料接收裝置24相連接之資料處理器25、一與該資料接收裝置24和該資料處理器25相連接之狀態判斷裝置26、一與該狀態判斷裝置26相連接之執行裝置 28、一沿機器人運動路徑按均等間距分佈之定位裝置210 以及一與該資料處理器25相連接之誤差校正裝置212。 ^ 如圖2所示，該感測裝置22包括一加速度計221和一201015457 IX. Description of the invention: * [Technical field to which the invention pertains] * The present invention relates to a robot state sensing system. [Prior Art] With the rapid development of mechanical manufacturing technology and automatic control technology, various robots have entered the fields of human life and production to replace or imitate humans to perform various tasks. One of the most critical technologies in controlling the robot's activities is the state-aware technology of the robot. The state perception of the existing robot mainly obtains the motion state information of the robot by measuring and calculating the relevant motion parameters of the robot through a Global Positioning System (GPS), an infrared navigation system or a inertial navigation system. However, the GPS system can not lock the target indoors or in the environment with strong electromagnetic interference, and the signal is interrupted, and the use cost is high, and the infrared navigation system is sensitive to obstacles, and can only satisfy the sensing within a small range. Precision requirements. An inertial navigation system composed of a motion sensing device such as an accelerometer and a gyroscope has a high precision in a short time and is inexpensive, but because it needs to use an integral operation to estimate the position information of the sensed object, the error generated will be Accumulated over time and cannot be used for a long time. SUMMARY OF THE INVENTION In view of the above, it is necessary to provide a robot state sensing system that can accurately sense the state of a robot for a long time. 7 201015457 A robot state sensing system comprising: a sensing device 'for measuring an acceleration signal and an angular velocity signal of a robot motion. The data receiving device is coupled to the sensing device for filtering and digitizing the acceleration signal and the angular velocity signal measured by the sensing device. The data processor is coupled to the data receiving device for accelerating acceleration, angular velocity and displacement of the motion of the robot based on the acceleration signal and the angular velocity signal after filtering and digitizing the data receiving device. The state judging device is connected to the data receiving device and the data processor, and is configured to determine the motion state of the robot according to the acceleration signal after the data receiving device filtering and digitizing and the acceleration calculated by the data processor. The executing device is coupled to the state determining device for emitting a corresponding alarm sound according to the motion state of the robot. G positioning devices, which are distributed at equal intervals along the path of the robot movement. The positioning device is provided with a digital label, and the content on the digital standard is the correction information related to the position of the positioning device. An error correction device is coupled to the data processor for reading correction information on the digital tag and correcting the displacement of the robot motion calculated by the data processor based on the correction information. Compared with the cymbal technology, the robot state sensing system provided by the present invention uses a sensing device to measure various motion parameters during the motion of the robot, and calculates and analyzes the motion state of the robot by calculation and analysis, and the structure of the simple 201015457 is lower. The cost realizes the state perception of the robot. Secondly, positioning means with equal spacing are arranged on the * robot motion path to provide correction information related to the position of the fixed position device, and the correction information on the positioning device is sensed by the error correction device to process the data The calculated motion displacement of the robot is corrected to overcome the cumulative error generated by the robot state sensing system during long-term use. The present invention provides a robot state sensing system 2 including a sensing device 22, a data receiving device 24 connected to the sensing device 22, and a data receiving device 24. The connected data processor 25, a state judging device 26 connected to the data receiving device 24 and the data processor 25, and an executing device 28 connected to the state judging device 26 are equally spaced along the robot motion path. The distributed positioning device 210 and an error correction device 212 coupled to the data processor 25. As shown in FIG. 2, the sensing device 22 includes an accelerometer 221 and a

陀螺儀222。該加速度計221和陀螺儀222利用微機電技術 (Micro Electro-Mechanical System，MEMS )固裝於同一平臺220上。該加速度計221包括感測軸方向相互垂直之X 軸加速度感測器221a、Y軸加速度感測器221b及Z軸加速度感測器221c。該X軸、Y軸、Z軸加速度感測器221a、 221b和221c分別用於測量機器人於X軸、Y軸和Z轴方向上之加速度。該陀螺儀222包括感測軸方向分別與該加速度感測器221a、221b和221c之感測轴方向相對應之X 9 201015457 軸角速度感測器222a、Y軸角速度感測器222b及Z軸角速 . 度感測器222c。該X軸、Y軸、Z軸角速度感測器222a、 222b和222c分別用於測量機器人在X轴、γ軸和z轴方向上之角速度。以該X軸、Y軸、Z軸加速度感測器221a、 221b、和221c以及X軸、Y軸、Z軸角速度感測器222a、 222b和222c之感測軸方向為坐標軸可定義一隨機器人一起移動之感測活動直角坐標系，將機器人初始狀態時之感 ©測活動直角坐標系；定義為一起始參考直角坐標系 Χ-ΚΖ。。該加速度計221直接測量反映機器人相對於感測活動直角坐標系^07之加速度變化之加速度類比訊號。該陀螺儀222直接測量反映機器人相對於感測活動直角坐標系 I之角速度變化之角速度類比訊號。如圖1所示，該資料接收裝置24包括一截止頻率為1〇赫茲之高通濾波器241、一截止頻率為12·5赫茲之低通濾波器242及一截止頻率為50赫茲之低通濾波器243。〇該截止頻率為10赫茲之高通濾波器241與該加速度計 221電連接，用於對由該加速度計221測得之加速度類比訊號中之低頻干擾成分進行過濾並將其轉換成加速度數位訊號後傳輸至該狀態判斷裝置26。該截止頻率為12·5赫茲之低通濾波器242與該加速度计2=1電連接，用於對由該加速度計221測得之加速度類比訊號中之高頻干擾成分進行過濾並將其轉換成加速度數位訊號後傳輸至該資料處理器25。該截止頻率為5〇赫茲之低通濾波器243與該陀螺儀 201015457 222電連接’用於對由該陀螺儀222測得之角速度類比訊號中之干擾成分進行過濾並將其轉換成角速度數位訊號後傳輸至該資料處理器25。該資料處理器25根據經低通濾波器242處理後之加速Gyro 222. The accelerometer 221 and the gyroscope 222 are mounted on the same platform 220 using a Micro Electro-Mechanical System (MEMS). The accelerometer 221 includes an X-axis acceleration sensor 221a, a Y-axis acceleration sensor 221b, and a Z-axis acceleration sensor 221c that sense the axis directions perpendicular to each other. The X-axis, Y-axis, and Z-axis acceleration sensors 221a, 221b, and 221c are used to measure the acceleration of the robot in the X-axis, Y-axis, and Z-axis directions, respectively. The gyro 222 includes an X 9 201015457 shaft angular velocity sensor 222a, a Y-axis angular velocity sensor 222b, and a Z-axis angle, respectively, in which the sensing axis directions correspond to the sensing axis directions of the acceleration sensors 221a, 221b, and 221c, respectively. Speed sensor 222c. The X-axis, Y-axis, and Z-axis angular velocity sensors 222a, 222b, and 222c are used to measure the angular velocity of the robot in the X-axis, γ-axis, and z-axis directions, respectively. The X-axis, Y-axis, Z-axis acceleration sensors 221a, 221b, and 221c and the X-axis, Y-axis, and Z-axis angular velocity sensors 222a, 222b, and 222c can be defined as coordinate axes. The robot moves together to sense the active rectangular coordinate system, and the sense of the initial state of the robot is measured by the Cartesian coordinate system; it is defined as a starting reference rectangular coordinate system Χ-ΚΖ. . The accelerometer 221 directly measures an acceleration analog signal that reflects the change in acceleration of the robot relative to the sensing active rectangular coordinate system ^07. The gyroscope 222 directly measures an angular velocity analog signal that reflects the angular velocity of the robot relative to the sensed coordinate Cartesian coordinate system I. As shown in FIG. 1, the data receiving device 24 includes a high pass filter 241 having a cutoff frequency of 1 Hz, a low pass filter 242 having a cutoff frequency of 12 Hz, and a low pass filter having a cutoff frequency of 50 Hz. 243. The high-pass filter 241 having a cutoff frequency of 10 Hz is electrically connected to the accelerometer 221 for filtering the low-frequency interference component of the acceleration analog signal measured by the accelerometer 221 and converting it into an acceleration digital signal. It is transmitted to the state judging means 26. The low pass filter 242 having a cutoff frequency of 12·5 Hz is electrically connected to the accelerometer 2=1 for filtering and converting the high frequency interference component in the acceleration analog signal measured by the accelerometer 221 The acceleration digital signal is transmitted to the data processor 25. The low pass filter 243 having a cutoff frequency of 5 Hz is electrically connected to the gyro 201015457 222 for filtering the interference component in the angular velocity analog signal measured by the gyro 222 and converting it into an angular velocity digital signal. It is then transferred to the data processor 25. The data processor 25 is accelerated according to the processing by the low pass filter 242

度數位訊號計算出機器人相對於感測活動直角坐標系JiTZ 之加速度α。該加速度^包括加速度值之大小〇和加速度之方向，即加速度與感測活動直角坐標系^；^三個坐標軸之夾角：Θ、彡、γ 〇 © ^ 該資料處理器25根據經低通濾波器243處理後之角速度數位訊號計算出機器人相對於感測活動直角坐標系义泣之角速度V。將該角速度γ沿感測活動直角坐標系^^之 X轴、Υ轴、Ζ軸分解得到角速度γ沿χ軸、γ軸、ζ軸之分量：α、々、仞。因為感測裝置22直接測得之係機器人相對於感測活動直角坐標系之運動參量，若要得到機器人相對於起始參 ❹考直角坐標系m之運動參量必㈣求得感測活動直角坐標系π與起始參考直角坐標系ζχζ。之間之轉換矩陣〇該轉換矩陣c可通過對角速度分量之積分和矩陣運算求得0 如圖3所示，感測活動直角坐標系皿於初始狀態時與起始參考直角坐標系ΙΚΖ。相同，經過取樣時間^後新之感測活動直角坐標系'該角速度沿三勒之分量：心广，取樣時間_分可㈣ = △Γ内三個坐標轴所轉動之角度為“…將該感：: 11 201015457The degree signal calculates the acceleration α of the robot relative to the sensing active rectangular coordinate system JiTZ. The acceleration ^ includes the magnitude of the acceleration value and the direction of the acceleration, that is, the acceleration and the sensing active rectangular coordinate system ^; ^ the angle between the three coordinate axes: Θ, 彡, γ 〇© ^ The data processor 25 according to the low pass The angular velocity digital signal processed by the filter 243 calculates the angular velocity V of the robot relative to the sensing active rectangular coordinate system. The angular velocity γ is decomposed along the X-axis, the Υ-axis, and the Ζ-axis of the rectangular coordinate system of the sensing activity to obtain the components of the angular velocity γ along the χ axis, the γ axis, and the ζ axis: α, 々, 仞. Because the sensing device 22 directly measures the motion parameter of the robot relative to the sensing active rectangular coordinate system, if the motion parameter of the robot relative to the initial reference coordinate coordinate system m is to be obtained (4), the sensing activity rectangular coordinate is obtained. The system is π and the starting reference Cartesian coordinate system ζχζ. The conversion matrix 〇 The transformation matrix c can be obtained by integrating the angular velocity component and the matrix operation. As shown in Fig. 3, the active rectangular coordinate system is sensed in the initial state and the initial reference Cartesian coordinate system. The same, after the sampling time ^ new sensing activity Cartesian coordinate system 'The angular velocity along the three elements: heart wide, sampling time _ minutes can be (four) = △ Γ within the three axes of rotation of the angle is "... Sense:: 11 201015457

動直角坐標系观之轉動過程依次分解為繞X軸、Y軸、Z ，之三次轉動。首先將感測活動直角坐標系取繞2轴依逆時針方向轉動角得到第一次分解雜動弟次刀解轉動後之感測活動直角坐私系，則座標轉換關係為： V" X / y y Z z 其中，The rotation process of the Cartesian coordinate system is sequentially decomposed into three rotations around the X axis, the Y axis, and Z. Firstly, the rectangular coordinate system of the sensing activity is rotated around the 2 axes in the counterclockwise direction to obtain the sensing activity of the first decomposition of the turbulent rotation. The coordinates of the coordinates are: V" X / Yy Z z where,

轉換矩陣Ctf= 0 0 0 cos or sincr 0 -sin or cos a 接著將第一次分解轉動後之感測活動直义yz繞尤’軸依逆時針方向轉動々角得到第二次分A韓二系之感測活動直角坐標系，則座標轉換關係”為坪後 V] |V| / S / ff / z z J L· _ 其中，Conversion matrix Ctf= 0 0 0 cos or sincr 0 -sin or cos a Next, the sensing activity after the first decomposition is rotated, and the yz is rotated in the counterclockwise direction to get the second time. In the case of the sensing activity Cartesian coordinate system, the coordinate conversion relationship is "Ping Hou V] |V| / S / ff / zz JL· _ where

cos^ 0 sin>^ 轉換矩陣Cf 〇 1 〇 -sin 夕 0 cos p 最後將第二次分解轉動後之感測活動吉 ^ 4® ^ 繞Γ "軸依逆時針方向轉動似角得到第三次八 ’' 後之感測活動直角坐標系，則座標轉換關^為轉動 ^ X” ym / 其中， 12 201015457 -sin <y 〇 coso 〇 0 1Cos^ 0 sin>^ Conversion matrix Cf 〇1 〇-sin 夕0 cos p Finally, the second decomposition of the rotation after the sensing activity ji ^ 4® ^ Γ quot " axis counterclockwise rotation like the corner to get the third After the second eight'' sensing activity Cartesian coordinate system, the coordinate conversion is OFF ^ X" ym / where, 12 201015457 -sin <y 〇coso 〇0 1

COSO 轉換矩陣C；= sinfi； 0COSO conversion matrix C; = sinfi; 0

c = 其中，轉換矩陣〇 CaCfiCa =〇 COS or 0 sin or 0 -sin or cos or cos 户 0 sin P 0 10 -sin/? 0 cosΘ COSfi> sine; 0 -sino 0' coso 〇 0 1 综上該，起始參考直角坐標系XKZ。與轉動後之感測活動直角坐標系間之座標轉換關係為： •X”” ym zm =c y z - CaCjgCω y z 在經過n個取樣時間ΔΓ後只需要在上一個轉換矩陣 C«-i之基礎上乘上最後一次轉動所對應之轉換矩陣C則可得到感測活動直角坐標系與起始參考直角坐標系之間之轉換矩陣。該資料處理器25將機器人相對於感測活動直角坐標系 ❿之加速度α通過直角坐標系轉換後得到機器人相對於起始參考直角坐標系ΙΚΖ。之加速度汰。該加速度化與起始參考直角坐標系ζβκζ.三個坐標軸之夾角分別為汉、砵、 f，該加速度值大小為仏。將該加速度队對取樣時間積分可得到在取樣時間Ar内機器人相對於起始參考直角坐標系χβκζ。之運動速度γ。將該運動速度γ對取樣時間積分可得到在取樣時間ΔΓ内機器人相對於起始參考直角坐標系 Ζ-ΚΖ。之位移5。從初始狀態開始累積計算每一個取樣時間 △Γ内機器人相對於起始參考直角坐標系义。κζ。之位移S便 13 201015457 可得到機器人相對起始參考直角坐標系xxz。之總位移又，同時可描繪出機器人之運動軌跡。該狀態判斷裝置26内預設有用於判斷機器人是否被搖晃之搖晃時間閾值Η。該狀態判斷裝置26檢測經高通濾波器241處理後之加速度數位訊號。因高通濾波器241已將低頻振動引起之干擾訊號濾除，若狀態判斷裝置26檢測到訊號持續時間超過該搖晃時間閾值Η，且加速度值呈正負 ❹相間交替變化之加速度數位訊號時則判斷機器人處於被搖晃狀態並向該執行裝置28發出搖晃警報指令。該狀態判斷裝置26内預設有用於判斷機器人傾斜狀態之傾斜角範圍（0，& )、（戎，^ （。該狀態判斷裝置26接收由資料處理器25計算得到之機器人相對於起始參考直角坐標系UJ。之加速度α，並分別檢測該加速度久與起始參考直角坐標系tKZ。的尤軸、κ軸、乙軸之夾角：汉、戎'，。是否於該傾斜角範圍（q，色么χ( η,厂2) ❹内。如果該加速度夾角<9。、戎、γβ超出該傾斜角範圍則判斷機器人處於傾斜狀態中並向該執行裝置28發出傾斜擎報指令。。該執行裝置28包括一警報器281。若該執行裝置28 接收狀態判斷裝置26發出之搖晃控制指令則通過該警報器 281發出搖晃警報聲。若該執行裝置28接收到來自狀態判斷裝置26發出之傾斜警報指令則通過該警報器281發出傾斜警報聲。因為該加速度計221和陀螺儀222之測量誤差隨著感 201015457 測時間之延長上升得很快，而該資料處理器25係通過對該加速度計221和陀螺儀222所感測到之加速度β。進行積分來推算出機器人運動之速度值和位移值。所以該陀螺儀222 和加速度計221之測量誤差會進入積分公式進行累積從而導致所計算出來之速度值和位移值之誤差越來越大。因此，每當機器人運動一段距離後就必須通過設置於運動路徑上之定位裝置210來對機器人所感測到之相對於起始參考直角坐標系尤工乙之總位移S進行校正。 ® 如圖1所示，該定位裝置210包括一初始***2101 和至少一個輔助***2102。該初始***2101設置於機器人之運動起點，即該起始參考直角坐標系Χπζ-之原點處。在機器人每次重新開始運動之前，該初始***2101 對機器人進行初始化設定，清除資料處理器25内關於機器人前一次運動之運動參量。機器人每次從運動起點出發之前都已設置好即將運行 @之運動路徑。該輔助***2102從初始***2101開始沿設置好之機器人運動路徑按均等間距分佈。因為該每一個輔助***2102相對於該初始***2101具有確定之位移Υ，將該位移Υ定義為對應之輔助***2102之校正位移。該每一個輔助***2102上均設置有一與該校正位移Y相對應之數位標籤2103。該誤差校正裝置212包括一讀取器2120。在機器人每經過一輔助***2102時，該讀取器從輔助***2102 之數位標籤2103上讀取對應之校正位移Y。該誤差校正裝 15 201015457 置212將所獲取之板正位移V與該資料處理器25所計算出來之機器人相對於起始參考直角坐標系⑽。之總位移$ 相減得到位移校正差值批。在經過下—個輔助***測之前’該資料處理器25根據該位移校正差值^對其計算出來之機器人相對於起始參考直角坐標系尤κζ。之總位移乂進行校正。此後運動過程中，機器人將不斷地到達新之輔助***2102以更新該位移校正差值必來對由該資料處 ❹理器25計算出來之相對於起始參考直角坐標系辺。之總位移進行校正，從而確保所得到之總位移义精確度。可理解，需要設置之輔助***21〇2之個數由機器人運動路徑之長短和該感測裝置22之精確度所決定。該感測裝置22 之精密度越高則在相同之運動路徑内所需要之辅助*** 之個數越少。與先前技術相比，本發明所提供之機器人狀態感知系統利用加速度計和陀螺儀測量機器人運動過程中之各種運 ❹動參量，並通過計算和分析來判斷機器人所處之運動狀態，以簡單之結構和較低之成本實現了機器人之狀態感知。其次，在機器人運動路經上設置間距相等之定位裝置以提供與該定位裝置所處位置相關之校正資訊，並通過誤差校正裝置來感測該定位裝置上之校正資訊從而對由資料處理器計算出來之機器人運動位移進行校正以克服機器人狀態感知系統於長時間使用過程中所產生之累積誤差。綜上所述，本發明確已符合發明專利之要件，遂依法提出專利申請。惟，以上所述者僅為本發明之較佳實施方 16 201015457 式’自不能以此限制本案之申請專利範圍。舉凡熟悉本案技藝之人士援依本發明之精神所作之等效修飾或變化，皆應涵蓋於以下申請專利範圍内。【圖式簡單說明】圖1係本發明提供之機器人狀態感知系統之硬體架構圖。圖2係圖1之機器人狀態感知系統之感測裝置結構示意圖。圖3係感測活動直角坐標系與起始參考直角坐標系之間之轉動關係示意圖。【主要元件符號說明】機器人狀態感知系統 2 感測裝置 22 平台 220 加速度計 221 X軸加速度感測器 221a Y軸加速度感測器 221b Z軸加速度感測器 221c 陀螺儀 222 X轴角加速度感測器 222a Y軸角加速度感測器 222b 17 201015457c = where, the transformation matrix 〇CaCfiCa =〇COS or 0 sin or 0 -sin or cos or cos household 0 sin P 0 10 -sin/? 0 cosΘ COSfi>sine; 0 -sino 0' coso 〇0 1 , starting with reference to the Cartesian coordinate system XKZ. The coordinate conversion relationship with the sensed rectangular coordinate system after the rotation is: • X”” ym zm =cyz - CaCjgCω yz After n sampling times ΔΓ, only need to be multiplied on the previous conversion matrix C«-i The transformation matrix C corresponding to the last rotation can obtain the transformation matrix between the sensing active Cartesian coordinate system and the starting reference Cartesian coordinate system. The data processor 25 converts the acceleration of the robot relative to the sensed active rectangular coordinate system by a Cartesian coordinate system to obtain a robot with respect to the initial reference Cartesian coordinate system. The acceleration of the elimination. The acceleration and the initial reference rectangular coordinate system ζβκζ. The three coordinate axes are respectively Han, 砵, f, and the acceleration value is 仏. The acceleration team is integrated with the sampling time to obtain the χβκζ of the robot relative to the starting reference Cartesian coordinate system within the sampling time Ar. The speed of movement γ. Integrating the motion velocity γ with the sampling time results in the robot relative to the starting reference Cartesian coordinate system Ζ-ΚΖ within the sampling time ΔΓ. The displacement is 5. Cumulative calculation of each sampling time from the initial state ΔΓ The robot is relative to the starting reference Cartesian coordinate system. Κζ. The displacement S will be 13 201015457 to obtain the robot relative starting reference rectangular coordinate system xxz. The total displacement is again, and the trajectory of the robot can be depicted at the same time. The state judging means 26 is preliminarily provided with a shaking time threshold value Η for judging whether or not the robot is shaken. The state judging means 26 detects the acceleration digital signal processed by the high pass filter 241. Since the high-pass filter 241 has filtered out the interference signal caused by the low-frequency vibration, if the state judging device 26 detects that the signal duration exceeds the shaking time threshold Η, and the acceleration value is positive and negative, the acceleration digital signal alternates between the phases, the robot is judged. It is in a state of being shaken and issues a shaking alarm command to the actuator 28. The state judging means 26 is preliminarily provided with a tilt angle range (0, &) for determining the tilt state of the robot, (戎, ^ (the state judging means 26 receives the robot calculated by the data processor 25 with respect to the start Referring to the acceleration α of the Cartesian coordinate system UJ, and detecting the angle between the acceleration axis and the initial reference rectangular coordinate system tKZ, the angles of the y-axis, the κ axis, and the B-axis: Han, 戎', whether or not in the range of the tilt angle ( q, color χ (η, factory 2) ❹. If the acceleration angle <9, 戎, γβ exceeds the tilt angle range, it is judged that the robot is in the tilt state and issues a tilting command to the executing device 28. The actuator 28 includes an alarm 281. If the actuator 28 receives the shake control command from the state determination device 26, a shake alarm sound is emitted through the alarm 281. If the actuator 28 receives the state determination device 26 The tilting alarm command sends a tilting alarm sound through the alarm 281. Since the measurement error of the accelerometer 221 and the gyroscope 222 rises with the extension of the sensing time of 201015457 Soon, the data processor 25 estimates the velocity and displacement values of the robot motion by integrating the accelerometer 221 and the acceleration β sensed by the gyroscope 222. Therefore, the gyroscope 222 and the accelerometer 221 The measurement error will enter the integral formula to accumulate, resulting in the error of the calculated velocity value and displacement value becoming larger and larger. Therefore, each time the robot moves a certain distance, it must pass through the positioning device 210 disposed on the motion path. The robot senses that it is corrected relative to the total displacement S of the initial reference Cartesian coordinate system. As shown in Figure 1, the positioning device 210 includes an initial positioner 2101 and at least one auxiliary positioner 2102. The initial The positioner 2101 is disposed at the starting point of the movement of the robot, that is, the origin of the initial reference rectangular coordinate system Χπζ-. The initial positioner 2101 initializes the robot every time the robot restarts the motion, and clears the data processor 25 Inside the motion parameters of the robot's previous motion. The robot has been set up before the start of the movement. The motion path of @ is about to run. The auxiliary locator 2102 is distributed at equal intervals along the set robot motion path from the initial locator 2101. Because each of the auxiliary locators 2102 has a determined displacement relative to the initial locator 2101. That is, the displacement Υ is defined as the corrected displacement of the corresponding auxiliary locator 2102. Each of the auxiliary locators 2102 is provided with a digital label 2103 corresponding to the corrected displacement Y. The error correcting device 212 includes a reading The reader 2120 reads the corresponding corrected displacement Y from the digital tag 2103 of the auxiliary positioner 2102 each time the robot passes an auxiliary positioner 2102. The error correction device 15 201015457 sets 212 to obtain the plate positive displacement V and the robot calculated by the data processor 25 with respect to the starting reference Cartesian coordinate system (10). The total displacement $ is subtracted to obtain a displacement correction difference batch. The data processor 25 calculates the calculated robot based on the displacement correction difference ^ relative to the starting reference Cartesian coordinate system before passing through the next auxiliary positioner. The total displacement 乂 is corrected. Thereafter, during the movement, the robot will continually arrive at the new auxiliary positioner 2102 to update the displacement correction difference to be calculated by the data processor 25 relative to the initial reference Cartesian coordinate system. The total displacement is corrected to ensure the resulting total displacement accuracy. It can be understood that the number of auxiliary positioners 21〇2 to be set is determined by the length of the robot motion path and the accuracy of the sensing device 22. The higher the precision of the sensing device 22, the fewer the number of auxiliary locators required in the same motion path. Compared with the prior art, the robot state sensing system provided by the invention uses an accelerometer and a gyroscope to measure various movement parameters during the movement of the robot, and calculates and analyzes the motion state of the robot, which is simple. The structure and lower cost enable the state perception of the robot. Secondly, a positioning device with equal spacing is arranged on the robot motion path to provide correction information related to the position of the positioning device, and the correction information on the positioning device is sensed by the error correction device to be calculated by the data processor. The resulting robot motion displacement is corrected to overcome the cumulative error generated by the robot state sensing system during prolonged use. In summary, the present invention has indeed met the requirements of the invention patent, and has filed a patent application according to law. However, the above is only the preferred embodiment of the present invention. Equivalent modifications or variations made by persons skilled in the art in light of the present invention are intended to be included within the scope of the following claims. BRIEF DESCRIPTION OF THE DRAWINGS Fig. 1 is a hardware architecture diagram of a robot state sensing system provided by the present invention. Fig. 2 is a schematic diagram showing the structure of a sensing device of the robot state sensing system of Fig. 1. Figure 3 is a schematic diagram showing the rotational relationship between the active rectangular coordinate system and the initial reference Cartesian coordinate system. [Main component symbol description] Robot state sensing system 2 Sensing device 22 Platform 220 Accelerometer 221 X-axis acceleration sensor 221a Y-axis acceleration sensor 221b Z-axis acceleration sensor 221c Gyro 222 X-axis angular acceleration sensing 222a Y-axis angular acceleration sensor 222b 17 201015457

Z軸角加速度感測器 222c 資料接收裝置 24 尚通滤波器 241 低通滤波器 242 > 243 資料處理器 25 狀態判斷裝置 26 執行裝置 28 警報器 281 定位裝置 210 初始*** 2101 輔助*** 2102 數碼標籤 2103 誤差校正裝置 212 讀取器 2120 18Z-axis angular acceleration sensor 222c data receiving device 24 UPS filter 241 low-pass filter 242 > 243 data processor 25 state determining device 26 performing device 28 alarm 281 positioning device 210 initial locator 2101 auxiliary locator 2102 Digital tag 2103 error correcting device 212 reader 2120 18

Claims

201015457 · 十、申請專利範圍： 1 · 一種機器人狀態感知系統，其包括：感測裝置，用於測量機器人運動之加速度訊號和角速度訊號；資料接收裝置，與所述感測裝置連接，用於對該感測裝置測得之加速度訊號和角速度訊號進行濾波和數位化處理；資料處理器，與所述資料接收裝置連接，用於根據該資料接收裝置濾波和數位化處理後之加速度訊號和角速度訊號 ❹計算機器人運動之加速度、角速度和位移；狀態判斷裝置，與所述資料接收裝置和資料處理器連接，用於根據該資料接收裝置濾波和數位化處理後之加速度訊號和該資料處理器計算出之加速度判斷機器人所處之運動狀態；執行裝置，與所述狀態判斷裝置連接，用於根據機器人所處之狀態發出相對應之警報聲， ❹其中，所述機器人狀態感知系統進一步包括：定位裝置，其沿機器人運動路徑按均等間距分佈，所述定位裝置上設置有數位標籤，該數位標籤上之内容為與該定位裝置所處位置相關之校正資訊；、誤差校正裝置，其與所述資料處理器相連接，用於讀取該數=標籤上之校正資訊並根據該校正資訊對該資料處理= 計算出來之機器人運動之位移進行校正。 2.如申請專利範圍第1項所述之機器人狀態感知系統，其中’該資料接收裝置包括-截止頻率為1G赫兹之高通遽波 19 201015457 器、一截止頻率為12.5赫茲之低通濾波器及一截止頻率為 50赫兹之低通濾波器，該高通濾波器和低通濾波器對感測裝置測得之加速度類比訊號和角速度類比訊號進行濾波處理並分別轉換為加速度數位訊號和角速度數位訊號。 3·如申請專利範圍第1項所述之機器人狀態感知系統，其中，該感測裝置包括一加速度計和一陀螺儀，該加速度計包括感測軸方向相互垂直之三個加速度感測器，該陀螺儀 ❹包括感測轴方向與該加速度感測器之感測軸方向相對應之二個角速度感測器，以該加速度感測器和角速度感測器之感測軸方向為坐標軸定義一感測活動直角坐標系，將機器人初始狀態時之感測活動直角坐標系定義為起始參考直角坐標系。 4·如申請專利範圍第3項所述之機器人狀態感知系統，其中，該資料處理器根據經截止頻率為5〇赫茲之低通濾波器處理後之角速度數位訊號計算出該感測活動直角坐標系與 ©起始參考直角坐標系之間之座標轉換矩陣。 5·如申請專利範圍第4項所述之機器人狀態感知系統，其中，該資料處理器根據經截止頻率為12·5赫茲之低通濾波器處理後之加速度數位訊號和該感測活動直角坐標系與起始參考直角坐標系之間之座標轉換矩陣計算出機器人相對於起始參考直角坐標系之加速度。 6.如申請專利範圍第5項所述之機器人狀態感知系統，其中，該狀態判斷裝置根據機器人相對於起始參考直角坐標系之加速度與該起始參考直角坐標系三個坐標軸之間之傾 20 201015457 斜角以判斷機器人是否處於傾斜狀態。 .7·如申請專利範圍第6項所述之機器人狀態感知系統，其中’如果該狀態判斷裝置判斷機器人處於傾斜狀態則向該執行裝置發出傾斜警報指令，該執行裝置接收到該傾斜警報指令後發出傾斜警報聲。 8. 如申請專利範圍第5項所述之機器人狀態感知系統，其中’該資料處理器通過對該機器人相對於起始參考直角坐標系之加速度進行積分運算得到該機器人相對於起始參考直角坐標系之速度。 9. 如申請專利範圍第8項所述之機器人狀態感知系統，其中，該資料處理器通過對該機器人相對於起始參考直角坐標系之速度進行積分運算得到該機器人相對於起始參考直角坐標系之位移。 10·如申請專利範圍第2項所述之機器人狀態感知系統，其中，該狀態判斷裝置檢測經截止頻率為10赫茲之高通濾 ❿波器處理後之加速度數位訊號，若檢測到持續時間超過一預设搖晃時間閾值且加速度值呈正負相間變化之加速度數位訊號時’該狀態判斷裝置判斷機器人處於被搖晃之狀ς。 η.如中請專利範圍第1G項所述之機器人狀態感知祕，其中，如果該狀態判斷裝置判斷機器人處於被搖晃之狀離則向該執行裝置發出搖晃警報指令，該執行装置在接收= 搖晃警報指令後發出搖晃警報聲。 12.如申請專利範圍第1 其中，該定位装置包括一項所述之機器人狀初始***和至少一個輔助定位 21 201015457 器’該初始***設置於機人進行初妒+ ' ff之運動起點，用於對機器益人運動路徑按均等間距分佈。始疋位器開始/口機 L3中如t請專利範圍第12項所述之機器人狀態感知系統， 2 ’母—個該輔較位器相對於初始***之位移定義 :又正位移’該每—個辅助***上均設置有—與該校正位移相對應之數位標籤。201015457 · X. Patent application scope: 1 · A robot state sensing system, comprising: a sensing device for measuring an acceleration signal and an angular velocity signal of a robot motion; and a data receiving device connected to the sensing device for The acceleration signal and the angular velocity signal measured by the sensing device are filtered and digitized; and the data processor is connected to the data receiving device for the acceleration signal and the angular velocity signal after filtering and digitizing the data receiving device. ❹ calculating the acceleration, angular velocity and displacement of the robot motion; the state judging device is connected to the data receiving device and the data processor, and is configured to calculate the acceleration signal after filtering and digitizing the data receiving device and the data processor The acceleration device determines the motion state of the robot; the executing device is coupled to the state determining device for emitting a corresponding alarm sound according to the state of the robot, wherein the robot state sensing system further comprises: a positioning device Along the machine The human motion paths are distributed at equal intervals, and the positioning device is provided with a digital label, the content of the digital label is correction information related to the position of the positioning device; and an error correction device, which is associated with the data processor The connection is used to read the correction information on the number=tag and correct the displacement of the calculated robot motion according to the correction information. 2. The robot state sensing system according to claim 1, wherein the data receiving device comprises: a high pass chopper 19 201015457 with a cutoff frequency of 1 GHz, a low pass filter with a cutoff frequency of 12.5 Hz and A low-pass filter with a cut-off frequency of 50 Hz, the high-pass filter and the low-pass filter filter the acceleration analog signal and the angular velocity analog signal measured by the sensing device and convert them into an acceleration digital signal and an angular velocity digital signal, respectively. 3. The robot state sensing system of claim 1, wherein the sensing device comprises an accelerometer and a gyroscope, the accelerometer comprising three accelerometers sensing the direction of the axes perpendicular to each other. The gyroscope includes two angular velocity sensors whose sensing axis direction corresponds to the sensing axis direction of the acceleration sensor, and the sensing axis direction of the acceleration sensor and the angular velocity sensor is defined as an axis A sensing rectangular coordinate system defines the sensing active rectangular coordinate system in the initial state of the robot as the starting reference rectangular coordinate system. 4. The robot state sensing system according to claim 3, wherein the data processor calculates the sensing active rectangular coordinate according to the angular velocity digital signal processed by the low pass filter with a cutoff frequency of 5 Hz. The coordinate transformation matrix between the system and the starting reference Cartesian coordinate system. 5. The robot state sensing system of claim 4, wherein the data processor is based on an acceleration digital signal processed by a low pass filter having a cutoff frequency of 12·5 Hz and the sensed active rectangular coordinate The acceleration of the robot relative to the starting reference Cartesian coordinate system is calculated from the coordinate transformation matrix between the starting reference Cartesian coordinate system. 6. The robot state sensing system according to claim 5, wherein the state judging means is based on an acceleration between the robot relative to the initial reference Cartesian coordinate system and the three coordinate axes of the initial reference Cartesian coordinate system. Tilt 20 201015457 Bevel to determine if the robot is tilted. The robot state sensing system according to claim 6, wherein if the state judging device judges that the robot is in a tilt state, a tilting alarm command is issued to the executing device, and the executing device receives the tilting alarm command A tilting alarm sounds. 8. The robot state sensing system according to claim 5, wherein the data processor obtains the robot relative to the starting reference rectangular coordinate by integrating the acceleration of the robot with respect to the initial reference Cartesian coordinate system. The speed of the system. 9. The robot state sensing system of claim 8, wherein the data processor obtains the robot relative to the starting reference Cartesian coordinates by integrating the speed of the robot relative to the starting reference Cartesian coordinate system. The displacement of the system. 10. The robot state sensing system according to claim 2, wherein the state determining means detects the acceleration digital signal processed by the high pass filter chopper having a cutoff frequency of 10 Hz, and if the detected duration exceeds one When the threshold value of the shaking time is preset and the acceleration value is an acceleration digital signal that changes between positive and negative phases, the state determining device determines that the robot is in a state of being shaken. η. The robot state sensing secret described in the scope of claim 1G, wherein if the state judging device judges that the robot is in a state of being shaken, a shaking alarm command is issued to the executing device, and the executing device is receiving = shaking A shaking alarm sounds after the alarm command. 12. The scope of claim 1 wherein the positioning device comprises a robot-like initial positioner and at least one auxiliary positioning 21 201015457 'the initial positioner is set to the start of the movement of the machine + 'ff It is used to distribute the machine's beneficial movement path at equal intervals. In the start of the starter/porter L3, please refer to the robot state sensing system described in item 12 of the patent scope, 2 'mother-the displacement of the auxiliary positioner relative to the initial positioner: positive displacement' Each of the auxiliary positioners is provided with a digital label corresponding to the corrected displacement.

14.如申請專利範圍第13項所述之機器人狀態感知系統，其中，該誤差校正裝置包括一讀取器，在機器人每次經過該輔助***時，該讀取器讀取與數位標籤相對應之校正位移資訊’並根據該校正位移校正由該資料處理器計算出來之機器人運動之位移。 15.如申請專利範圍第12項所述之機器人狀態感知系統，其中’該輔助***之個數取決於該感測裝置之精密度，若該感測裝置之精密度越高則在相同運動路徑内所需要之 ❹辅助***之個數越少。 22 201015457 Η•一、圖式：14. The robot state sensing system according to claim 13, wherein the error correcting device comprises a reader, and the reader reads the digital tag each time the robot passes the auxiliary positioner. Corresponding to correcting the displacement information' and correcting the displacement of the robot motion calculated by the data processor according to the corrected displacement. 15. The robot state sensing system according to claim 12, wherein the number of the auxiliary locator depends on the precision of the sensing device, and the higher the precision of the sensing device is in the same motion The fewer the number of auxiliary locators required in the path. 22 201015457 Η•一,图: