兩自由度茶園綜合作業(yè)平臺研制

楊擁軍，潘 峰，吳文亮，羅妙輝

（1.湖南省農(nóng)業(yè)科學(xué)院茶葉研究所，湖南 長沙 410125；2.上海贏浩機(jī)電設(shè)備有限公司，上海 201708；3.株洲市錦瑞機(jī)械有限公司，412000）

該茶園綜合作業(yè)平臺采用電動缸兩自由度調(diào)控設(shè)計(jì)，能夠隔離載體角運(yùn)動在載體機(jī)動狀態(tài)下建立穩(wěn)定基準(zhǔn)面，使安裝在平臺上的茶園作業(yè)裝備不會因茶園地表起伏產(chǎn)生的抖動和擺動而丟失目標(biāo)，確保準(zhǔn)確作業(yè)姿態(tài)，在茶園輕簡化綜合作業(yè)過程中得到很好的利用。

平臺結(jié)構(gòu)如圖1所示，平臺主要由下平臺、上平臺、電動缸、輔助支軸和鉸鏈組成。調(diào)平平臺外形尺寸約為400mm×400mm×388mm。根據(jù)茶園立地條件的要求，通過對調(diào)平平臺的優(yōu)化設(shè)計(jì)，可以得出運(yùn)動范圍滿足要求、剛度大、可實(shí)現(xiàn)性好的調(diào)平平臺的幾何參數(shù)。即運(yùn)動平臺上連接電動缸的兩個鉸鏈中心與平臺中心呈等腰直角形布置，下平臺鉸鏈中心與上平臺鉸鏈中心垂直對稱布置。

使用Solidwoks建立調(diào)平平臺三維模型并進(jìn)行運(yùn)動空間模擬，當(dāng)電動缸行程為85mm時，可保證調(diào)平平臺橫滾、俯仰角度達(dá)到±26°。

下平臺固定于自行式工作機(jī)架，是整個設(shè)備的基座。

分析結(jié)果表明，該系統(tǒng)結(jié)構(gòu)簡單，穩(wěn)定性好，實(shí)時性強(qiáng)，具有良好的穩(wěn)態(tài)和動態(tài)性能，能夠滿足茶園工作平臺系統(tǒng)的性能要求。

圖1 平臺模型圖

1 工作原理

兩自由度茶園作業(yè)平臺控制系統(tǒng)采用CAN0PEN總線通訊和數(shù)字輸入輸出功能的運(yùn)動控制器、全數(shù)字電機(jī)控制器、傾角傳感器、控制軟件等組成。

平臺的工作原理如圖2所示。平臺為一臺兩自由度并聯(lián)機(jī)構(gòu)，該并聯(lián)機(jī)構(gòu)的兩個支腿為兩個電動缸。兩自由度運(yùn)動平臺在控制系統(tǒng)監(jiān)測和控制下運(yùn)行。控制系統(tǒng)為分層控制結(jié)構(gòu)，監(jiān)控單元負(fù)責(zé)人機(jī)界面交互，接受簡述工作過程如下：用戶操作指令，并將控制信號下達(dá)給實(shí)時控制計(jì)算機(jī)。

圖2 系統(tǒng)原理框圖

平臺控制系統(tǒng)采用全數(shù)字伺服控制系統(tǒng)，姿態(tài)測量設(shè)備測量上平臺實(shí)時姿態(tài)，通過串口通訊將實(shí)際姿態(tài)傳輸至微型控制器；控制器接收平臺位置姿態(tài)后，經(jīng)過空間運(yùn)動模型變換，由反解運(yùn)算解出補(bǔ)償平臺姿態(tài)所需兩只電動缸的伸長量，通過總線傳遞給驅(qū)動器，由驅(qū)動器內(nèi)部PC得到信息并驅(qū)動電動機(jī)轉(zhuǎn)動，電動缸按照指令伸縮，平臺亦運(yùn)行到指定水平姿態(tài)。而安裝在電動機(jī)上的編碼器實(shí)時檢測出電動機(jī)的力矩、速度、位置信息并發(fā)送到驅(qū)動器，構(gòu)成閉環(huán)控制系統(tǒng)，以實(shí)時精確地控制各電動缸的伸長量，同時信息傳輸給微型控制器，微型控制器通過反解計(jì)算，得出上平臺當(dāng)前姿態(tài)，作為解算參考，以確保兩只電動缸的協(xié)調(diào)動作以及控制精度。

兩自由度茶園作業(yè)平臺控制流程圖如圖3所示，主控制回路：傾角傳感器測量平臺實(shí)際姿態(tài)傳輸給微型控制器，微型控制器通過位姿轉(zhuǎn)換和解算，計(jì)算出兩只電動缸的運(yùn)動參數(shù)（電動缸伸縮量），并通過CAN通訊把相應(yīng)的位移量傳遞給相應(yīng)的伺服驅(qū)動器，伺服驅(qū)動器控制電機(jī)的轉(zhuǎn)動，進(jìn)而控制電動缸的長度，以實(shí)現(xiàn)上平臺兩自由度角度調(diào)節(jié)。

