機(jī)器人機(jī)動(dòng)技術(shù)與運(yùn)動(dòng)控制

1、Motion Control & Locomotion of RobotMotion Control 一切形式的移動(dòng)式機(jī)器人運(yùn)行時(shí)，運(yùn)動(dòng)控制算法都必須考慮以下三個(gè)問(wèn)題：坐標(biāo)系如何確定？（機(jī)構(gòu)的局部坐標(biāo)系，全局坐標(biāo)，世界坐標(biāo)系）； -怎樣去觀察運(yùn)動(dòng)才是合理的，符合要求的？ -這與機(jī)器人運(yùn)動(dòng)形式和精度有關(guān)，如結(jié)構(gòu)場(chǎng)地的運(yùn)動(dòng)，旋翼飛行器在三維空間的運(yùn)動(dòng)；2. 當(dāng)前機(jī)器人處于什么位置，目標(biāo)點(diǎn)在哪里？ -Current Location 的 “定位” 。-機(jī)器人的定位。3. 以什么樣的方式，沿什么樣的路徑，向目標(biāo)點(diǎn)運(yùn)動(dòng)？-機(jī)器人的驅(qū)動(dòng)控制或?qū)Ш?。Motion Control 定位的目標(biāo)是：任何時(shí)

2、刻t下，機(jī)器人在全局場(chǎng)地（運(yùn)動(dòng)平面上）的坐標(biāo)值（Xc,Yc,c）。 若機(jī)器人是直線運(yùn)動(dòng)，根據(jù)v-t曲線，利用初等幾何或簡(jiǎn)單的微積分可以得到機(jī)器人的坐標(biāo)。這是一維運(yùn)動(dòng)的情況。 在平面上的運(yùn)動(dòng)：機(jī)器人在場(chǎng)地上的運(yùn)動(dòng)路徑可能是任意形狀，所以使用一種依賴于行走路徑的定位方法是不現(xiàn)實(shí)的，因?yàn)椴豢赡苁褂靡环N統(tǒng)一的路徑數(shù)學(xué)模型去描述所有可能的情況。但是如果使用微積分思想，把曲線看成是很多段微小直線組成，就可解決定位問(wèn)題。Motion ControlMotion ControlMotion Control SensorsEncoder 將幾何位移量轉(zhuǎn)換成脈沖信號(hào)的傳感器，是應(yīng)用廣泛的角位移傳感元件。 Enco

3、der由光柵盤(pán)和光電檢測(cè)元件組成。機(jī)器人結(jié)構(gòu)中，Encoder與被測(cè)軸同軸，電動(dòng)機(jī)旋轉(zhuǎn)時(shí)光柵盤(pán)與電機(jī)同步旋轉(zhuǎn)。光敏二極管等電子元件組成的檢測(cè)裝置檢測(cè)，輸出脈沖信號(hào)，原理如圖。 Encoder 輸出脈沖的數(shù)，反映當(dāng)前電機(jī)轉(zhuǎn)角。Motion Control Sensors關(guān)于運(yùn)動(dòng)的方向？ Encoder 輸出相位相差90的A/B 2個(gè)感光元件，產(chǎn)生A、B兩路信號(hào)，相位相差90。只對(duì)某一相信號(hào)進(jìn)行計(jì)數(shù)，計(jì)數(shù)結(jié)果反映轉(zhuǎn)動(dòng)圈數(shù)。 判斷其正轉(zhuǎn)還是反轉(zhuǎn)，須采用兩路正交信號(hào)信號(hào)。如正轉(zhuǎn)時(shí)，A的相位超前于B的相位90，那么反轉(zhuǎn)時(shí)，B的相位超前于A的相位90。據(jù)此對(duì)A相信號(hào)計(jì)數(shù)時(shí)，判斷B相信號(hào)處于高電平還是低電平

4、即可判斷編碼盤(pán)處于正轉(zhuǎn)狀態(tài)還是反轉(zhuǎn)狀態(tài)。 Encoder 有光學(xué)式、磁式、感應(yīng)式和電容式。Encoder 根據(jù)其刻度方法及信號(hào)輸出形式，可分為增量式、絕對(duì)式以及混合式3種。 Motion Control Sensors輸出倍頻（四倍頻的算法）數(shù)學(xué)上fA(t)和fB(t)可描述成（1（t)是階躍函數(shù)）：fA(t) =1kt- k(T/2)-1(kt- kT) fB(t) =1kt- k(T/2)- k(/2)-1kt- kT- k(/2) 用數(shù)字延遲電路使fA(t)和fB(t)產(chǎn)生時(shí)刻延遲: fA(t-) =1kt-k(T/2)-k-1(kt- kT- k)fB(t-) =1kt- k(T/2)

