本發(fā)明涉及一種末端臂架姿態(tài)控制裝置及方法，具體涉及一種對(duì)隧道多臂架、多關(guān)節(jié)作業(yè)設(shè)備的末端臂架姿態(tài)多模式自動(dòng)控制的裝置及方法。

背景技術(shù)：

現(xiàn)有工業(yè)用智能機(jī)器手臂實(shí)現(xiàn)臂架的控制，主要分為兩種控制方式：第一種，按照預(yù)定軌跡進(jìn)行運(yùn)動(dòng)控制；第二種，通過(guò)圖像識(shí)別、位置傳感器等設(shè)備采集信號(hào)，由專用計(jì)算機(jī)軟件進(jìn)行運(yùn)算處理，控制其運(yùn)動(dòng)。顯然，作為隧道作業(yè)設(shè)備，其作業(yè)工況多變復(fù)雜，無(wú)法預(yù)先設(shè)定器運(yùn)動(dòng)軌跡，且現(xiàn)場(chǎng)實(shí)際工況條件惡劣，高精度的傳感設(shè)備無(wú)法應(yīng)用，也不適合采用專用計(jì)算機(jī)進(jìn)行現(xiàn)場(chǎng)運(yùn)算處理。

現(xiàn)有隧道作業(yè)設(shè)備對(duì)臂架控制，一般采用在臂架上加裝傾角傳感器的方式，實(shí)時(shí)測(cè)量相應(yīng)臂架與水平面的夾角，以此作為臂架姿態(tài)控制的依據(jù)，基本實(shí)現(xiàn)了臂架末端與水平面保持固定姿態(tài)的功能。但是，隧道作業(yè)設(shè)備受其作業(yè)空間的限制，往往要求作業(yè)設(shè)備具有很好的靈活性，為此多關(guān)節(jié)、可多方向運(yùn)動(dòng)的臂架的結(jié)構(gòu)運(yùn)用得越來(lái)越廣泛，在帶來(lái)靈活性的同時(shí)，也造成了臂架的運(yùn)動(dòng)控制越來(lái)越復(fù)雜，尤其是末端姿態(tài)的控制，往往需要操作手反復(fù)調(diào)整才能接近目標(biāo)位置。為此，開(kāi)發(fā)一種針對(duì)多臂架、多運(yùn)動(dòng)方向作業(yè)設(shè)備的末端臂架姿態(tài)控制裝置及方法意義重大。

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

本發(fā)明所需要解決的技術(shù)問(wèn)題是：針對(duì)上述現(xiàn)有技術(shù)的不足，提供一種隧道多臂架、多關(guān)節(jié)作業(yè)設(shè)備的末端臂架姿態(tài)多模式自動(dòng)控制裝置及方法。

為解決上述技術(shù)問(wèn)題，本發(fā)明所采用的技術(shù)方案是：一種隧道多臂架、多關(guān)節(jié)作業(yè)設(shè)備的末端臂架姿態(tài)多模式自動(dòng)控制裝置，其特點(diǎn)是，該控制裝置包括傳感單元、控制單元、人機(jī)交互單元及執(zhí)行單元，其中：

該傳感單元，包括傾角傳感器、回轉(zhuǎn)角度傳感器及位移傳感器，該傾角傳感器設(shè)于作業(yè)設(shè)備臂架組的各臂架上以測(cè)量各臂架與水平面的夾角值，該回轉(zhuǎn)角度傳感器設(shè)于該臂架組中各臂架的回轉(zhuǎn)機(jī)構(gòu)上以測(cè)量各回轉(zhuǎn)機(jī)構(gòu)的回轉(zhuǎn)角度值，該位移傳感器設(shè)于該臂架組中各臂架的伸縮機(jī)構(gòu)上以測(cè)量各伸縮機(jī)構(gòu)的運(yùn)動(dòng)位移值；該傳感單元與該控制單元連接，以通過(guò)各傳感器實(shí)時(shí)采集傳感信號(hào)并傳送至控制單元；

該人機(jī)交互單元，與該控制單元連接，用于接收操作者的操控指令并發(fā)送至控制單元；同時(shí)接受并顯示控制單元發(fā)出的控制指令信息，將該臂架組的當(dāng)前運(yùn)行狀態(tài)實(shí)時(shí)展示給操作者；其中，該操作者的操控指令包括末端臂架水平方向調(diào)節(jié)指令、末端臂架朝向調(diào)節(jié)指令以及末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)指令；

該控制單元，其包括工業(yè)控制器，該工業(yè)控制器用于接收各傳感信號(hào)，同時(shí)接收人機(jī)交互單元傳送的操控指令，并結(jié)合傳感信號(hào)進(jìn)行運(yùn)算處理后輸出控制指令，并將控制指令發(fā)送到執(zhí)行機(jī)構(gòu)單元，同時(shí)將控制指令的相關(guān)信息反饋至人機(jī)交互系統(tǒng)進(jìn)行顯示；

該執(zhí)行機(jī)構(gòu)單元，與該控制單元連接；該執(zhí)行機(jī)構(gòu)單元包括控制閥組，用于接收控制單元發(fā)出的控制指令，以使控制閥組執(zhí)行該控制指令使該臂架組的基本臂架和末端臂架動(dòng)作，實(shí)現(xiàn)對(duì)該臂架組末端臂架的姿態(tài)控制。