圖3 系統(tǒng)控制流程圖

2 性能指標(biāo)

2.1 運(yùn)動能力指標(biāo)

名稱主要技術(shù)指標(biāo)兩自由度茶園作業(yè)平臺性能指標(biāo)（1）有效載荷：2 0 k g；總載荷：4 0 k g（含上平臺自重）（2）運(yùn)動參數(shù)：位移 速度俯仰（α）±2 5°±2 0°/s±4 0°/s 2 ≤0.1° ≤0.0 6°姿態(tài) 加速度 定位精度重復(fù)定位精度滾轉(zhuǎn)（β） ±4 0°/s 2 ≤0.1°±2 5°±2 0°/s ≤0.0 6°其他技術(shù)指標(biāo)（1）系統(tǒng)響應(yīng)頻率：0.0 H Z～2 0 H Z。（2）穩(wěn)態(tài)精度：≤0.3°（靜態(tài)）（3）裝機(jī)功率：0.2 k w 輸入電壓：4 8 V D C（4）防護(hù)等級：I P 5 4（5）平臺總重：約2 8 k g(不含控制箱重量)

2.2 組裝形式

機(jī)械組件需具有開放性，可拆卸組裝。

2.3 產(chǎn)品壽命

額定載荷下，全行程往復(fù)工作壽命不小于1×107次，存儲壽命不小于48個月。

2.4 噪聲

平臺系統(tǒng)以20°/s速度運(yùn)動時，距平臺系統(tǒng)1m處的噪聲強(qiáng)度低于65dB。

2.5 安全功能

2.5.1 機(jī)械結(jié)構(gòu)安全性設(shè)計(jì)

所有關(guān)鍵受力部件都依照國標(biāo)要求，機(jī)械設(shè)計(jì)安全系數(shù)不小于3.0，驅(qū)動裕度不小于3.0。

2.5.2 安全特性

為了保證系統(tǒng)安全、可靠的運(yùn)行，防止系統(tǒng)失控時對平臺結(jié)構(gòu)產(chǎn)生破壞，系統(tǒng)設(shè)有三級保護(hù)功能：軟件保護(hù)、電路保護(hù)和硬件保護(hù)。

軟件保護(hù)：軟件保護(hù)為第一級保護(hù)，是最基本保護(hù)功能，保護(hù)發(fā)生在系統(tǒng)控制軟件運(yùn)行周期內(nèi)；實(shí)驗(yàn)運(yùn)動平臺系統(tǒng)控制軟件設(shè)計(jì)時，對電機(jī)運(yùn)行的最大行程、運(yùn)行速度和加速度都設(shè)有閥值，防止電機(jī)運(yùn)動時出現(xiàn)超行程超速運(yùn)動引起的過沖現(xiàn)象。

電路保護(hù)：電路保護(hù)為第二級保護(hù)，保護(hù)發(fā)生在系統(tǒng)上電運(yùn)行周期內(nèi)；系統(tǒng)電機(jī)驅(qū)動控制器對電機(jī)輸出碼值、最大速度和加速度，最大輸出電流設(shè)定有閥值，防止電機(jī)超速運(yùn)動引起的過沖現(xiàn)象。

硬件保護(hù)：硬件保護(hù)為第三級保護(hù)，最高級的保護(hù)，保護(hù)發(fā)生在系統(tǒng)安裝、驗(yàn)收完成后的全部周期內(nèi)硬件保護(hù)，保證任何失控情況下實(shí)驗(yàn)平臺結(jié)構(gòu)不發(fā)生破壞，用戶試驗(yàn)設(shè)備不被損壞，保證實(shí)驗(yàn)平臺工作和保存的所有工況下平臺結(jié)構(gòu)不發(fā)生破壞。

2.6 環(huán)境條件

a.工作溫度：-20℃～40℃

b.儲存溫度：-40℃～65℃

c.相對濕度：95%±3%（40℃）

2.7 可靠性和可維修性

a.平臺系統(tǒng)一次連續(xù)工作時間不低于48h。

b.使用壽命不小于22000h。

c.平均無故障時間MTBF≥2200h。

d、平均維修時間 MTBF＜4h。

2.8 供電條件要求

a.直流 48VDC，波動范圍：-10%～+10%；

b.容量 0.2kVA。

3 控制系統(tǒng)組成及技術(shù)組合

3.1 控制系統(tǒng)的組成

圖4 六自由度平臺控制器

