本發(fā)明屬于機(jī)器人控制器相關(guān)領(lǐng)域，尤其涉及一種關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制系統(tǒng)及控制方法。

背景技術(shù)：

在基于運(yùn)動(dòng)學(xué)控制方法的關(guān)節(jié)型機(jī)械臂設(shè)計(jì)領(lǐng)域中主要有（1）cpac機(jī)器人控制系統(tǒng)、（2）crp-s80工業(yè)機(jī)器人控制系統(tǒng)以及（3）kemotion機(jī)器人控制解決方案；在驅(qū)控一體化控制系統(tǒng)設(shè)計(jì)領(lǐng)域中主要有（4）驅(qū)控一體化控制器和控制系統(tǒng)、（5）開(kāi)放式驅(qū)控一體化的工業(yè)機(jī)器人控制器研發(fā)以及（6）眾為興qc驅(qū)控一體系統(tǒng)。

對(duì)于（1）中方案所述的機(jī)器人控制系統(tǒng)，關(guān)鍵技術(shù)在于逆運(yùn)動(dòng)學(xué)路徑規(guī)劃解算技術(shù)的實(shí)現(xiàn)、奇異點(diǎn)避免方法和操作系統(tǒng)實(shí)時(shí)性設(shè)計(jì)，該方案逆運(yùn)動(dòng)學(xué)路徑規(guī)劃結(jié)算主要采用幾何方法求解，奇異點(diǎn)避免方法的實(shí)現(xiàn)通過(guò)繞奇異點(diǎn)轉(zhuǎn)動(dòng)機(jī)制實(shí)現(xiàn)，實(shí)時(shí)性設(shè)計(jì)主要通過(guò)將wince操作系統(tǒng)進(jìn)行內(nèi)核任務(wù)調(diào)度機(jī)制進(jìn)行優(yōu)化與改造進(jìn)行實(shí)現(xiàn)。對(duì)于（2）中方案所述的機(jī)器人控制系統(tǒng)，關(guān)鍵技術(shù)在于不同構(gòu)型機(jī)器人運(yùn)動(dòng)學(xué)求解過(guò)程的歸一化求解，該方案針對(duì)不同構(gòu)型的機(jī)器人分別設(shè)計(jì)運(yùn)動(dòng)學(xué)控制算法，然后將設(shè)計(jì)的運(yùn)動(dòng)學(xué)算法進(jìn)行分析、比較，對(duì)相同模塊進(jìn)行歸一化處理，最后通過(guò)優(yōu)化技術(shù)進(jìn)行總體整合，使用時(shí)再通過(guò)特殊設(shè)置進(jìn)行構(gòu)型配置。對(duì)于（3）中方案所屬的機(jī)器人控制系統(tǒng)，關(guān)鍵技術(shù)在于將機(jī)器人運(yùn)動(dòng)學(xué)控制技術(shù)與應(yīng)用工藝進(jìn)行了深度整合和優(yōu)化，在傳統(tǒng)機(jī)器人運(yùn)動(dòng)學(xué)技術(shù)的基礎(chǔ)上，結(jié)合搬運(yùn)、碼垛、焊接等應(yīng)用的特殊要求，進(jìn)行專(zhuān)門(mén)的工藝特殊設(shè)計(jì)，并以此為優(yōu)化機(jī)制進(jìn)行運(yùn)動(dòng)學(xué)控制設(shè)計(jì)。對(duì)于（4）中方案所屬的機(jī)器人控制系統(tǒng)，關(guān)鍵技術(shù)主要包括控制器接口設(shè)計(jì)、應(yīng)用層操作系統(tǒng)設(shè)計(jì)、應(yīng)用程序和人機(jī)交互設(shè)計(jì)、運(yùn)動(dòng)控制卡設(shè)計(jì)及用于io控制的可編程邏輯控制器。對(duì)于（5）中方案所述的機(jī)器人控制系統(tǒng)，關(guān)鍵技術(shù)在于將常規(guī)運(yùn)動(dòng)控制器、常規(guī)驅(qū)動(dòng)器的控制模塊、工控機(jī)、安全控制卡、示教盒的cpu處理模塊有機(jī)集成，形成一個(gè)一體化的運(yùn)動(dòng)控制器，并將其中運(yùn)動(dòng)控制接口進(jìn)行封裝開(kāi)放，使用戶(hù)可以進(jìn)行二次開(kāi)發(fā)。對(duì)于（6）中所述的機(jī)器人控制系統(tǒng)，關(guān)鍵技術(shù)在于通過(guò)分片式嵌入式設(shè)計(jì)實(shí)現(xiàn)了機(jī)器人控制系統(tǒng)邏輯部分功能、人機(jī)交互功能、電機(jī)實(shí)時(shí)運(yùn)動(dòng)控制功能、機(jī)器人安全控制功能和io控制功能。