上述傳感單元采集的信號(hào)包括所述臂架組的各臂架與水平面的當(dāng)前夾角值；該臂架組中各回轉(zhuǎn)機(jī)構(gòu)的當(dāng)前回轉(zhuǎn)角度值；該臂架組中各伸縮機(jī)構(gòu)的當(dāng)前運(yùn)動(dòng)位移值。

本發(fā)明同時(shí)提供一種隧道多臂架、多關(guān)節(jié)作業(yè)設(shè)備的末端臂架姿態(tài)多模式自動(dòng)控制方法，該方法步驟如下：

A.于該作業(yè)設(shè)備一臂架組的伸縮機(jī)構(gòu)、回轉(zhuǎn)機(jī)構(gòu)、以及各臂架上設(shè)置對(duì)應(yīng)的位移傳感器、回轉(zhuǎn)角度傳感器、和傾角傳感器；

B.人機(jī)交互單元獲取操作者的操控指令并發(fā)送至控制單元，該操控指令包括末端臂架水平方向調(diào)節(jié)指令、末端臂架朝向調(diào)節(jié)指令以及末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)指令；同時(shí)傳感單元中各回轉(zhuǎn)角度傳感器、傾角傳感器及位移傳感器采集當(dāng)前傳感信號(hào)并發(fā)送至控制單元；

C.控制單元接收操作者的操控指令和當(dāng)前的各傳感信號(hào)，經(jīng)運(yùn)算處理后輸出相應(yīng)的控制指令至執(zhí)行機(jī)構(gòu)單元使臂架組基本臂架和末端臂架動(dòng)作，實(shí)現(xiàn)對(duì)末端臂架姿態(tài)的自動(dòng)控制；

上述提及的控制指令的運(yùn)算處理過(guò)程如下：

末端臂架水平方向自動(dòng)調(diào)節(jié)控制指令：通過(guò)人機(jī)交互單元設(shè)定末端臂架與水平面的夾角值θe和操作者發(fā)出的末端臂架水平方向調(diào)節(jié)控制指令中的末端臂架實(shí)際水平調(diào)節(jié)角度即目標(biāo)夾角值θm的最大允許差值為Δθ；若傾角傳感器采集的末端臂架與水平面的當(dāng)前夾角值和操作者發(fā)出的目標(biāo)夾角值之間的差值Δθ₀大于Δθ時(shí)，則由控制單元計(jì)算出Δθ₀與Δθ之間的差值，根據(jù)該計(jì)算出的差值發(fā)出末端臂架水平方向調(diào)節(jié)控制指令至執(zhí)行機(jī)構(gòu)單元使末端臂架于垂直方向運(yùn)動(dòng)，使傾角傳感器采集的末端臂架與水平面的當(dāng)前夾角值和目標(biāo)夾角值的差值小于等于Δθ；若傾角傳感器采集的末端臂架與水平面的當(dāng)前夾角值和操作者發(fā)出的目標(biāo)夾角值之間的差值Δθ₀小于等于Δθ，則末端臂架不動(dòng)作，從而實(shí)現(xiàn)末端臂架的水平方向自動(dòng)調(diào)節(jié)控制；

末端臂架朝向自動(dòng)調(diào)節(jié)控制指令：設(shè)定操作者發(fā)出的末端臂架朝向調(diào)節(jié)操控指令中的目標(biāo)朝向調(diào)節(jié)角度為Δα，若基本臂架和末端臂架的回轉(zhuǎn)角度傳感器采集的該兩臂架的當(dāng)前回轉(zhuǎn)角度之和Δα₀等于Δα，則基本臂架和末端臂架不動(dòng)作；若基本臂架和末端臂架的回轉(zhuǎn)角度傳感器采集的兩臂架的當(dāng)前回轉(zhuǎn)角度之和Δα₀小于或大于Δα，則控制單元計(jì)算Δα₀與Δα的差值，再根據(jù)計(jì)算出的該差值發(fā)出末端臂架朝向調(diào)節(jié)控制指令至執(zhí)行機(jī)構(gòu)單元使基本臂架和末端臂架中的至少一個(gè)臂架進(jìn)行回轉(zhuǎn)動(dòng)作，直至基本臂架和末端臂架的回轉(zhuǎn)角度傳感器采集的該兩臂架的當(dāng)前回轉(zhuǎn)角度之和等于Δα，從而實(shí)現(xiàn)末端臂架朝向的自動(dòng)調(diào)節(jié)控制；

末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制指令：設(shè)定操作者發(fā)出的末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)指令中的目標(biāo)垂直調(diào)節(jié)距離為L(zhǎng)；

當(dāng)臂架組的基本臂架不動(dòng)作即不進(jìn)行垂直或回轉(zhuǎn)運(yùn)動(dòng)時(shí)，設(shè)定人機(jī)交互單元顯示的當(dāng)前垂直調(diào)節(jié)距離為L(zhǎng)₀；若L₀小于或大于L，則由控制單元計(jì)算出L₀與L的差值，再根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)該計(jì)算出的差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；若L₀等于L，則末端臂架的伸縮機(jī)構(gòu)不動(dòng)作，從而實(shí)現(xiàn)末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制；