多自由度運(yùn)動平臺控制器如圖4所示，控制系統(tǒng)由運(yùn)動控制計(jì)算機(jī)、全數(shù)字電機(jī)控制器、數(shù)字輸入輸出模塊、CAN總線通訊模嵌入式塊、操作系統(tǒng)、姿態(tài)測量系統(tǒng)和控制軟件組成。

3.2 運(yùn)動控制計(jì)算機(jī)

運(yùn)動控制計(jì)算機(jī)設(shè)計(jì)方案需要滿足如下幾點(diǎn)要求：

（1）必須選擇數(shù)學(xué)運(yùn)算能力較強(qiáng)的控制器或處理器，必須采用工業(yè)級性能穩(wěn)定可靠的主控芯片，且有足夠的片內(nèi)存儲器。

（2）主控制器必須有豐富的外設(shè)，可以方便快捷地搭建硬件平臺，簡化硬件設(shè)計(jì)提高硬件系統(tǒng)穩(wěn)定性。必須具有以太網(wǎng)、CAN通訊、串口通訊等常用的通訊外設(shè)接口。

（3）采用模塊化設(shè)計(jì)，開放式硬件架構(gòu)，各部分功能相互獨(dú)立，必須具有良好的EMC特性。

（4）采集模塊與主控制模塊相分離，采集模塊必須有較好的濾波特性和抗干擾能力，采集模塊和主控模塊之間的通訊必須安全可靠。

（5）充分考慮到硬件設(shè)計(jì)可靠性及抗干擾設(shè)計(jì)。

考慮到以上問題，采用基于ARM的嵌入式控制方案。嵌入式系統(tǒng)被描述為：“以應(yīng)用為中心、軟件硬件可裁剪的、適應(yīng)應(yīng)用系統(tǒng)對功能、可靠性、成本、體積、功耗等嚴(yán)格綜合性要求的專用計(jì)算機(jī)系統(tǒng)”。系統(tǒng)的采集、處理、傳輸采用相對獨(dú)立的模塊化設(shè)計(jì)，具有良好的協(xié)調(diào)能力。圖5為系統(tǒng)硬件方案的示意圖。

圖5 系統(tǒng)方案的示意圖

3.2.1 基于Cortex-M4運(yùn)動控制卡設(shè)計(jì)[1]

ST公司的STM32F4系列是高性能的基于ARM Cortex-M4的32位微控制器,工作頻率高達(dá)168MHz。Cortex M4具有浮點(diǎn)單元(FPU)，支持所有的ARM單精度數(shù)據(jù)處理指令和數(shù)據(jù)類型，具有DSP處理指令和自適應(yīng)的實(shí)時加速器(ART)。STM32 F4改進(jìn)了一些外設(shè)，如全雙工I2S、小于1 uA的RTC以及2.44 MHz/s采樣率的ADC,STM32 F4的通信接口高達(dá)15個。STM32 F4還包含512KB～1MB的片內(nèi)內(nèi)存存儲器，多達(dá)192KB的SRAM。系統(tǒng)采用STM32F4作為主控芯片。

3.2.2 主控制器總體設(shè)計(jì)

硬件電路由STM32F407微控制器和外圍電路組成，主要包括：STM32F407基本電路、電源電路、時鐘電路、復(fù)位電路、串口通信電路、以太網(wǎng)通信電路、JTAG調(diào)試接口電路、CAN總線通信電路、AD/DA電路、濾波放大電路、數(shù)字量輸入輸出電路等。圖6所示為主控制器總體設(shè)計(jì)框圖。

圖6 主控制器總體設(shè)計(jì)框圖

電源模塊：主要是為整個硬件平臺系統(tǒng)正常工作提供所有必須的電源，包括為主控芯片的工作提供穩(wěn)定的3.3V工作電壓，為放大電路的運(yùn)算放大器提供±15電壓。STM32F4主控芯片具有獨(dú)立的模擬電源引腳，模擬部分和數(shù)字部分分開供電，這樣有效提高了模擬系統(tǒng)的抗噪性。

串口電路：作為軟件開發(fā)重要的調(diào)試手段，異步串行通信(USRT)在調(diào)試的時候可以用來查看和輸入相關(guān)的信息，在本系統(tǒng)中通過一片MAX3232把串口1和2的3.3V電平轉(zhuǎn)換為RS-232電平，它也是一個和外設(shè)(GPRS模塊)通信的重要渠道。

CAN電路：CAN通信以最小的CPU負(fù)荷來高效處理大量收到的報(bào)文，同時減少了線束的數(shù)量，采用65HVD230驅(qū)動芯片連接到CAN總線上。

濾波放大電路：用于模擬量輸入輸出，將系統(tǒng)的0-3.3V模擬電壓線性放大到系統(tǒng)所需的 10V模擬量信號。

