TW201015457A - Robot state inspecting system - Google Patents
Robot state inspecting system Download PDFInfo
- Publication number
- TW201015457A TW201015457A TW97138159A TW97138159A TW201015457A TW 201015457 A TW201015457 A TW 201015457A TW 97138159 A TW97138159 A TW 97138159A TW 97138159 A TW97138159 A TW 97138159A TW 201015457 A TW201015457 A TW 201015457A
- Authority
- TW
- Taiwan
- Prior art keywords
- robot
- state
- acceleration
- sensing
- data processor
- Prior art date
Links
Landscapes
- Manipulator (AREA)
Abstract
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 (1)
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW97138159A TW201015457A (en) | 2008-10-03 | 2008-10-03 | Robot state inspecting system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW97138159A TW201015457A (en) | 2008-10-03 | 2008-10-03 | Robot state inspecting system |
Publications (1)
Publication Number | Publication Date |
---|---|
TW201015457A true TW201015457A (en) | 2010-04-16 |
Family
ID=44830044
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
TW97138159A TW201015457A (en) | 2008-10-03 | 2008-10-03 | Robot state inspecting system |
Country Status (1)
Country | Link |
---|---|
TW (1) | TW201015457A (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108369092A (en) * | 2015-10-15 | 2018-08-03 | 微观调控物理光谱公司 | Method and system for the trueness error for compensating six sufficient instrument |
-
2008
- 2008-10-03 TW TW97138159A patent/TW201015457A/en unknown
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108369092A (en) * | 2015-10-15 | 2018-08-03 | 微观调控物理光谱公司 | Method and system for the trueness error for compensating six sufficient instrument |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US20060185432A1 (en) | Five degree of freedom intertial measurement device | |
US10521011B2 (en) | Calibration of inertial measurement units attached to arms of a user and to a head mounted device | |
KR101107538B1 (en) | Sensor-based orientation system | |
US7395181B2 (en) | Motion tracking system | |
US8010308B1 (en) | Inertial measurement system with self correction | |
CN103591955B (en) | Integrated navigation system | |
US10495456B2 (en) | Method for calibrating a detection device, and detection device | |
CN101685308A (en) | Robot state perception system | |
US7835878B2 (en) | Travel angle detection system for mobile object | |
WO2015048094A1 (en) | Directed registration of three-dimensional scan measurements using a sensor unit including an accelerometer and a gyroscope | |
JP2007040762A (en) | Optical gyro calibration system, robot equipped with optical gyro, and optical gyro calibration program | |
KR20090009172A (en) | Method and apparatus for measuring position of the mobile robot | |
WO2016198009A1 (en) | Heading checking method and apparatus | |
JP2012008096A (en) | Bias estimation method, attitude estimation method, bias estimation device and attitude estimation device | |
JP2012173190A (en) | Positioning system and positioning method | |
JP2013106768A (en) | State detecting device, electronic device, and program | |
EP3227634B1 (en) | Method and system for estimating relative angle between headings | |
Chang-Siu et al. | Time-varying complementary filtering for attitude estimation | |
JP5082001B2 (en) | Object direction detection method, position detection method, direction detection device, position detection device, movement dynamic recognition method, and movement dynamic recognition device | |
CN108801253A (en) | Robot builds figure positioning system and robot | |
Wang et al. | Extended kalman filtering for robot joint angle estimation using mems inertial sensors | |
CN203719666U (en) | Combined navigation system | |
TW201015457A (en) | Robot state inspecting system | |
JP2020160594A (en) | Self-position estimating method | |
JP2015135349A (en) | Bias estimation method, attitude estimation method, bias estimation device and attitude estimation device |