當(dāng)臂架組的基本臂架只進(jìn)行垂直運(yùn)動(dòng)時(shí)，則先由控制單元根據(jù)基本臂架的傾角傳感器采集的垂直運(yùn)動(dòng)后基本臂架與水平面的夾角，依據(jù)基本臂架的長(zhǎng)度，計(jì)算出基本臂架垂直運(yùn)動(dòng)后的垂直方向值，再計(jì)算出該垂直方向值與L的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)該計(jì)算出的差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋，從而實(shí)現(xiàn)末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制；

當(dāng)臂架組的基本臂架只進(jìn)行回轉(zhuǎn)方向運(yùn)行時(shí)，則先由控制單元根據(jù)基本臂架的回轉(zhuǎn)角度傳感器采集的基本臂架回轉(zhuǎn)運(yùn)行前后的回轉(zhuǎn)角度值，依據(jù)基本臂架的長(zhǎng)度，計(jì)算出基本臂架回轉(zhuǎn)運(yùn)動(dòng)后于水平方向的垂直變化值L_基，若末端臂架不進(jìn)行回轉(zhuǎn)和垂直運(yùn)動(dòng)，則直接根據(jù)控制單元計(jì)算出的基本臂架回轉(zhuǎn)運(yùn)動(dòng)后于水平方向的垂直方向值與L進(jìn)行比較，計(jì)算出差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；若末端臂架進(jìn)行回轉(zhuǎn)而不進(jìn)行垂直運(yùn)動(dòng)，則由控制單元根據(jù)末端臂架的回轉(zhuǎn)角度傳感器采集的末端臂架回轉(zhuǎn)運(yùn)動(dòng)前后的回轉(zhuǎn)角度值，依據(jù)末端臂架的長(zhǎng)度，計(jì)算出末端臂架的回轉(zhuǎn)機(jī)構(gòu)回轉(zhuǎn)帶來(lái)的末端臂架于水平方向的垂直變化值L_末，將L_基、L_末、L₀和L相結(jié)合計(jì)算出當(dāng)前末端臂架尾端點(diǎn)與L之間的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；若末端臂架同時(shí)進(jìn)行回轉(zhuǎn)和垂直運(yùn)動(dòng)，則先由控制單元根據(jù)末端臂架的回轉(zhuǎn)角度傳感器采集的末端臂架回轉(zhuǎn)前后的回轉(zhuǎn)角度值以及末端臂架的傾角傳感器采集的末端臂架垂直運(yùn)動(dòng)前后與水平面的夾角值，依據(jù)末端臂架的長(zhǎng)度，計(jì)算出末端臂架在回轉(zhuǎn)和垂直運(yùn)動(dòng)后于水平方向上的垂直變化值，再結(jié)合L_基、L₀和L，計(jì)算出當(dāng)前末端臂架尾端點(diǎn)與L之間的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；若末端臂架不回轉(zhuǎn)而進(jìn)行垂直運(yùn)動(dòng)，則先由控制單元根據(jù)末端臂架的傾角傳感器采集的末端臂架垂直運(yùn)動(dòng)前后與水平面的夾角值，依據(jù)末端臂架的長(zhǎng)度，計(jì)算出末端臂架在垂直運(yùn)動(dòng)后于水平方向上的垂直變化值，再結(jié)合L_基、L₀和L，計(jì)算出當(dāng)前末端臂架尾端點(diǎn)與L之間的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；從而實(shí)現(xiàn)末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制；

當(dāng)臂架組的基本臂架同時(shí)進(jìn)行回轉(zhuǎn)和垂直方向運(yùn)行時(shí)，則先由控制單元根據(jù)基本臂架的回轉(zhuǎn)角度傳感器采集的基本臂架回轉(zhuǎn)前后的回轉(zhuǎn)角度值以及基本臂架的傾角傳感器采集的基本臂架垂直運(yùn)動(dòng)前后與水平面的夾角值，依據(jù)基本臂架的長(zhǎng)度，計(jì)算出基本臂架在回轉(zhuǎn)和垂直運(yùn)動(dòng)后于水平方向上的垂直變化值，再同理計(jì)算出末端臂架于水平方向上的垂直變化值，將該兩垂直變化值與L₀、L相結(jié)合計(jì)算出當(dāng)前末端臂架尾端點(diǎn)與L之間的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；從而實(shí)現(xiàn)末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制。