數(shù)字量輸入輸出電路：提供8路光耦隔離的數(shù)字量輸入信號，和8路數(shù)字量輸出信號。

以太網(wǎng)電路：利用STM32F4處理器自身的互聯(lián)特性與以太網(wǎng)控制芯片DP83848C實(shí)現(xiàn)以太網(wǎng)通信，通過外接的PHY接口，支持10/100Mbit/s的數(shù)據(jù)傳輸速率。

復(fù)位電路：系統(tǒng)內(nèi)部具有完善的內(nèi)部復(fù)位電路，外部電路采用簡單的阻容復(fù)位方式用于手動系統(tǒng)復(fù)位。

3.2.3 基于MII以太網(wǎng)電路設(shè)計(jì)

以太網(wǎng)模塊包括一個符合802.3協(xié)議的MAC(介質(zhì)訪問控制器)和專用的DMA控制器。該模塊支持獨(dú)立于介質(zhì)的接口(MII)和精簡的獨(dú)立于介質(zhì)的接口(RMII)，通過設(shè)置AFIO_MAPR寄存器可以選擇使用哪種接口。

獨(dú)立于介質(zhì)的接口(MII)用于MAC子層和PHY之間的互聯(lián)，允許10M位/s和100M位/s數(shù)據(jù)傳輸。圖7為獨(dú)立于介質(zhì)的接口(MII)信號線示意圖：

圖7 獨(dú)立于介質(zhì)的接口(MII)信號線示意圖

物理層接口芯片DP83848CVV與處理器接口部分硬件原理圖如圖8所示。

圖8 以太網(wǎng)硬件原理圖

3.2.4 光耦隔離CAN總線電路設(shè)計(jì)[2]

為了增強(qiáng)CAN總線節(jié)點(diǎn)的抗干擾能力，并不是在CAN控制器與CAN驅(qū)動芯片之間直接相連，而是加入了高速光耦隔離器。這樣能很好地實(shí)現(xiàn)總線上各個CAN節(jié)點(diǎn)之間的電氣隔離。在本研究中，系統(tǒng)選用高速光耦6N137，它可以很好的兼容TTL和COMS電平，信號寬度為10MHz，完全可滿足CAN總線信號1Mb/s通信速率的要求。有效地提高了主控制器和采集模塊之間通信的抗強(qiáng)干擾的能力，使得整個系統(tǒng)的可靠性和穩(wěn)定性得到增強(qiáng)。采用光耦隔離的CAN總線電路示意圖如圖9所示。

圖9 光耦隔離CAN總線電路示意圖

3.2.5 數(shù)字量信號輸入輸出電路設(shè)計(jì)

由于外界環(huán)境以及電路、器件本身對系統(tǒng)引入了干擾和噪聲，為了提高CAN總線的數(shù)據(jù)傳輸性能，保證其可靠性，采用光耦器件進(jìn)行信號隔離來抑制干擾。光耦器件是將電信號轉(zhuǎn)變?yōu)楣庑盘枺?dāng)光信號傳送到接收方再將其轉(zhuǎn)換為電信號。由于光信號的傳送不需要共地，所以可以將光耦器件兩側(cè)的地加以隔離。因此，加入光耦器件后，在傳輸信號的過程中能有效地抑制高頻脈沖和各種噪聲干擾，大大提高通道上的信噪比。

圖10為開關(guān)量輸入信號電路示意圖，VDD為工業(yè)24V開關(guān)電源正極，VDD為系統(tǒng)主控芯片GPIO電壓3.3V，通過光耦器件將兩側(cè)的地隔離。

圖10 開關(guān)量輸入信號電路示意圖

圖11為開關(guān)量輸出信號電路示意圖，VDD為系統(tǒng)主控芯片GPIO電壓3.3V，R1為上拉電阻，Q1為NPN三極管，通過光耦器件控制繼電器通斷來開啟和關(guān)閉外部開關(guān)量信號。

圖11 開關(guān)量輸出信號電路示意圖

3.3 全數(shù)字電機(jī)驅(qū)動器

電機(jī)驅(qū)動器選用深圳步科公司推出了新一代伺服控制系統(tǒng)SMC系列。步科SMCX系列主要使用調(diào)頻調(diào)壓以及調(diào)整相位的方法，通過速度反饋和位置反饋監(jiān)控，控制電機(jī)的高精度運(yùn)行。該系統(tǒng)在硬件的快速拆裝性、控制的精確性和安全性，以及通訊協(xié)議的兼容性等方面都有了跨越式的進(jìn)步，其中向?qū)讲僮骱蛨D形化界面可以大大方便用戶編程和調(diào)試。此外，步科SMC伺服系統(tǒng)的模塊化設(shè)計(jì)，也使現(xiàn)場總線協(xié)議、存儲器容量和安全級別的選擇更加靈活。高度的模塊化還為將來的升級提供了保證。