對(duì)于（1）~（6）所述的方案，皆存在成本過(guò)高、實(shí)時(shí)性解決不徹底、使用時(shí)需要二次集成不能直接使用的缺點(diǎn)；有的還有奇異點(diǎn)問(wèn)題解決耗時(shí)太長(zhǎng)，驅(qū)控一體設(shè)計(jì)方案不完整、封裝后的二次開(kāi)發(fā)接口復(fù)雜使用技術(shù)門(mén)檻高，系統(tǒng)硬件過(guò)于復(fù)雜、穩(wěn)定性有待提高、負(fù)載功率有限的缺點(diǎn)。

導(dǎo)致（1）所述方案在于系統(tǒng)主控制器使用intel架構(gòu)工控機(jī)成本較高，操作系統(tǒng)wince內(nèi)核改造只能保證軟實(shí)時(shí)，該操作系統(tǒng)架構(gòu)設(shè)計(jì)時(shí)根本不存在硬實(shí)時(shí)控制機(jī)制，奇異點(diǎn)解決算法過(guò)于單一，所設(shè)計(jì)方案產(chǎn)品為機(jī)器人控制系統(tǒng)主控制器，沒(méi)有將驅(qū)動(dòng)部分和配件設(shè)計(jì)入內(nèi)，因此需要二次集成到控制柜中才能在產(chǎn)業(yè)中進(jìn)行實(shí)際應(yīng)用。導(dǎo)致（2）和（3）所述方案缺點(diǎn)的原因在于主控制器使用intel架構(gòu)工控機(jī)成本較高，操作系統(tǒng)直接使用windows系統(tǒng)沒(méi)有考慮實(shí)時(shí)性設(shè)計(jì)，所設(shè)計(jì)方案產(chǎn)品為機(jī)器人控制系統(tǒng)主控制器，沒(méi)有將驅(qū)動(dòng)部分和配件設(shè)計(jì)入內(nèi)，因此需要二次集成到控制柜中才能在產(chǎn)業(yè)中進(jìn)行實(shí)際應(yīng)用。導(dǎo)致（4）所述方案缺點(diǎn)的原因在于主控制器使用intel架構(gòu)工控機(jī)成本較高，操作系統(tǒng)存在版權(quán)成本，驅(qū)控一體化方案中只設(shè)計(jì)了傳統(tǒng)伺服驅(qū)動(dòng)器的控制部分而未設(shè)計(jì)電源模塊和放大模塊，所設(shè)計(jì)方案產(chǎn)品為機(jī)器人控制系統(tǒng)主控制器，沒(méi)有將驅(qū)動(dòng)部分和配件設(shè)計(jì)入內(nèi)，因此需要二次集成到控制柜中才能在產(chǎn)業(yè)中進(jìn)行實(shí)際應(yīng)用。導(dǎo)致（5）所屬方案缺點(diǎn)的原因在于主控制器使用intel架構(gòu)工控機(jī)成本較高，操作系統(tǒng)存在版權(quán)成本，驅(qū)控一體化方案中只設(shè)計(jì)了傳統(tǒng)伺服驅(qū)動(dòng)器的控制部分而未設(shè)計(jì)電源模塊和放大模塊，所涉及的封裝后開(kāi)放接口屬于高級(jí)軟件開(kāi)發(fā)范疇，對(duì)設(shè)計(jì)人員技術(shù)要求較高，所設(shè)計(jì)方案產(chǎn)品為機(jī)器人控制系統(tǒng)主控制器，沒(méi)有將驅(qū)動(dòng)部分和配件設(shè)計(jì)入內(nèi)，因此需要二次集成到控制柜中才能在產(chǎn)業(yè)中進(jìn)行實(shí)際應(yīng)用。導(dǎo)致（6）所屬方案缺點(diǎn)的原因在于使用了多片嵌入式控制芯片，據(jù)不完全分析，該系統(tǒng)至少使用了1片arm芯片、2片fpga芯片、9片dsp芯片，造成系統(tǒng)過(guò)于復(fù)雜，穩(wěn)定性問(wèn)題也隨之浮現(xiàn)，主控系統(tǒng)復(fù)雜導(dǎo)致功耗過(guò)大，進(jìn)而使得電源和伺服放大模塊功率受到限制，該控制系統(tǒng)的封裝采用盒式封裝，現(xiàn)場(chǎng)抗干擾能力有限，擴(kuò)展后的外部配件不好固定安裝，因此需要二次集成為控制柜才能在實(shí)際產(chǎn)線(xiàn)中使用。