如此，本發(fā)明通過(guò)在臂架組回轉(zhuǎn)機(jī)構(gòu)中安裝回轉(zhuǎn)角度傳感器測(cè)量回轉(zhuǎn)角度值，從而實(shí)現(xiàn)對(duì)末端臂架的朝向進(jìn)行實(shí)時(shí)自動(dòng)控制；通過(guò)在臂架上安裝傾角傳感器，測(cè)量臂架與水平面的夾角值，從而實(shí)現(xiàn)對(duì)末端臂架的水平位置進(jìn)行實(shí)時(shí)自動(dòng)控制；通過(guò)臂架上安裝傾角傳感器、回轉(zhuǎn)角度傳感器，可實(shí)時(shí)計(jì)算出操作者在操作臂架時(shí)，帶來(lái)的末端臂架與支撐裝置鉸接點(diǎn)之間垂直距離的變化值，通過(guò)控制伸縮機(jī)構(gòu)并測(cè)量其伸縮長(zhǎng)度值，從而實(shí)現(xiàn)對(duì)臂架末端位置的實(shí)時(shí)自動(dòng)控制；通過(guò)人機(jī)交互單元，可依據(jù)各類(lèi)設(shè)備及其作業(yè)的實(shí)際情況，對(duì)上述三項(xiàng)自動(dòng)控制功能進(jìn)行自由組合，實(shí)現(xiàn)末端臂架的多模式姿態(tài)控制，極大程度上滿足了隧道作業(yè)設(shè)備對(duì)臂架結(jié)構(gòu)的智能操控要求，大大降低設(shè)備的操控難度，實(shí)現(xiàn)智能化自動(dòng)控制。

附圖說(shuō)明

圖1是本發(fā)明控制裝置的總體組成框圖。

圖2是實(shí)施例所示的臂架組機(jī)構(gòu)示意圖。

圖3是回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)于垂直方向變化示意圖。

圖4是臂架在水平方向的等效長(zhǎng)度。

圖5是回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)于回轉(zhuǎn)方向變化示意圖。

具體實(shí)施方式

如圖1所示，本發(fā)明為一種隧道多臂架、多關(guān)節(jié)作業(yè)設(shè)備的末端臂架姿態(tài)多模式自動(dòng)控制裝置，其是在現(xiàn)有多臂架、多關(guān)節(jié)作業(yè)設(shè)備的基礎(chǔ)上，通過(guò)于臂架組的基本臂架和活動(dòng)臂架上裝設(shè)回轉(zhuǎn)角度傳感器和傾角傳感器以測(cè)量回轉(zhuǎn)機(jī)構(gòu)的回轉(zhuǎn)角度值以及各臂架與水平面的夾角值；于各活動(dòng)臂架上裝設(shè)位移傳感器和回轉(zhuǎn)角度傳感器以測(cè)量伸縮機(jī)構(gòu)的伸縮長(zhǎng)度值和回轉(zhuǎn)機(jī)構(gòu)的回轉(zhuǎn)角度值。在各傳感器采集的位移和角度信號(hào)的基礎(chǔ)上，由控制單元結(jié)合人機(jī)交互單元傳送的操作者的操控指令進(jìn)行處理運(yùn)算以輸出控制指令，對(duì)末端臂架朝向、末端臂架水平位置以及末端臂架與支撐裝置鉸接點(diǎn)（臂架組的基本臂架與支撐裝置于該鉸接點(diǎn)鉸接）之間的垂直距離進(jìn)行自動(dòng)控制。具體地，本控制裝置包括傳感單元、控制單元、人機(jī)交互單元及執(zhí)行單元，其中：

該傳感單元，與該控制單元連接；該傳感單元包括傾角傳感器、回轉(zhuǎn)角度傳感器及位移傳感器，該傾角傳感器設(shè)于一臂架組的基本臂架和各活動(dòng)臂架上以測(cè)量各臂架與水平面的夾角值，該回轉(zhuǎn)角度傳感器設(shè)于該臂架組中各臂架（本文中的各臂架是指基本臂架和各活動(dòng)臂架）的回轉(zhuǎn)機(jī)構(gòu)上以測(cè)量各回轉(zhuǎn)機(jī)構(gòu)的回轉(zhuǎn)角度值，該位移傳感器設(shè)于該臂架組中各活動(dòng)臂架的伸縮機(jī)構(gòu)上以測(cè)量各伸縮機(jī)構(gòu)的運(yùn)動(dòng)位移值。如此，該傳感單元通過(guò)各傳感器采集的信號(hào)能獲取以下信息：各臂架與水平面的夾角值、該臂架組中各回轉(zhuǎn)機(jī)構(gòu)的回轉(zhuǎn)角度值、該臂架組中各伸縮機(jī)構(gòu)的運(yùn)動(dòng)位移值，同時(shí)傳感單元將采集的信號(hào)傳送至控制單元。

該人機(jī)交互單元，與該控制單元連接，用于接收操作者的操控指令并發(fā)送至控制單元；同時(shí)接受并顯示控制單元發(fā)出的控制指令信息，將臂架組的當(dāng)前運(yùn)行狀態(tài)實(shí)時(shí)展示給操作者即工況顯示。其中，該操控指令包括末端臂架水平方向調(diào)節(jié)指令、末端臂架朝向調(diào)節(jié)指令以及末端臂架與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)指令。

該控制單元，其包括工業(yè)控制器，該工業(yè)控制器用于采集傳感信號(hào)，同時(shí)接受當(dāng)前人機(jī)交互單元傳送的操控指令，并結(jié)合采集的傳感信號(hào)進(jìn)行運(yùn)算處理后輸出控制指令（該控制指令包括末端臂架水平方向調(diào)節(jié)控制指令、末端臂架朝向調(diào)節(jié)控制指令以及末端臂架與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)控制指令），并將控制指令發(fā)送到執(zhí)行機(jī)構(gòu)單元以控制臂架組的運(yùn)動(dòng)，同時(shí)將控制指令的相關(guān)信息反饋至人機(jī)交互系統(tǒng)進(jìn)行顯示。