5、- k(/2)- k-1kt- kT- k(/2)- k 所以：fA(t)-fA(t-) =(kt - k)-(kt- kT - k) fB(t)-fB(t-) =kt- k(T/2)- k(/2) - k-kt- kT- k(/2) - k (A-)(B-)(A+)(B+)Motion Control SensorsGyroscope角動(dòng)量守恒：一個(gè)旋轉(zhuǎn)物體的旋轉(zhuǎn)軸所指的方向在不受外力影響時(shí)，是不會(huì)改變的。Gyroscope基于此原理來(lái)傳感/維持方向。 Gyroscope由位于軸心可旋轉(zhuǎn)的輪子構(gòu)成。 陀輪一旦開(kāi)始旋轉(zhuǎn)，由于陀輪的角動(dòng)量，陀螺儀有抗拒方向改變的趨向。讀取軸所指示的方向（檢測(cè)扭矩

6、），即可獲得角加速度。 Gyroscope多用于飛機(jī)導(dǎo)航、定位等系統(tǒng)，在移動(dòng)機(jī)器人中普遍使用。積分算法的累積誤差 Motion ControlFiltering of MEMS Gyroscope: Kalman濾波是“optimal recursive data processing algorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法）”。 可盡可能減少系統(tǒng)噪聲影響，從含有噪聲的測(cè)量值中得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì), 用在機(jī)器人導(dǎo)航和控制用來(lái)預(yù)估位姿。 Motion Control可得到系統(tǒng)的狀態(tài)方程和測(cè)量方程。 Motion ControlMotion ControlObstacle-avoiding

7、:輸出25KHZ的方波信號(hào)，經(jīng)放大后和線圈升壓，探頭發(fā)射。當(dāng)超聲波遇到障礙物反射后，接受頭接收回波，經(jīng)電容，電阻濾波后由兩個(gè)反向并聯(lián)的穩(wěn)壓管穩(wěn)壓，再經(jīng)四級(jí)放大后送回單片機(jī)供單片機(jī)處理。單片機(jī)記錄收發(fā)的時(shí)間T計(jì)算出當(dāng)前機(jī)器人距障礙物距離S=V*T/2。Motion Control：case從動(dòng)輪與Encoder的配合（14:35）：Motion Control：case2 另一個(gè)Encoder / Gyroscope應(yīng)用的移動(dòng)機(jī)器人例子(2009)MEMS Gyroscope: ADIS16365通過(guò)SPI協(xié)議與外部控制器實(shí)現(xiàn)數(shù)據(jù)和指令通信， ADIS16365擁有31個(gè)控制和數(shù)據(jù)寄存器。集成3

8、個(gè)慣性傳感器和3個(gè)角速度傳感器，帶自校準(zhǔn)功能。 角度分辨率： 0.0125/s/LSB (80/s) 典型帶寬 0.33KHz 動(dòng)態(tài)線性加速度補(bǔ)償因子： 0.05/sec/g 運(yùn)動(dòng)偏移穩(wěn)定性： 0.009/secMotion Control：case對(duì)震動(dòng)敏感，故盡量選用柔對(duì)震動(dòng)敏感，故盡量選用柔性安裝，或減震設(shè)計(jì)。性安裝，或減震設(shè)計(jì)。Motion Control: Motor關(guān)于驅(qū)動(dòng)：直流伺服系統(tǒng) + 減速機(jī), 適用于移動(dòng)機(jī)器人交流伺服系統(tǒng) + 減速機(jī)，需要逆變步進(jìn)電機(jī) + 減速機(jī)，價(jià)格便宜，性能較差。關(guān)于減速機(jī)，用于提升電機(jī)輸出扭矩：* 行星減速機(jī) * RV減速機(jī)：機(jī)器人關(guān)節(jié)最好的減速機(jī)

9、* 蝸輪蝸桿減速機(jī)Motion Control需要良好的機(jī)械傳動(dòng)系統(tǒng)保證運(yùn)動(dòng)的易控性：場(chǎng)地滾動(dòng)摩擦條件，機(jī)器人的速度加速度參數(shù)要求電機(jī) Rated speed, OUTPUT TorqueGear Box Rate, Input/OUTPUT Torque加/減速測(cè)試，最大速度測(cè)試，可控性測(cè)試，輪子的調(diào)校傳動(dòng)剛度校驗(yàn)Motion ControlThe first challenge of mobile robots is locomotion. Robots can move unsupervised through real world environments to fulfill its

10、 task. How should a mobile robot move? And what is it about a particular locomotion mechanism that make it superior to alternative ones when moving on the specific ground? Motion ControlWheeled mobile robots:By far the most popular locomotion mechanism in man-made vehicles in general. It can achieve