技術(shù)實(shí)現(xiàn)要素：

為了解決上述問(wèn)題，本發(fā)明提出了一種關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制系統(tǒng)，其特征在于，包括主控板、信號(hào)集成模塊、ipm驅(qū)動(dòng)放大模塊以及強(qiáng)電管理電路；主控板和信號(hào)集成板之間通過(guò)spi總線(xiàn)進(jìn)行通信連接，信號(hào)集成模塊通過(guò)其ipm接口與ipm驅(qū)動(dòng)放大模塊連接，信號(hào)集成模塊通過(guò)其強(qiáng)電控制接口與強(qiáng)電管理電路連接；

主控板包括master-fpga、arm-dsp雙核處理器、擴(kuò)展dram、sd卡、隔離io、網(wǎng)絡(luò)總線(xiàn)模塊；主控板的master-fpga的擴(kuò)展集成板接口、隔離io、sd卡、擴(kuò)展dram分別采集相關(guān)信息后，交由arm-dsp雙核處理器進(jìn)行處理，產(chǎn)生控制信息，控制信息通過(guò)master-fpga擴(kuò)展的集成板接口發(fā)送到信號(hào)集成板，并寫(xiě)入sd卡和擴(kuò)展dram，同時(shí)操作主控板控制io動(dòng)作，發(fā)送網(wǎng)絡(luò)總線(xiàn)指令至網(wǎng)絡(luò)總線(xiàn)模塊；

信號(hào)集成板上包括slave-fpga接口管理、ipm接口和隔離模塊，所述隔離模塊包括編碼器信號(hào)隔離、抱閘電路隔離、強(qiáng)電模塊監(jiān)控隔離、通信隔離、pc調(diào)試串行口隔離，各隔離模塊分別與slave-fpga連接；pc調(diào)試串行口隔離連接示教器或pc端；ipm接口連接六路電機(jī)控制信號(hào)線(xiàn)，抱閘電路隔離連接電機(jī)抱閘線(xiàn)，編碼器信號(hào)隔離連接電機(jī)編碼器反饋信號(hào)線(xiàn)。

所述強(qiáng)電管理電路主要完成電源整流、過(guò)壓過(guò)流保護(hù)、大電容充放電管理、再生電阻溫度管理。

所述master-fpga還用于為運(yùn)動(dòng)控制多路電流環(huán)dq電流分解使用、編碼器信號(hào)接收后的crc校驗(yàn)和旨在速度檢測(cè)平滑的快速移動(dòng)窗口平均、生成4路擴(kuò)展pwm發(fā)生器、電流采樣檢測(cè)三階降噪濾波器；

所述arm-dsp雙核處理器中的dsp模塊主要完成系統(tǒng)的路徑規(guī)劃和電機(jī)實(shí)時(shí)pid控制算法以及相應(yīng)的快速和平滑處理，arm-dsp雙核處理器中的arm模塊主要完成系統(tǒng)的狀態(tài)監(jiān)測(cè)、信號(hào)管理和任務(wù)調(diào)度；

所述信號(hào)集成板用于在主控板統(tǒng)一時(shí)鐘下各接口的信號(hào)采集、隔離和管理，具體包括ipm接口的電流采集、異常信號(hào)采集、電機(jī)控制信號(hào)發(fā)送，抱閘電路接口的信號(hào)控制管理，強(qiáng)電控制接口的信息采集和控制管理，擴(kuò)展io的隔離、信號(hào)讀取、控制管理，rs485通信接口的隔離、信號(hào)讀取、信號(hào)發(fā)送，編碼器接口的隔離、信號(hào)讀取、信號(hào)發(fā)送等。

一種關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制系統(tǒng)的控制方法，所述方法包括任務(wù)示教、路徑提取、奇異點(diǎn)處理、運(yùn)動(dòng)學(xué)路徑規(guī)劃、路徑平滑，完成運(yùn)動(dòng)學(xué)路徑規(guī)劃和路徑平滑后將大規(guī)模路徑規(guī)劃數(shù)據(jù)轉(zhuǎn)為實(shí)時(shí)運(yùn)動(dòng)控制部分參考數(shù)據(jù)，傳遞至各個(gè)軸的位置環(huán)、速度環(huán)、電流環(huán)伺服電機(jī)實(shí)時(shí)運(yùn)動(dòng)控制模塊。