該執(zhí)行機(jī)構(gòu)單元，與該控制單元連接；該執(zhí)行機(jī)構(gòu)單元包括控制閥組，用于接收控制單元發(fā)出的控制指令，以使控制閥組執(zhí)行該控制指令實(shí)現(xiàn)對(duì)臂架組末端臂架的姿態(tài)控制。

本發(fā)明的隧道多臂架、多關(guān)節(jié)作業(yè)設(shè)備的末端臂架姿態(tài)多模式自動(dòng)控制方法，其具體步驟如下：

A.于該作業(yè)設(shè)備的臂架組的各伸縮機(jī)構(gòu)、回轉(zhuǎn)機(jī)構(gòu)以及臂架上設(shè)置對(duì)應(yīng)的傳感器，具體地，臂架上設(shè)置傾角傳感器，伸縮機(jī)構(gòu)設(shè)置位移傳感器，回轉(zhuǎn)機(jī)構(gòu)設(shè)置回轉(zhuǎn)角度傳感器；

B.人機(jī)交互單元獲取操作者的操控指令并發(fā)送至控制單元，該操控指令包括末端臂架水平方向調(diào)節(jié)指令、末端臂架朝向調(diào)節(jié)指令以及末端臂架與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)指令；同時(shí)傳感單元中各回轉(zhuǎn)角度傳感器、傾角傳感器及位移傳感器采集各傳感信號(hào)并發(fā)送至控制單元；

C.控制單元接收操作者的操控指令，并通過(guò)傳感單元中各傳感器實(shí)時(shí)采集的各傳感信號(hào)，經(jīng)運(yùn)算處理后輸出相應(yīng)的控制指令（該控制指令包括末端臂架水平方向調(diào)節(jié)控制指令、末端臂架朝向調(diào)節(jié)控制指令以及末端臂架與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)控制指令），實(shí)現(xiàn)對(duì)末端臂架的姿態(tài)自動(dòng)控制。

上述提及的控制指令的具體運(yùn)算處理過(guò)程如下：

末端臂架水平方向自動(dòng)調(diào)節(jié)控制指令：通過(guò)人機(jī)交互單元設(shè)定末端臂架與水平面的夾角值θe，設(shè)定操作者發(fā)出的末端臂架水平方向調(diào)節(jié)控制指令中的末端臂架實(shí)際水平調(diào)節(jié)角度即目標(biāo)夾角值θm，θe與θm的最大允許差值為Δθ；若傾角傳感器采集的末端臂架與水平面的當(dāng)前夾角值和操作者發(fā)出的目標(biāo)夾角值之間的差值Δθ₀大于Δθ時(shí)，則由控制單元計(jì)算出Δθ₀與Δθ之間的差值，根據(jù)該計(jì)算出的差值發(fā)出末端臂架水平方向調(diào)節(jié)控制指令至執(zhí)行機(jī)構(gòu)單元使末端臂架于垂直方向運(yùn)動(dòng)，使傾角傳感器采集的末端臂架與水平面的當(dāng)前夾角值和目標(biāo)夾角值的差值小于等于Δθ；若傾角傳感器采集的末端臂架與水平面的當(dāng)前夾角值和操作者發(fā)出的目標(biāo)夾角值之間的差值Δθ₀小于等于Δθ，則末端臂架不動(dòng)作，從而實(shí)現(xiàn)末端臂架的水平方向自動(dòng)調(diào)節(jié)控制。

末端臂架朝向自動(dòng)調(diào)節(jié)控制指令：設(shè)定操作者發(fā)出的末端臂架朝向調(diào)節(jié)操控指令中的目標(biāo)朝向調(diào)節(jié)角度為Δα，若基本臂架和末端臂架的回轉(zhuǎn)角度傳感器采集的該兩臂架的當(dāng)前回轉(zhuǎn)角度之和Δα₀等于Δα，則基本臂架和末端臂架不動(dòng)作；若基本臂架和末端臂架的回轉(zhuǎn)角度傳感器采集的兩臂架的當(dāng)前回轉(zhuǎn)角度之和Δα₀小于或大于Δα，則控制單元計(jì)算Δα₀與Δα的差值，再根據(jù)計(jì)算出的該差值發(fā)出末端臂架朝向調(diào)節(jié)控制指令至執(zhí)行機(jī)構(gòu)單元使基本臂架和末端臂架中的至少一個(gè)臂架進(jìn)行回轉(zhuǎn)動(dòng)作，直至基本臂架和末端臂架的回轉(zhuǎn)角度傳感器采集的該兩臂架的當(dāng)前回轉(zhuǎn)角度之和等于Δα，從而實(shí)現(xiàn)末端臂架朝向的自動(dòng)調(diào)節(jié)控制。實(shí)際操作中，可依習(xí)慣設(shè)置左轉(zhuǎn)為正、右轉(zhuǎn)為負(fù)，或是右轉(zhuǎn)為負(fù)、右轉(zhuǎn)為正。