步科SMC伺服系統(tǒng)的指令給定方式，除了使用內(nèi)置于SMC伺服控制器內(nèi)的IO點(diǎn)和CANopen總線接口之外，還可以使用擴(kuò)展的通信接口兼容Profibus、Profinet、EtherCAT、DeviceNet、Ethernet、

TCP/IP、Ethernet、Powerlink 等總線系統(tǒng)進(jìn)行通信。通過Ethernet(以太網(wǎng))，可以實(shí)現(xiàn)遠(yuǎn)程的調(diào)試和診斷，確保高精度和高可靠性的運(yùn)行。

3.4 調(diào)平系統(tǒng)

3.4.1特 點(diǎn)

該平臺采用的傾角傳感器系統(tǒng)是一款微型雙軸傾角傳感裝置，選用北微生產(chǎn)的一款CAN輸出的小體積雙軸傾角傳感器（BWM425S）來檢測調(diào)平平臺的水平面角度偏差，該傾角傳感器采用成熟的工業(yè)級MEMS加速度計(jì)，測量范圍±88°，最高精度0.01°，工作溫度-40℃～+85℃，體積小，重量輕，可以滿足對空間有限制的應(yīng)用需求。本品通過把靜態(tài)重力場的變化轉(zhuǎn)化成傾角變化，以數(shù)字方式直接輸出水平傾角數(shù)值，具有成本低、溫漂小、使用簡單、抗外界干擾能力強(qiáng)等特點(diǎn)。

3.4.2 主要特性

（1）雙軸傾角測量

（2）最高精度0.01°

（3）分辨率：0.001°

（4）量程：±88°

（5）通信方式：CAN

（6）體積：L90*W40.5*H26(mm)

（7）供電電壓：9-35V

（8）防護(hù)等級：IP67

3.5 控制軟件及控制算法

3.5.1 調(diào)試軟件

調(diào)平平臺調(diào)試軟件界面如圖12所示，軟件各模塊定義及功能如下：

圖12 監(jiān)控軟件界面

主操作窗口：主操作窗口主要包括平臺的開機(jī)、關(guān)機(jī)、回零位、數(shù)據(jù)處理等操作輸入按鈕,見圖13。

圖13 主操作窗口示意圖

監(jiān)控窗口：主要用于監(jiān)控系統(tǒng)當(dāng)前狀態(tài)，包括實(shí)時顯示平臺實(shí)際姿態(tài)、電機(jī)伺服狀態(tài)、傳感器測量值等。

演示窗口：演示輸入窗口，可進(jìn)行單軸正弦運(yùn)動和多軸復(fù)合運(yùn)動。對于單軸運(yùn)動，輸入幅值和頻率后，平臺開始做單姿態(tài)的正弦運(yùn)動；對于復(fù)合運(yùn)動，點(diǎn)擊復(fù)合運(yùn)動按鈕即可，平臺開始做任意姿態(tài)的組合運(yùn)動。此功能主要用于平臺性能測試之用。

單步運(yùn)行窗口：包括單步點(diǎn)動輸入及單步連續(xù)運(yùn)動兩部分。單步點(diǎn)動輸入用于平臺微動控制，步距及速度均可設(shè)置；單步連續(xù)運(yùn)動，需設(shè)定目標(biāo)位置與運(yùn)行速度，平臺自動平滑運(yùn)行到指令位置。

主操作窗口：主操作窗口主要包括平臺的開機(jī)/關(guān)機(jī)、回零位、暫停/恢復(fù)等操作輸入按鈕。

監(jiān)控窗口：主要用于監(jiān)控系統(tǒng)當(dāng)前狀態(tài)，包括實(shí)時顯示平臺實(shí)際姿態(tài)、電機(jī)伺服狀態(tài)等，見圖14。

圖14 姿態(tài)曲線顯示窗口示意圖

單步運(yùn)行窗口：包括單步點(diǎn)動輸入及單步連續(xù)運(yùn)動兩部分。單步點(diǎn)動輸入用于平臺微動控制，步距及速度均可設(shè)置；單步連續(xù)運(yùn)動，需設(shè)定目標(biāo)位置與運(yùn)行速度，平臺自動運(yùn)行到指令位置。

反解算算法

本方案的平臺位置可分解為：

a.由電動缸控制的俯仰、橫滾角度運(yùn)動；

b.由回轉(zhuǎn)平臺控制的航向角度運(yùn)動。

在本方案中，俯仰、橫滾兩個自由度的位置反解是指：在上平臺的俯仰、橫滾角度和機(jī)構(gòu)的結(jié)構(gòu)參數(shù)已知時，求解兩個電動缸的伸縮長度。圖15所示建立調(diào)平平臺俯仰、橫滾兩個自由度運(yùn)動系統(tǒng)的固定坐標(biāo)系OXYZ和運(yùn)動坐標(biāo)系。