11、 very good efficiencies when moving on the structural ground. With a relatively simple mechanical implementation, and balance is usually not a research problem.Motion ControlWheeled mobile robots:Three wheels are sufficient to guarantee stable balance, two wheels robots can also be stable. When more

12、 than three wheels are used, a suspension system is required to allow all wheels to maintain ground contact when the robot encounters uneven terrain. Motion ControlSteered Wheel: A. Free steered wheel，two traction wheels, differential； B. steered traction wheel；Locomotion of the RobotThree-Wheeled m

13、obile robots:Locomotion of the RobotFour-Wheeled mobile robots:Motion Control2-Legs robots:SONY dream robot，Honda Asimo，MIT Spring FlamingoComplex mechanism & freedomsJoints used hereinDynamic balance control is hardMotion ControlStepped Climbing type:University of Karsruhe, Germany: six legs，MI

14、T: genghisMotion control can be used integrated algorithm, but error of legs structure will enlarges error of position.Motion ControlLegs robots:Numbers of legs: Insects and spiders are immediately able to walk when born. Humans,with 2 legs,can not stand in one palce with static stability.Infants re

15、quies months to study stand and walk.Stable balance.Motion ControlTracked locomotion robots:Robot can make use of tread have much larger ground contact patches,the maneuverability can be improved significantly in loose terrain compared to conventional wheeled designs.Due to large ground contact patc

16、h, changing the orientation of the robots requies a skidding turn, wherein a large portion of the track must slide against the terrain.The exact center of the rotation of the robot is hard to predict and the exact change in position and in orientation is also subject to variations depending on the g

17、round friction.Motion ControlTracked locomotion robots:The microrover Nanokhod: developed for European Space AgencyDesigned in 2007Motion ControlWalking wheels robots:Offer the best maneuverability in rough terrain, but inefficient on flat ground and need sophisticated controll. Hybrid solution, com

18、bining the adaptability of legs with the efficiency of the wheels, offer an interesting compromise. Solution that passively adapt to the terrain are of particular interest for field and space robotics.Motion ControlSwedish-Wheeled mobile robots:Motion ControlSwedish-Wheeled mobile robots:Motion Cont

19、rol structural ground and locomotion controlAnother case (22.27, 3120)Motion Controlstructural ground and Line-following controlPerception & Sensors: v matrix of Infrared diode;v Vision-based sensorv Color sensorv 2 casev http:/ ControlMotion Control 機(jī)動(dòng)的其他形式： 舵機(jī) 步進(jìn)爬行式 兩足步行式 噴氣動(dòng)量式（15:26）Navigatio

20、n Algorithmof Mobile RobotNavigating AlgorithmNavigating Algorithm 鑒于機(jī)器人任務(wù)和路徑的多樣性，場(chǎng)地條件復(fù)雜性，沒(méi)有固定的控制算法最適應(yīng)所有機(jī)器人。 在機(jī)器人算法設(shè)計(jì)中，根據(jù)實(shí)際的需求考慮算法，以達(dá)到機(jī)器人的較精確控制。 結(jié)構(gòu)性場(chǎng)地中，大量實(shí)驗(yàn)表明直線運(yùn)行用西格沃特+PID算法控制，彎道用西格沃特算法+勢(shì)場(chǎng)效應(yīng)法控制。 可設(shè)定一個(gè)綜合標(biāo)準(zhǔn)，以完成不同算法間的過(guò)渡和聯(lián)系，這樣以達(dá)到控制效果的連續(xù)性和系統(tǒng)的穩(wěn)定性。Navigating Algorithm西格沃特算法是一種點(diǎn)鎮(zhèn)定的路徑控制方法。所謂點(diǎn)鎮(zhèn)定，是一種路徑控制的方法，即為

21、已知兩終端的狀態(tài)，按一定的規(guī)律來(lái)實(shí)時(shí)生成參數(shù)以控制中間的狀態(tài)。通俗上說(shuō)就是終端已知，中段未知的控制方法。Navigating Algorithm 目標(biāo)位置點(diǎn) 紅色代表期望路徑線 當(dāng)前位置點(diǎn) 藍(lán)色代表實(shí)際控制路線Navigating AlgorithmNavigating Algorithm設(shè)計(jì)控制量v，w（線速度，角速度），以把它從實(shí)際位置以曲線驅(qū)動(dòng)到目標(biāo)位置。 機(jī)器人參考框架XR，YR, ，和全局框架X，Y, ?？梢栽O(shè)計(jì)如下控制策略： V = k W = k + k 式中k，k，k為控制參數(shù)；，表示在（, ）中； Navigating Algorithm控制參數(shù)的穩(wěn)定性：運(yùn)動(dòng)路徑依賴于參數(shù)k