末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制指令：設(shè)定操作者發(fā)出的末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)指令中的目標(biāo)垂直調(diào)節(jié)距離為L(zhǎng)；

當(dāng)臂架組的基本臂架不動(dòng)作即不進(jìn)行垂直或回轉(zhuǎn)運(yùn)動(dòng)時(shí)，設(shè)定人機(jī)交互單元顯示的當(dāng)前垂直調(diào)節(jié)距離為L(zhǎng)₀；若L₀小于或大于L，則由控制單元計(jì)算出L₀與L的差值，再根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)該計(jì)算出的差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；若L₀等于L，則末端臂架的伸縮機(jī)構(gòu)不動(dòng)作，從而實(shí)現(xiàn)末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制；

當(dāng)臂架組的基本臂架只進(jìn)行垂直運(yùn)動(dòng)時(shí)，則先由控制單元根據(jù)基本臂架的傾角傳感器采集的垂直運(yùn)動(dòng)后基本臂架與水平面的夾角，依據(jù)基本臂架的長(zhǎng)度，計(jì)算出基本臂架垂直運(yùn)動(dòng)后的垂直方向值，再計(jì)算出該垂直方向值與L的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)該計(jì)算出的差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋，從而實(shí)現(xiàn)末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制；

當(dāng)臂架組的基本臂架只進(jìn)行回轉(zhuǎn)方向運(yùn)行時(shí)，則先由控制單元根據(jù)基本臂架的回轉(zhuǎn)角度傳感器采集的基本臂架回轉(zhuǎn)運(yùn)行前后的回轉(zhuǎn)角度值，依據(jù)基本臂架的長(zhǎng)度，計(jì)算出基本臂架回轉(zhuǎn)運(yùn)動(dòng)后于水平方向的垂直變化值L_基，若末端臂架不進(jìn)行回轉(zhuǎn)和垂直運(yùn)動(dòng)，則直接根據(jù)控制單元計(jì)算出的基本臂架回轉(zhuǎn)運(yùn)動(dòng)后于水平方向的垂直方向值與L進(jìn)行比較，計(jì)算出差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；若末端臂架進(jìn)行回轉(zhuǎn)而不進(jìn)行垂直運(yùn)動(dòng)，則由控制單元根據(jù)末端臂架的回轉(zhuǎn)角度傳感器采集的末端臂架回轉(zhuǎn)運(yùn)動(dòng)前后的回轉(zhuǎn)角度值，依據(jù)末端臂架的長(zhǎng)度，計(jì)算出末端臂架的回轉(zhuǎn)機(jī)構(gòu)回轉(zhuǎn)帶來(lái)的末端臂架于水平方向的垂直變化值L_末，將L_基、L_末、L₀和L相結(jié)合計(jì)算出當(dāng)前末端臂架尾端點(diǎn)與L之間的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；若末端臂架同時(shí)進(jìn)行回轉(zhuǎn)和垂直運(yùn)動(dòng)，則先由控制單元根據(jù)末端臂架的回轉(zhuǎn)角度傳感器采集的末端臂架回轉(zhuǎn)前后的回轉(zhuǎn)角度值以及末端臂架的傾角傳感器采集的末端臂架垂直運(yùn)動(dòng)前后與水平面的夾角值，依據(jù)末端臂架的長(zhǎng)度，計(jì)算出末端臂架在回轉(zhuǎn)和垂直運(yùn)動(dòng)后于水平方向上的垂直變化值，再結(jié)合L_基、L₀和L，計(jì)算出當(dāng)前末端臂架尾端點(diǎn)與L之間的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；若末端臂架不回轉(zhuǎn)而進(jìn)行垂直運(yùn)動(dòng)，則先由控制單元根據(jù)末端臂架的傾角傳感器采集的末端臂架垂直運(yùn)動(dòng)前后與水平面的夾角值，依據(jù)末端臂架的長(zhǎng)度，計(jì)算出末端臂架在垂直運(yùn)動(dòng)后于水平方向上的垂直變化值，再結(jié)合L_基、L₀和L，計(jì)算出當(dāng)前末端臂架尾端點(diǎn)與L之間的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；從而實(shí)現(xiàn)末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制；

當(dāng)臂架組的基本臂架同時(shí)進(jìn)行回轉(zhuǎn)和垂直方向運(yùn)行時(shí)，則先由控制單元根據(jù)基本臂架的回轉(zhuǎn)角度傳感器采集的基本臂架回轉(zhuǎn)前后的回轉(zhuǎn)角度值以及基本臂架的傾角傳感器采集的基本臂架垂直運(yùn)動(dòng)前后與水平面的夾角值，依據(jù)基本臂架的長(zhǎng)度，計(jì)算出基本臂架在回轉(zhuǎn)和垂直運(yùn)動(dòng)后于水平方向上的垂直變化值，再同理計(jì)算出末端臂架于水平方向上的垂直變化值，將該兩垂直變化值與L₀、L相結(jié)合計(jì)算出當(dāng)前末端臂架尾端點(diǎn)與L之間的差值，然后根據(jù)該差值，發(fā)出垂直距離自動(dòng)調(diào)節(jié)控制指令使末端臂架的伸縮機(jī)構(gòu)運(yùn)動(dòng)計(jì)算出的該差值距離，并通過(guò)該伸縮機(jī)構(gòu)的位移傳感器進(jìn)行伸縮長(zhǎng)度的反饋；從而實(shí)現(xiàn)末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離的自動(dòng)調(diào)節(jié)控制。