所述路徑提取首先要做的是任務(wù)路徑描述，任務(wù)路徑描述采用標(biāo)準(zhǔn)最小指令集格式進(jìn)行；然后是任務(wù)描述解析，任務(wù)描述解析分為兩部分：一部分為將提取的任務(wù)采用二進(jìn)制編碼發(fā)送到arm-dsp雙核處理器，arm-dsp雙核處理器會(huì)周期性的將機(jī)器人運(yùn)行狀態(tài)發(fā)送至任務(wù)示教端，按照二進(jìn)制譯碼后將該信息賦于最小指令集格式電機(jī)位置上進(jìn)行顯示；另一部分為將提取的任務(wù)以及當(dāng)前機(jī)器人狀態(tài)在示教器上進(jìn)行顯示，該部分采用ascii編碼，按照標(biāo)準(zhǔn)最小指令集格式進(jìn)行顯示；提取后的任務(wù)文件直接以二進(jìn)制格式保存于sd卡中，sd卡信息能復(fù)制到其他機(jī)器人中直接使用。

所述標(biāo)準(zhǔn)最小指令集格式為從左向右依次為指令id、指令類(lèi)型、插補(bǔ)類(lèi)型、加速度、平滑類(lèi)型、速度單位、精度類(lèi)型、斷點(diǎn)類(lèi)型、工具id以及各電機(jī)位置。

所述奇異點(diǎn)處理采用人機(jī)交互式，其過(guò)程為奇異點(diǎn)檢測(cè)、奇異點(diǎn)報(bào)警及操作者交互和奇異點(diǎn)躲避；

所述奇異點(diǎn)檢測(cè)首先檢測(cè)路徑的位置奇異信息，然后基于位置奇異信息檢測(cè)速度奇異信息，最后基于速度奇異信息檢測(cè)加速度奇異信息，

所述奇異點(diǎn)報(bào)警及操作者交互即檢測(cè)到奇異信息后，控制相關(guān)io發(fā)出報(bào)警燈信號(hào)和報(bào)警聲音信號(hào)，并在示教裝置上進(jìn)行顯示，由操作者根據(jù)任務(wù)特點(diǎn)選擇奇異點(diǎn)躲避方法。

所述奇異點(diǎn)躲避方法分為三種，分別為奇異路徑重新制定、有限步奇異點(diǎn)躲避和實(shí)時(shí)監(jiān)督奇異點(diǎn)躲避，奇異路徑重新制定是指放棄當(dāng)前遇到奇異點(diǎn)的路徑重新制定路徑避免奇異點(diǎn)，有限步奇異點(diǎn)躲避是指控制機(jī)器人每個(gè)軸轉(zhuǎn)過(guò)特定距離的步長(zhǎng)使當(dāng)前位置偏離奇異點(diǎn)，實(shí)時(shí)監(jiān)督奇異點(diǎn)躲避是指各個(gè)軸每次轉(zhuǎn)非常小的步長(zhǎng)并實(shí)時(shí)反饋各軸位置信息自動(dòng)判斷是否已經(jīng)偏離了奇異點(diǎn)，如果已經(jīng)偏離，則躲避完成。

所述運(yùn)動(dòng)學(xué)路徑規(guī)劃包括首先提取任務(wù)點(diǎn)的起點(diǎn)和終點(diǎn)位置、速度這些運(yùn)動(dòng)約束，然后進(jìn)行路徑規(guī)劃插補(bǔ)方式選擇；

所述插補(bǔ)方式分為軸插補(bǔ)、直線(xiàn)插補(bǔ)和圓弧插補(bǔ)，軸插補(bǔ)軌跡生成流程包括多軸同步s曲線(xiàn)路徑生成和7段變速時(shí)間劃分機(jī)制，7段變速包括加加速、勻加速、減加速、勻速、加減速、勻減速和減減速；直線(xiàn)插補(bǔ)軌跡生成流程包括正運(yùn)動(dòng)學(xué)位資求解、變步長(zhǎng)路徑離散、逆運(yùn)動(dòng)學(xué)求解、加權(quán)最小范數(shù)優(yōu)化；圓弧插補(bǔ)需要由任務(wù)起點(diǎn)、終點(diǎn)和中間點(diǎn)三個(gè)點(diǎn)構(gòu)成圓弧曲線(xiàn)，然后進(jìn)行變步長(zhǎng)路徑離散、逆運(yùn)動(dòng)學(xué)求解和加權(quán)最小范數(shù)優(yōu)化步驟。