圖15 俯仰、橫滾機(jī)構(gòu)原理圖

O’X’Y’Z’，動平臺分別沿 X、Y、Z 軸分別平移xp、yp、zp距離，且按序繞 X、Y 軸旋轉(zhuǎn) α、β 角度，則有：

式中，x1i、y1i、z1i分別為動平臺三個鉸點(diǎn)相對于動坐標(biāo)系原點(diǎn)O’的空間位置坐標(biāo)，x1i、y1i、z1i分別是動平臺上三個鉸點(diǎn)在動平臺旋轉(zhuǎn)后相對于固定坐標(biāo)系原點(diǎn)O的空間位置坐標(biāo)，H為動平臺到回轉(zhuǎn)平臺的垂直距離，RXY為旋轉(zhuǎn)變換矩陣，即

由于鉸點(diǎn)D10在運(yùn)動過程中空間位置保持不變，所以動平臺不會產(chǎn)生附加移動，即xp=yp=zp=0，同時在虎克鉸的約束下能完全克服繞X、Y軸旋轉(zhuǎn)所帶來的Z軸上的附加轉(zhuǎn)動。

從上面的推到可以看出，已知平臺的俯仰、橫滾姿態(tài)可以唯一確定電動缸的缸長。

正解算算法

同樣將平臺位置可分解為由電動缸控制的俯仰、橫滾角度運(yùn)動。

沿用求位姿反解中的坐標(biāo)系、坐標(biāo)點(diǎn)和結(jié)構(gòu)參數(shù)，已知電動缸的缸長l1,l2并得到動平臺三個鉸點(diǎn)在固定坐標(biāo)系的位置x'1i、y'1i、z'1i以及在運(yùn)動坐標(biāo)系的初始位置 x1i、y1i、z1i，求解分別繞 X、Y 軸旋轉(zhuǎn)的獨(dú)立轉(zhuǎn)角α、β。在運(yùn)動坐標(biāo)系中，已知初始位置D10（0，0，0），設(shè)D11（r,r,0）和設(shè)D12(-r,r,0)，由式（4-1）～式（4-3）可得到：

通過式（4-1）和式（4-2）可進(jìn)一步推導(dǎo)出電動缸的缸長：

由式（4-4）即可得α、β的解析解，然后根據(jù)平臺的結(jié)構(gòu)參數(shù)即運(yùn)動范圍即可得到α、β的唯一解。

軌跡規(guī)劃和插補(bǔ)算法

軌跡規(guī)劃與插補(bǔ)算法的目的是讓平臺實(shí)現(xiàn)沿給定的位置指令平穩(wěn)運(yùn)動。對于給定的指令軌跡，需要選擇合適的控制算法和參數(shù)，產(chǎn)生輸出，同步控制各軸準(zhǔn)確跟蹤指令軌跡。

在運(yùn)動控制中，常用的插補(bǔ)算法有逐點(diǎn)比較插補(bǔ)法、數(shù)字積分法和時間分割法。本方案使用時間分割法（亦稱數(shù)據(jù)采樣插補(bǔ)法）實(shí)現(xiàn)兩軸軸聯(lián)動控制，即將給定軌跡的運(yùn)行時間細(xì)分為許多相等的時間間隔，稱為單位時間間隔（或插補(bǔ)周期），每經(jīng)過一個單位時間間隔進(jìn)行一次插補(bǔ)計(jì)算，算出這段時間間隔內(nèi)各軸的運(yùn)動距離，直至整個軌跡運(yùn)行結(jié)束。各軸在每個插補(bǔ)周期的啟動和停止階段分別會出現(xiàn)加速和減速的過渡過程。為了保證伺服系統(tǒng)運(yùn)動的平穩(wěn)性，避免沖擊和振蕩，做到啟動不失步、停止不超程，必須對過渡過程進(jìn)行有效的加減速控制。

數(shù)據(jù)采樣插補(bǔ)可分為兩個階段：粗插補(bǔ)和精細(xì)插補(bǔ)。粗插補(bǔ)是用微小的直線線段逼近給定的輪廓，通過下位機(jī)軟件實(shí)現(xiàn)；精插補(bǔ)是在上述微小的直線線段上進(jìn)行“數(shù)據(jù)點(diǎn)的密化”。