下面以圖2所示的臂架組來(lái)詳細(xì)闡述本發(fā)明的實(shí)現(xiàn)方法。但實(shí)際運(yùn)用并不局限于該類(lèi)臂架的組合形式。

圖2所示的臂架組包括基本臂架1和活動(dòng)臂架（即末端臂架）2，該基本臂架1一端設(shè)于支撐裝置3上并設(shè)有回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4，活動(dòng)臂架2上設(shè)有伸縮機(jī)構(gòu)5和回轉(zhuǎn)機(jī)構(gòu)6，該基本臂架與活動(dòng)臂架連接處設(shè)有垂直方向運(yùn)動(dòng)機(jī)構(gòu)7。該回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4設(shè)有回轉(zhuǎn)角度傳感器41及傾角傳感器42，該伸縮機(jī)構(gòu)5設(shè)有位移傳感器51，該回轉(zhuǎn)機(jī)構(gòu)6設(shè)有回轉(zhuǎn)角度傳感器61，該活動(dòng)臂架2設(shè)有傾角傳感器21。上述各機(jī)構(gòu)與傳感器的功能如下：

回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4：可使整個(gè)臂架組進(jìn)行回轉(zhuǎn)及仰俯運(yùn)動(dòng)；

垂直方向運(yùn)動(dòng)機(jī)構(gòu)7：實(shí)現(xiàn)活動(dòng)臂架的仰俯運(yùn)動(dòng)；

伸縮機(jī)構(gòu)5：實(shí)現(xiàn)活動(dòng)臂架的前后伸縮運(yùn)動(dòng)；

回轉(zhuǎn)機(jī)構(gòu)6：實(shí)現(xiàn)活動(dòng)臂架的回轉(zhuǎn)動(dòng)作；

末端臂架尾端點(diǎn)8，本發(fā)明的最終姿態(tài)控制對(duì)象；

回轉(zhuǎn)角度傳感器41：用于測(cè)量回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4當(dāng)前的回轉(zhuǎn)角度值；

傾角傳感器42：用于測(cè)量回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4當(dāng)前的仰俯角度值；

位移傳感器51：用于測(cè)量伸縮機(jī)構(gòu)5的伸縮長(zhǎng)度值；

回轉(zhuǎn)角度傳感器61：用于測(cè)量回轉(zhuǎn)機(jī)構(gòu)6的回轉(zhuǎn)角度值；

傾角傳感器21：用于測(cè)量末端臂架的仰俯角度值。

圖2中標(biāo)示的垂直距離L：即末端臂架尾端點(diǎn)8與支撐裝置3的鉸接點(diǎn)9（基本臂架1與支撐裝置3于鉸接點(diǎn)9鉸接）在其平行水平線上的垂直距離。

本發(fā)明的控制實(shí)現(xiàn)方法如下：

步驟1：通過(guò)人機(jī)交互單元獲取操作者的協(xié)助調(diào)節(jié)控制請(qǐng)求指令（即前面提及的操控指令），該指令包括末端臂架水平方向調(diào)節(jié)指令、末端臂架朝向調(diào)節(jié)指令以及末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)的垂直距離調(diào)節(jié)指令。

步驟2：控制單元接收操作者的操控指令，并通過(guò)傳感單元中各傳感器實(shí)時(shí)采集的各傳感信號(hào)，經(jīng)運(yùn)算處理后輸出相應(yīng)的控制指令，實(shí)現(xiàn)末端臂架的姿態(tài)自動(dòng)控制。對(duì)末端臂架的姿態(tài)自動(dòng)控制包括以下三項(xiàng)控制，控制單元依據(jù)操作者的操控指令，進(jìn)行組合，實(shí)現(xiàn)多模式自動(dòng)控制?？刂七^(guò)程如下：

控制1. 末端臂架水平方向自動(dòng)調(diào)節(jié)：

由傳感單元的傾角傳感器21實(shí)時(shí)反饋末端臂架（即活動(dòng)臂架2）與水平面的夾角θe，操作者通過(guò)人機(jī)交互單元設(shè)定好目標(biāo)夾角值θm（該目標(biāo)夾角值即操作者發(fā)出的末端臂架水平方向調(diào)節(jié)控制指令中的水平調(diào)節(jié)角度），皆發(fā)送至控制單元，控制單元運(yùn)算如下：

設(shè)定θm與θe的最大允許差值為Δθ，當(dāng)操作者操作回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4垂直方向運(yùn)動(dòng)時(shí)，θe將發(fā)生改變，當(dāng)-Δθ>θm-θe或Δθ<θm-θe時(shí)，控制單元輸出對(duì)垂直方向運(yùn)動(dòng)機(jī)構(gòu)7的運(yùn)動(dòng)控制指令至執(zhí)行機(jī)構(gòu)單元使垂直方向運(yùn)動(dòng)機(jī)構(gòu)7運(yùn)動(dòng)，使θm-θe始終保持在允許差值范圍內(nèi)，實(shí)現(xiàn)末端臂架的水平方向自動(dòng)調(diào)節(jié)。