22、，k，k，要使機(jī)器人路徑收斂于目標(biāo)點(diǎn)，可證明參數(shù)需滿足： k 0， k 0， k k 0 為魯棒位置控制，用強(qiáng)穩(wěn)定性條件，保證在機(jī)器人到達(dá)目標(biāo)時(shí)，不改變方向： k 0， k 0， k+5/3 k 2/k 0 Navigating AlgorithmNavigating AlgorithmPID導(dǎo)航控制算法是一種路徑實(shí)時(shí)控制的過(guò)程控制方法，控制過(guò)程中，按實(shí)時(shí)偏差的比例（P）、積分（I）和微分（D）進(jìn)行相應(yīng)的參數(shù)選擇，來(lái)完成過(guò)程的精確調(diào)整和控制。其中，P參數(shù)使系統(tǒng)保持與預(yù)定相符的運(yùn)動(dòng)趨勢(shì)，I參數(shù)使系統(tǒng)穩(wěn)態(tài)誤差逐漸消除，D參數(shù)調(diào)節(jié)運(yùn)動(dòng)的快速性，但會(huì)加劇系統(tǒng)振蕩。合適的P、I、D參數(shù)可以使系統(tǒng)在預(yù)定的

23、運(yùn)動(dòng)趨勢(shì)上逐漸靠攏預(yù)定的軌跡，這需要根據(jù)場(chǎng)地條件不同而改變，做大量的實(shí)驗(yàn)。Navigating Algorithm 目標(biāo)位置點(diǎn) 當(dāng)前位置點(diǎn) 紅色代表期望路徑線 藍(lán)色代表實(shí)際控制路徑線Navigating Algorithm 控制機(jī)器人以直線行走，假設(shè)機(jī)器人在全局坐標(biāo)系中的位置為（x，y），目標(biāo)坐標(biāo)為（x0，y0），兩點(diǎn)可確定一條直線，設(shè)直線方程為AX + BY + C = 0， 系數(shù)A,B,C可以由下式得出： A = y0- y; B = x x0; C = x0*y x*y0; Navigating Algorithm要驅(qū)動(dòng)機(jī)器人沿此直線運(yùn)動(dòng)，假設(shè)機(jī)器人沿此直線運(yùn)動(dòng)的過(guò)程中坐標(biāo)為（X,Y）,

24、由于控制有誤差，（X,Y）不是精確在此直線上，那么可以求出點(diǎn)（X,Y）到直線的距離，也就是運(yùn)動(dòng)的誤差: ERROR = (AX+BY+C)/E E = sqrt( A*A + B*B); Navigating Algorithm采用增量PID算法控制此誤差. 偏差量 e(k)=Error= (AX+BY+C)/E； e(k) = e(k) - e(k-1)； e(k-1) = e(k-1) - e(k-2)； 設(shè)u(k) = u(k-1) + kp*e(k) + ki*e(k)+ kd*(e(k) - e(k-1)； 其中u(k)為輸出控制量，kp為比例系數(shù)，ki為積分系數(shù)，kd為微分系數(shù)。N

25、avigating Algorithm PID的控制具體行為如下： 給左右兩輪一恒定的線速度v，根據(jù)在運(yùn)動(dòng)過(guò)程中的機(jī)器人坐標(biāo)，計(jì)算出誤差。 通過(guò)PID計(jì)算出控制量u（k），控制機(jī)器人轉(zhuǎn)向角速度w，使機(jī)器人能夠在有干擾的情況下沿預(yù)定直線運(yùn)動(dòng)到目標(biāo)點(diǎn)。 Navigating Algorithm復(fù)雜路徑的綜合控制策略 將機(jī)器人從任意位置任意方向驅(qū)動(dòng)到目標(biāo)點(diǎn)，首先將機(jī)器人以較大的轉(zhuǎn)向速度和較小的前進(jìn)速度，轉(zhuǎn)向正對(duì)目標(biāo)點(diǎn)，再以較大的前進(jìn)速度和較小的轉(zhuǎn)向速度，到達(dá)目標(biāo)點(diǎn)的可接受范圍。 在點(diǎn)鎮(zhèn)定的前段路徑，以西格沃特算法控制，在點(diǎn)鎮(zhèn)定的后段路徑，以強(qiáng)PID加弱反饋的算法進(jìn)行控制。Navigating AlgorithmEPOS電機(jī)（ARM內(nèi)核） 工控板驅(qū)動(dòng)輪場(chǎng)地路徑定位系統(tǒng)UART通信CAN總線錐齒傳動(dòng)被動(dòng)小輪測(cè)量反饋加載電流輸出控制算法c/os-A case: NavigatingA