采用數(shù)據(jù)采樣插補(bǔ)算法，首先要解決的問題是選擇合適的插補(bǔ)周期，本項(xiàng)目設(shè)計(jì)的調(diào)平平臺采用位置控制模式，采樣頻率為1000Hz，插補(bǔ)周期為1ms，即主控程序運(yùn)行時，插補(bǔ)程序每1ms執(zhí)行一次，為下一個周期算出調(diào)平平臺各軸的轉(zhuǎn)動角度，同時位置反饋程序?qū)?shí)時采集到的編碼器碼制換算為當(dāng)前各軸實(shí)際角度位置與目標(biāo)位置進(jìn)行比較。

3.5.2 通訊模塊

上位主機(jī)與運(yùn)動平臺控制主機(jī)的通訊方式，用戶通過運(yùn)動平臺的API接口，可以順利地與運(yùn)動平臺建立連接關(guān)系。

網(wǎng)絡(luò)通信接口協(xié)議如下：

位機(jī)固定IP：192.168.1.122

下位機(jī)監(jiān)聽端口號：5050

上位機(jī)固定IP：192.168.1.155

上位機(jī)監(jiān)聽端口號：6060

下位機(jī)接收數(shù)據(jù)結(jié)構(gòu)體如下：

typedefstruct

{

BYTE sMark; //默認(rèn)(=33)

BYTE sComd; //下位機(jī)接收的工作命令

BYTE sRepa[2];//默認(rèn)值[0,0]

float sAtti[6];//指令姿態(tài)[α,β,γ,X,Y,Z]

float sVelo[6];//默認(rèn)值[0,0,0,0,0,0]

float sAcce[3];//默認(rèn)值[0,0,0]

}

下位機(jī)接收工作命令（sComd）解析如下：

{

sComd=2;//開機(jī)命令,平臺開始初始化，并上升到工作位（中立位）

sComd=3;//握手命令，平臺切換到工作態(tài)

sComd=4;//工作命令，平臺在工作態(tài)，正常發(fā)送運(yùn)動參數(shù)[α,β,γ,X,Y,Z]

sComd=5;//回中位命令，平臺由任一姿態(tài)回到中位（工作零位）

sComd=6;//關(guān)機(jī)命令，平臺運(yùn)動到最低位，本次工作結(jié)束

}

監(jiān)控計(jì)算機(jī)接收數(shù)據(jù)結(jié)構(gòu)體如下：

typedefstruct

{

BYTE rMark;//默認(rèn)(=33)

BYTE rComd;//上位機(jī)接收的工作狀態(tài)

BYTE rRepa[2];//默認(rèn)值[0,0]

float rAtti[6];//當(dāng)前姿態(tài)[α,β,γ,X,Y,Z]

float rRese[6];//電機(jī)狀態(tài)

float rBack[6];//電機(jī)碼值

}

上位機(jī)接收工作狀態(tài)（rComd）解析如下：

{

rComd=1;//待命狀態(tài),平臺在最低位，等待開機(jī)命令

rComd=2;//開機(jī)狀態(tài)，平臺正在開機(jī)、初始化

rComd=3;//中位狀態(tài)，平臺開機(jī)完畢，等待握手命令

rComd=4;//工作狀態(tài)，平臺在工作態(tài)，可以執(zhí)行姿態(tài)指令

rComd=5;//回中狀態(tài)，平臺正在回到中位（工作零位）

rComd=6;//關(guān)機(jī)狀態(tài)，平臺正在關(guān)機(jī)

rComd=8;//故障狀態(tài)，關(guān)機(jī)重啟

}

串口通信協(xié)議：1位起始位，1位停止位，8位數(shù)據(jù)位，1位奇校驗(yàn)位，115.2kbps波特率；

通信通道：COM1；

通信周期：10ms。

數(shù)據(jù)結(jié)構(gòu)：

1.上位機(jī)接受串口數(shù)據(jù)定義

structDataToMain

{

BYTE rMark;//默認(rèn)值(=33)

floatrAngXYZ[3];//接收姿態(tài)：

};

通信總字節(jié)數(shù)：13

控制器串口通信模塊可使用232、485或422通信協(xié)議。

3.6 調(diào)平平臺速度理論設(shè)計(jì)[3]

XY理論速度設(shè)計(jì)

圖16 調(diào)平平臺極限位置運(yùn)動結(jié)構(gòu)示意圖

如圖16所示，點(diǎn)O為上平臺旋轉(zhuǎn)中心，點(diǎn)A為調(diào)平平臺旋轉(zhuǎn)5°時鉸鏈點(diǎn)位置，點(diǎn)A1為調(diào)平平臺在5°邊界擺動角度α后鉸鏈點(diǎn)位置。調(diào)平平臺在5°和-5°時，電動缸同樣的行程對應(yīng)的調(diào)平平臺角度最小

當(dāng)α角度極小時（假設(shè)為0.1mm），AA1=電動缸行程=角度α對應(yīng)弧長，

電動缸速度0-250mm/s，即電動缸運(yùn)動0.1mm所需最短時間為0.1/250=0.0004s，則調(diào)平平臺零位時最大速度為 0.0318/0.0004°/s=79.5°/s。