控制2. 末端臂架朝向自動(dòng)調(diào)節(jié)：

由傳感單元的回轉(zhuǎn)角度傳感器41測(cè)得回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4的回轉(zhuǎn)角度值α1，由回轉(zhuǎn)角度傳感器61測(cè)得回轉(zhuǎn)機(jī)構(gòu)6的回轉(zhuǎn)角度值α2，操作者通過(guò)人機(jī)交互單元設(shè)定末端臂架的朝向角度為Δα（該朝向角度即為操作者發(fā)出的臂架末端朝向調(diào)節(jié)控制指令中的朝向調(diào)節(jié)角度），此時(shí)Δα=α1+α2為一定值。

當(dāng)操作者操控回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4進(jìn)行回轉(zhuǎn)時(shí)，α1發(fā)生改變，控制單元輸出對(duì)回轉(zhuǎn)機(jī)構(gòu)6的控制指令至執(zhí)行機(jī)構(gòu)單元使回轉(zhuǎn)機(jī)構(gòu)6運(yùn)行，以使α2值發(fā)生改變，進(jìn)而保持Δα值不變，實(shí)現(xiàn)末端臂架朝向的自動(dòng)調(diào)節(jié)。

控制3. 末端臂架尾端點(diǎn)8與支撐裝置鉸接點(diǎn)9的垂直距離L的自動(dòng)調(diào)節(jié)：

操作者操控回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4進(jìn)行回轉(zhuǎn)或垂直方向運(yùn)動(dòng)時(shí)，通過(guò)控制1及控制2的方法能自動(dòng)調(diào)節(jié)臂架末端的水平位置及朝向，勢(shì)必會(huì)帶來(lái)末端臂架尾端點(diǎn)8與支撐裝置鉸接點(diǎn)9的垂直距離L發(fā)生改變。操作者通過(guò)人機(jī)交互單元設(shè)定好末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離L（該垂直距離L即操作者發(fā)出的末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離調(diào)節(jié)指令中的垂直調(diào)節(jié)距離L）。

當(dāng)回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4發(fā)生垂直方向的運(yùn)行時(shí)，例如向上運(yùn)動(dòng)時(shí)，會(huì)使L值變小，其計(jì)算方法為：見(jiàn)圖3所示，當(dāng)回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4發(fā)生垂直方向運(yùn)動(dòng)時(shí)，傾角傳感器42測(cè)得的角度值由θ1變化為θ2，該基本臂架的總長(zhǎng)度為L(zhǎng)1，則L2=cosθ1*L1- cosθ2*L1，即為末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)在垂直距離上的變化值。如此，便可通過(guò)控制單元經(jīng)運(yùn)算處理后輸出對(duì)調(diào)節(jié)伸縮機(jī)構(gòu)5的控制指令至執(zhí)行機(jī)構(gòu)單元使伸縮機(jī)構(gòu)5運(yùn)動(dòng)，并通過(guò)位移傳感器51進(jìn)行伸縮長(zhǎng)度的反饋，可保持末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離L不變。

當(dāng)回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4發(fā)生回轉(zhuǎn)方向運(yùn)行，致使末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)之間的垂直距離L減小時(shí)，其計(jì)算方法如下：

見(jiàn)圖4所示，α1為回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4與水平面的當(dāng)前夾角，即回轉(zhuǎn)角度傳感器41的測(cè)量值，該基本臂架的總長(zhǎng)度為L(zhǎng)1，當(dāng)回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4發(fā)生回轉(zhuǎn)方向運(yùn)行時(shí)，該臂在水平方向的等效長(zhǎng)度L3=L1*cosα1。

當(dāng)回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4發(fā)生回轉(zhuǎn)方向運(yùn)動(dòng)時(shí)，見(jiàn)圖5所示，回轉(zhuǎn)角度傳感器41測(cè)得的角度值由α1變化為α2，則L4=cosα1*L1- cosα2*L1，為回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4回轉(zhuǎn)帶來(lái)的末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)在垂直距離上的變化值。同時(shí)，當(dāng)回轉(zhuǎn)及垂直方向運(yùn)動(dòng)機(jī)構(gòu)4發(fā)生回轉(zhuǎn)方向運(yùn)動(dòng)帶來(lái)回轉(zhuǎn)機(jī)構(gòu)6發(fā)生反方向的調(diào)整運(yùn)動(dòng)時(shí)，按上述同樣方法可計(jì)算出回轉(zhuǎn)機(jī)構(gòu)6回轉(zhuǎn)帶來(lái)的末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)在垂直距離上的變化值。兩個(gè)變化值變化趨勢(shì)相反，結(jié)合處理即為末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)在垂直距離上的實(shí)際變化值，通過(guò)調(diào)節(jié)伸縮機(jī)構(gòu)5運(yùn)動(dòng)，并通過(guò)位移傳感器51進(jìn)行伸縮長(zhǎng)度的反饋，能保持末端臂架尾端點(diǎn)與支撐裝置鉸接點(diǎn)的垂直距離L不變。