所述大規(guī)模路徑規(guī)劃數(shù)據(jù)轉(zhuǎn)為實(shí)時(shí)運(yùn)動(dòng)控制部分參考數(shù)據(jù)的過(guò)程為arm-dsp雙核處理器中的dsp模塊完成大規(guī)模路徑規(guī)劃數(shù)據(jù)后首先以較慢速度寫(xiě)入eeprom中，然后在arm-dsp雙核處理器中的arm模塊控制下eeprom當(dāng)前數(shù)據(jù)交替快速寫(xiě)入提前分成兩頁(yè)的專(zhuān)用ram中，eeprom數(shù)據(jù)讀取首地址由arm模塊給出，eeprom數(shù)據(jù)讀取量由單頁(yè)ram存儲(chǔ)量決定，與此同時(shí)，dsp模塊轉(zhuǎn)入實(shí)時(shí)控制流程，錯(cuò)時(shí)訪問(wèn)已經(jīng)完成寫(xiě)入的ram頁(yè)數(shù)據(jù)作為參考位置進(jìn)行單軸電機(jī)實(shí)時(shí)控制。

有益效果

本發(fā)明利用一片集成式arm-dsp芯片完成機(jī)器人控制系統(tǒng)主控和電機(jī)運(yùn)動(dòng)控制，利用兩片fpga實(shí)現(xiàn)外部接口通信、電源控制、電流環(huán)設(shè)計(jì)，利用一片dsp芯片分時(shí)控制最多7路伺服電機(jī)；在本發(fā)明中arm和dsp芯片間通信數(shù)據(jù)交互采用內(nèi)存訪問(wèn)的方式，使得數(shù)據(jù)交互速度達(dá)到最快，采用無(wú)操作系統(tǒng)方案和中斷硬實(shí)時(shí)方案，使得系統(tǒng)的實(shí)時(shí)性得到最大程度的提高，在關(guān)節(jié)空間內(nèi)對(duì)笛卡爾空間運(yùn)動(dòng)學(xué)計(jì)算結(jié)果進(jìn)行二次優(yōu)化，保證電機(jī)運(yùn)動(dòng)平滑性，采用軟硬件并行處理機(jī)制，設(shè)計(jì)不間斷數(shù)據(jù)“交互-使用-重新填充”流程，保證系統(tǒng)數(shù)據(jù)交互與使用互不干擾。

附圖說(shuō)明

圖1為關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制系統(tǒng)架構(gòu)框圖。

圖2為關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制系統(tǒng)硬件結(jié)構(gòu)圖。

圖3為圖3關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制系統(tǒng)軟件架構(gòu)圖。

圖4為多路電機(jī)串并行混合控制機(jī)制圖。

圖5為多路電機(jī)同步控制時(shí)序圖。

圖6為關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制方法流程圖。

圖7為任務(wù)描述標(biāo)準(zhǔn)最小指令集格式圖。

圖8為人機(jī)交互式奇異點(diǎn)處理流程圖。

圖9為運(yùn)動(dòng)學(xué)路徑規(guī)劃流程圖。

圖10為eeprom填充頁(yè)1、dsp訪問(wèn)頁(yè)2示意圖。

圖11為eeprom填充頁(yè)2、dsp訪問(wèn)頁(yè)1示意圖。

具體實(shí)施方式

本發(fā)明提出了一種關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制系統(tǒng)及控制方法。下面結(jié)合附圖，對(duì)本發(fā)明作詳細(xì)說(shuō)明。

本發(fā)明利用一片集成式arm-dsp芯片、兩片fpga芯片完成機(jī)器人控制系統(tǒng)主控和電機(jī)運(yùn)動(dòng)控制。圖1為控制系統(tǒng)架構(gòu)圖，如圖所示，在物理模塊上系統(tǒng)核心部分為由多核arm+dsp、fpga組成的系統(tǒng)主控板、io擴(kuò)展模塊、電機(jī)伺服驅(qū)動(dòng)模塊以及ethercat或rs485或can或profilebus網(wǎng)絡(luò)總線(xiàn)通信接口。

圖2為系統(tǒng)硬件結(jié)構(gòu)圖，如圖所示，系統(tǒng)硬件主要由主控板、信號(hào)集成模塊、ipm驅(qū)動(dòng)放大模塊、系統(tǒng)強(qiáng)電電路接觸器模塊等構(gòu)成。其中，主控板上主要有master-fpga和arm-dsp雙核處理器、擴(kuò)展dram、sd卡接口、隔離io、網(wǎng)絡(luò)總線(xiàn)模塊等。信號(hào)集成板上主要包括slave-fpga接口管理和隔離模塊，包括編碼器信號(hào)隔離、抱閘電路隔離、強(qiáng)電模塊監(jiān)控隔離、rs485通信隔離、pc調(diào)試串行口隔離等。主控板和信號(hào)集成板之間通過(guò)兩塊fpga間的spi總線(xiàn)進(jìn)行通信，通信內(nèi)容主要包括adc采集數(shù)據(jù)、pwm信號(hào)、編碼器信號(hào)及時(shí)鐘統(tǒng)一管理信號(hào)、任務(wù)指令信息、控制數(shù)據(jù)信息等。