3.7 精度分析

X、Y軸角度精度分析

圖17 調(diào)平平臺零位運(yùn)動示意圖

平臺零位時，同樣的電動缸行程對應(yīng)的調(diào)平平臺角度最大。如圖17所示，零位時鉸鏈點(diǎn)位置為A1，平臺擺動角度α后鉸鏈點(diǎn)位置為A，由17位伺服電機(jī)編碼器，計(jì)算得到電動缸運(yùn)動分辨率為5mm÷131072=3.8×10-5mm，當(dāng)平臺在零位，電動缸運(yùn)動 3.8×10-5mm，平臺對應(yīng)角度為 3.8×10-5÷6.28÷180×360°＝1.2×10-5度，遠(yuǎn)小于技術(shù)指標(biāo)要求，因此，平臺精度理論設(shè)計(jì)完全滿足使用要求。

3.8 平滑性

控制器接收監(jiān)控計(jì)算機(jī)位置指令（指令周期為10ms）后，在控制軟件中將位置指令分解為5步之后依次通過CAN總線發(fā)送到伺服驅(qū)動器（指令周期為2ms）；伺服電機(jī)在2ms控制周期內(nèi)，由當(dāng)前位置以S曲線運(yùn)行到指令位置。即伺服系統(tǒng)的實(shí)際控制頻率為200Hz，且每個控制周期內(nèi)都由伺服驅(qū)動器實(shí)時進(jìn)行路徑規(guī)劃，確保平臺的光滑平穩(wěn)運(yùn)行。

4 結(jié)論

本項(xiàng)目是以穩(wěn)定平臺項(xiàng)目為依托，以實(shí)現(xiàn)平臺的高精度為目的，對穩(wěn)定平臺控制系統(tǒng)的設(shè)計(jì)展開了研究。在以下方面有突破：

（1）對穩(wěn)定平臺控制系統(tǒng)實(shí)現(xiàn)數(shù)字化，克服了以模擬電路實(shí)現(xiàn)為主的傳統(tǒng)的穩(wěn)定平臺控制方法中體積大、成本高、故障頻繁、對微弱信號信噪分離困難，很難將控制精度提高到1‰以上的級別、模擬器件的工作狀態(tài)極易受溫度漂移的影響，使位置控制產(chǎn)生零點(diǎn)漂移誤差等缺陷；鑒于國內(nèi)大部分穩(wěn)定控制系統(tǒng)的控制芯片為定點(diǎn)型芯片，其浮點(diǎn)運(yùn)算能力有限，本設(shè)計(jì)采用浮點(diǎn)DSP以提高算法的計(jì)算精度。

（2）控制系統(tǒng)控制方案的選擇對高精度的實(shí)現(xiàn)也至關(guān)重要。本設(shè)計(jì)采用電流環(huán)、速度環(huán)、位置環(huán)組合實(shí)現(xiàn)多閉環(huán)的復(fù)合控制方案。在位置環(huán)增加前饋控制器，對位置給定進(jìn)行濾波和預(yù)測，以超前的控制作用來補(bǔ)償系統(tǒng)的動態(tài)滯后,提高系統(tǒng)的響應(yīng)速度，解決系統(tǒng)的穩(wěn)定性和精度之間的矛盾。

（3）平臺的環(huán)架位置測量采用旋轉(zhuǎn)變壓器粗機(jī)、精機(jī)組合方式提高測量的角度范圍和精度。旋轉(zhuǎn)變壓器的測量精度達(dá)到0.0479mrad。

（4）信號經(jīng)過一階RC硬件濾波電路濾除高頻干擾后輸出，控制板采集的信號再在軟件上采用滑動平均濾波、小波變換濾、FIR濾波等多種濾波算法濾除干擾，提高信號的采樣精度。

（5）研究了平臺最優(yōu)安裝位置。不同安裝位置將導(dǎo)致不同的補(bǔ)償角速度形式，最終影響系統(tǒng)的精度。本系統(tǒng)采用采用測量軸與俯仰框的OX軸平行結(jié)構(gòu)，用于測量橫滾角速度；測試裝置安裝在俯仰框(內(nèi)框)上，測量軸與俯仰框的OY軸平行，用于測量俯仰角速度。

同時，穩(wěn)定平臺系統(tǒng)所要完成的任務(wù)是使得系統(tǒng)要求具有很高的穩(wěn)態(tài)精度和較好的動態(tài)品質(zhì)，同時又要有足夠大的穩(wěn)定裕度和抗干擾能力。因此該高精度穩(wěn)定平臺完全能夠滿足高剛度、高諧振頻率、高穩(wěn)定性、高強(qiáng)度茶園作業(yè)要求。