主控板工作流程為master-fpga擴(kuò)展的集成板接口、隔離io接口、sd卡接口、dram接口采集相關(guān)信息后，交由arm-dsp雙核處理器進(jìn)行處理，產(chǎn)生控制信息，控制信息再通過(guò)fpga擴(kuò)展的集成板接口、隔離io接口、sd卡接口、dram接口發(fā)送到信號(hào)集成板、sd卡、控制io動(dòng)作、發(fā)送網(wǎng)絡(luò)總線(xiàn)指令、生成4路基本pwm發(fā)生器等。信號(hào)集成板主要用于在主控板統(tǒng)一時(shí)鐘下的信號(hào)采集、隔離和管理，具體包括ipm驅(qū)動(dòng)放大電路的電流采集、異常信號(hào)采集、電機(jī)控制信號(hào)發(fā)送，抱閘接口信號(hào)控制管理，強(qiáng)電電路異常接口信息采集和控制管理，擴(kuò)展io隔離、信號(hào)讀取、控制管理，rs485通信接口隔離、信號(hào)讀取、信號(hào)發(fā)送，絕對(duì)值編碼器接口隔離、信號(hào)讀取、信號(hào)發(fā)送等。

強(qiáng)電管理電路主要完成電源整流、過(guò)壓過(guò)流保護(hù)、大電容充放電管理、再生電阻溫度管理等，master-fpga另一主要作用為運(yùn)動(dòng)控制多路電流環(huán)dq電流分解使用、編碼器信號(hào)接收后的crc校驗(yàn)和旨在速度檢測(cè)平滑的快速移動(dòng)窗口平均、生成4路擴(kuò)展pwm發(fā)生器、電流采樣檢測(cè)三階降噪濾波器等。arm-dsp雙核處理器中的dsp模塊主要完成系統(tǒng)的路徑規(guī)劃和電機(jī)實(shí)時(shí)pid控制算法以及相應(yīng)的快速和平滑處理，主控板arm模塊主要完成系統(tǒng)的狀態(tài)監(jiān)測(cè)、信號(hào)管理和任務(wù)調(diào)度。

系統(tǒng)軟件程序采用模塊化設(shè)計(jì)方法，如圖3所示，主要模塊包括m3-arm處理模塊、c28-dsp軟件模塊、mster-fpga軟件模塊、slave-fpga模塊、示教器通信管理模塊和pc端gui通信管理模塊等。這里的各個(gè)模塊為軟件模塊，邏輯關(guān)系為m3-arm處理模塊和c28-dsp軟件模塊為順序先后串聯(lián)連接關(guān)系，這兩個(gè)模塊與這里其他所述的模塊關(guān)系為同步并聯(lián)運(yùn)行關(guān)系。

其中，m3-arm處理模塊主要包括系統(tǒng)管理、示教器通信管理、dsp通信、集成板slave-fpga管理、外部io管理以及任務(wù)調(diào)度等；

c28-dsp模塊主要完成dsp子系統(tǒng)管理、m3通信處理、master-fpga通信管理、多路電機(jī)實(shí)時(shí)位置控制和速度控制以及路徑規(guī)劃算法實(shí)現(xiàn)等；

mster-fpga軟件模塊主要處理電流采樣、電流環(huán)解耦和控制、以及位置速度檢測(cè)濾波等流程；

集成板slave-fpga主要完成電源管理程序、抱閘控制程序和外部io管理程序等；

示教器管理模塊主要完成示教器按鍵管理、示教器窗口管理、示教器通信管理和示教器文本解析管理等；

pc端gui管理主要完成gui調(diào)試、參數(shù)配置、示教器模擬、數(shù)據(jù)采集分析和虛擬示教等；

整個(gè)系統(tǒng)位于統(tǒng)一時(shí)鐘管理之下，除pc端gui管理模塊外其他均采用硬件實(shí)時(shí)中斷處理機(jī)制。

統(tǒng)一時(shí)鐘管理下的多路電機(jī)串并行混合控制機(jī)制，如圖4所示為多路電機(jī)串并行混合控制機(jī)制圖，在master-fpga通過(guò)并行設(shè)計(jì)實(shí)現(xiàn)7路伺服電機(jī)控制的clark-park變換、park逆變換以及與電流解耦密切相關(guān)的電流環(huán)控制，在dsp上通過(guò)串行設(shè)計(jì)7路電機(jī)實(shí)時(shí)控制的位置環(huán)和速度環(huán)。clark變換的作用為將基于三軸、兩維的定子靜止坐標(biāo)系電流矢量轉(zhuǎn)換為基于兩軸的定子靜止坐標(biāo)系電流矢量，park變換的作用為將兩軸正交的定子靜止坐標(biāo)系矢量電流轉(zhuǎn)換為等效到d、q軸的轉(zhuǎn)動(dòng)系統(tǒng)矢量電流，其中clark變換如（1）所示，park變換如（2）所示,pi控制基本描述如（3）所示。

圖5統(tǒng)一時(shí)鐘管理下的dsp串行位置環(huán)、速度環(huán)計(jì)算任務(wù)與fpga并行clark-park變換、park逆變換的任務(wù)管理機(jī)制。采用這種機(jī)制的優(yōu)勢(shì)在于一方面合理處理了clark-park變換、park逆變換計(jì)算過(guò)于復(fù)雜、耗時(shí)遠(yuǎn)大于位置環(huán)、速度環(huán)pi計(jì)算而導(dǎo)致的兩者之間任務(wù)等待問(wèn)題，利于減少控制時(shí)間、提高控制頻率，另一方面代替?zhèn)鹘y(tǒng)方案中多路電機(jī)需要多路dsp控制造成的成本上升問(wèn)題。

quote(1)

如圖6所示為關(guān)節(jié)型機(jī)械臂驅(qū)控一體化控制方法流程圖包括任務(wù)示教、路徑提取、奇異點(diǎn)處理、運(yùn)動(dòng)學(xué)路徑規(guī)劃、路徑平滑以及各個(gè)軸的位置環(huán)、速度環(huán)、電流環(huán)伺服電機(jī)實(shí)時(shí)運(yùn)動(dòng)控制等模塊。

任務(wù)路徑提取首先要做的是任務(wù)路徑描述，任務(wù)路徑描述采用標(biāo)準(zhǔn)最小指令集格式，如圖7所示為任務(wù)描述標(biāo)準(zhǔn)最小指令集格式圖，任務(wù)路徑描述均按照該指令集進(jìn)行，路徑描述按照?qǐng)D7指令集格式進(jìn)行編碼解碼即可。任務(wù)路徑提取其次要做的是任務(wù)描述解析，任務(wù)描述解析分為兩部分，一部分為將提取的任務(wù)發(fā)送到arm-dsp主控系統(tǒng)，該部分采用二進(jìn)制編碼，以保證傳輸和運(yùn)行實(shí)時(shí)性最高，與此同時(shí)，arm-dsp主控系統(tǒng)會(huì)周期性的將機(jī)器人運(yùn)行狀態(tài)發(fā)送至任務(wù)示教端，由路徑提取模塊按照二進(jìn)制譯碼后將該信息賦于圖7最小指令集格式電機(jī)位置上進(jìn)行顯示。另一部分為將提取的任務(wù)以及當(dāng)前機(jī)器人狀態(tài)在示教器上進(jìn)行顯示，該部分采用ascii編碼，按照?qǐng)D7指令集格式進(jìn)行顯示方便操作者直接了解機(jī)器人狀態(tài)。提取后的任務(wù)文件直接以二進(jìn)制格式保存于系統(tǒng)sd卡中，將sd卡信息復(fù)制到其他機(jī)器人中，其他機(jī)器人可以直接使用。該關(guān)鍵技術(shù)本發(fā)明優(yōu)先在基于單獨(dú)系統(tǒng)的示教裝置上實(shí)現(xiàn)，但不是唯一限定。

如圖8所示，奇異點(diǎn)處理一共分為三個(gè)子模塊，分別為奇異點(diǎn)檢測(cè)、奇異點(diǎn)報(bào)警及操作者交互、奇異點(diǎn)躲避。其中，奇異點(diǎn)檢測(cè)首先檢測(cè)路徑的位置奇異信息，然后基于位置奇異信息檢測(cè)速度奇異信息，最后基于速度奇異信息檢測(cè)加速度奇異信息。奇異點(diǎn)檢測(cè)模塊檢測(cè)到奇異信息后會(huì)交給奇異點(diǎn)報(bào)警模塊處理，控制相關(guān)io發(fā)出報(bào)警燈信號(hào)和報(bào)警聲音信號(hào)，并在試教裝置上進(jìn)行顯示，由操作者根據(jù)任務(wù)特點(diǎn)選擇奇異點(diǎn)躲避方法。奇異點(diǎn)躲避方法分為三種，分別為奇異路徑重新制定、有限步奇異點(diǎn)躲避和實(shí)時(shí)監(jiān)督奇異點(diǎn)躲避，奇異路徑重新制定是指放棄當(dāng)前遇到奇異點(diǎn)的路徑重新制定路徑避免奇異點(diǎn)，有限步奇異點(diǎn)躲避是指控制機(jī)器人每個(gè)軸轉(zhuǎn)過(guò)特定距離的步長(zhǎng)使當(dāng)前位置偏離奇異點(diǎn)，實(shí)時(shí)監(jiān)督奇異點(diǎn)躲避是指各個(gè)軸每次轉(zhuǎn)非常小的步長(zhǎng)并實(shí)時(shí)反饋各軸位置信息自動(dòng)判斷是否已經(jīng)偏離了奇異點(diǎn)，如果已經(jīng)偏離，則躲避完成。該關(guān)鍵技術(shù)本發(fā)明優(yōu)先在主控arm芯片上實(shí)現(xiàn)，但不是唯一限定。

運(yùn)動(dòng)學(xué)路徑規(guī)劃技術(shù)基本流程如圖9所示，首先將提取任務(wù)點(diǎn)的起點(diǎn)和終點(diǎn)位置、速度等運(yùn)動(dòng)約束。然后進(jìn)行路徑規(guī)劃插補(bǔ)方式選擇，插補(bǔ)方式分為軸插補(bǔ)、直線(xiàn)插補(bǔ)和圓弧插補(bǔ)。軸插補(bǔ)軌跡生成流程包括多軸同步s曲線(xiàn)路徑生成和7段變速時(shí)間劃分機(jī)制（加加速、勻加速、減加速、勻速、加減速、勻減速、減減速）。直線(xiàn)插補(bǔ)軌跡生成流程包括正運(yùn)動(dòng)學(xué)位資求解、變步長(zhǎng)路徑離散、逆運(yùn)動(dòng)學(xué)求解、加權(quán)最小范數(shù)優(yōu)化等。圓弧插補(bǔ)與直線(xiàn)插補(bǔ)類(lèi)似，但是圓弧插補(bǔ)除了需要任務(wù)起點(diǎn)和終點(diǎn)外，還需要一個(gè)中間點(diǎn)，由三個(gè)點(diǎn)構(gòu)成圓弧曲線(xiàn)，然后再進(jìn)行變步長(zhǎng)路徑離散、逆運(yùn)動(dòng)學(xué)求解和加權(quán)最小范數(shù)優(yōu)化步驟。該關(guān)鍵技術(shù)本發(fā)明優(yōu)先在主控dsp芯片上實(shí)現(xiàn)，但不是唯一限定。

大規(guī)模路徑規(guī)劃數(shù)據(jù)轉(zhuǎn)為實(shí)時(shí)運(yùn)動(dòng)控制部分參考數(shù)據(jù)的傳遞流程為dsp完成大規(guī)模路徑規(guī)劃數(shù)據(jù)后首先以較慢速度寫(xiě)入eeprom中，然后在arm芯片控制下eeprom當(dāng)前數(shù)據(jù)交替快速寫(xiě)入提前分成兩頁(yè)的專(zhuān)用ram中，eeprom數(shù)據(jù)讀取首地址由arm芯片給出，eeprom數(shù)據(jù)讀取量由單頁(yè)ram存儲(chǔ)量決定。與此同時(shí)，dsp轉(zhuǎn)入實(shí)時(shí)控制流程，錯(cuò)時(shí)訪問(wèn)已經(jīng)完成寫(xiě)入的ram頁(yè)數(shù)據(jù)作為參考位置進(jìn)行單軸電機(jī)實(shí)時(shí)控制。圖10表示在arm控制下數(shù)據(jù)由eerpom填充到ram頁(yè)1的過(guò)程，同時(shí)在這個(gè)時(shí)間段也是dsp實(shí)時(shí)控制所需數(shù)據(jù)由ram頁(yè)2進(jìn)行提供的階段。圖11表示在arm控制下數(shù)據(jù)由eerpom填充到ram頁(yè)2的過(guò)程，同時(shí)在這個(gè)時(shí)間段也是dsp實(shí)時(shí)控制所需數(shù)據(jù)由ram頁(yè)1進(jìn)行提供的階段。需要特別指出的是，本發(fā)明dsp路徑規(guī)劃和dsp實(shí)時(shí)控制優(yōu)先在一片dsp芯片上通過(guò)錯(cuò)時(shí)交替運(yùn)算進(jìn)行實(shí)施。