本發(fā)明涉及一種機械臂及其控制方法，特別是一種電動汽車自動充電樁用六自由度機械臂及其控制方法。

背景技術：

電動汽車是汽車工業(yè)的發(fā)展趨勢，充電樁作為電動汽車的必不可少的附屬設施，具有極其廣闊的應用前景。但現(xiàn)有的充電樁均為手動型，由于充電槍連接著沉重的電線，整體質(zhì)量較大，因此手動充電存在諸多不便，對一些力氣比較小的人士，把充電槍插進電動汽車上的受電接口存在較大的困難。同時，隨著科技的進步，立體車庫以及無人自助充電站相應而生，這些都對充電樁能夠給電動汽車自動充電提出了需求。

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

本發(fā)明所要解決的技術問題是提供一種電動汽車自動充電樁用六自由度機械臂及其控制方法，它能夠自動將充電槍插入電動汽車上的受電接口。

為解決上述技術問題，本發(fā)明所采用的技術方案是：

一種電動汽車自動充電樁用六自由度機械臂，其特征在于：包含底盤旋轉(zhuǎn)舵機、旋轉(zhuǎn)底盤、大臂前傾舵機、中臂前傾舵機、小臂前傾舵機、第一支架、第二支架、充電槍旋轉(zhuǎn)舵機、充電槍、充電槍支架和充電槍俯仰舵機，旋轉(zhuǎn)底盤轉(zhuǎn)動設置在充電樁側(cè)面并且由底盤旋轉(zhuǎn)舵機驅(qū)動，大臂前傾舵機固定在旋轉(zhuǎn)底盤外側(cè)，第一支架一端設置在大臂前傾舵機上由大臂前傾舵機驅(qū)動，第一支架另一端設置在中臂前傾舵機上由中臂前傾舵機驅(qū)動，第二支架一端固定在中臂前傾舵機端部，第二支架另一端設置在小臂前傾舵機上由小臂前傾舵機驅(qū)動，充電槍旋轉(zhuǎn)舵機固定在小臂前傾舵機端部，充電槍支架設置在充電槍旋轉(zhuǎn)舵機上由充電槍旋轉(zhuǎn)舵機驅(qū)動，充電槍轉(zhuǎn)動設置在充電槍支架上并且充電槍與充電槍俯仰舵機連接由充電槍俯仰舵機驅(qū)動。

進一步地，所述第一支架由兩個u型支架固定構成，第二支架為u型支架。

進一步地，所述底盤旋轉(zhuǎn)舵機、大臂前傾舵機、中臂前傾舵機、小臂前傾舵機、充電槍旋轉(zhuǎn)舵機和充電槍俯仰舵機均與自動充電臂控制系統(tǒng)連接由自動充電臂控制系統(tǒng)控制驅(qū)動。

進一步地，所述充電槍支架上設置有充電槍定位裝置。

進一步地，所述充電槍定位裝置包含無線通信模塊1、無線通信模塊2、psd、激光準直發(fā)射器、受電接口中心強磁鐵和線性霍爾傳感器，無線通信模塊1、psd和受電接口中心強磁鐵設置在電動汽車尾部，無線通信模塊2、激光準直發(fā)射器和線性霍爾傳感器設置在充電槍支架上。

進一步地，電動汽車尾部無線通信模塊1與車主智能手機無線連接由智能手機內(nèi)置app控制操作。

一種電動汽車自動充電樁用六自由度機械臂的控制方法，其特征在于包含以下步驟：

步驟一：車主將電動汽車開到車位上后，在手機app上按下自動充電按鈕；

步驟二：自動充電臂收到指令后，使充電臂頂部的充電槍上的線性霍爾傳感器逐漸接近電動汽車受電接口中心處的強磁鐵的磁場，線性霍爾傳感器通過感知電動汽車尾部位于受電接口中心處的強磁鐵的磁感應強度，對充電槍的位置進行定位；

步驟三：通過線性霍爾傳感器定位將充電槍移動到受電接口中心處附近；

步驟四：通過激光準直發(fā)射器和psd進一步進行充電槍精確定位，并將充電槍移動到受電接口中心位置。

進一步地，所述步驟二中線性霍爾傳感器控制充電槍定位的具體過程為，線性霍爾傳感器在磁場中的位置(x0，y0)與線性霍爾傳感器兩端的電壓u0的關系式為

其中，u為霍爾傳感器輸出電壓，dsp首先控制充電臂使線性霍爾傳感器向右移動0.5mm，此時線性霍爾傳感器兩端電壓變化為u01，接著dsp控制充電臂使線性霍爾傳感器向上移動0.5mm，此時電壓變化為u02，將u01和u02代入關系式(1)，即可得到充電槍當前在磁場中的位置坐標(x0，y0)；由于磁場粗略定位模式下充電槍槍口與電動汽車受電接口之間的距離z0已設定好，即此時z0為已知值，因此充電槍在空間中的位置坐標為(x0，y0，z0)，其中z0為已知值。

進一步地，所述步驟三中充電槍移動控制過程為，

充電臂充電槍的位置坐標為(x0，y0，z0)，磁場中心即電動汽車上的受電接口中心的坐標為(0，0，0)，因此可以得到充電槍相對受電接口的位置關系為(x0，y0，z0)；

將充電臂的固定基原點定位絕對坐標系1(x1，y1，z1)，其原點位置在底盤中心線與舵機轉(zhuǎn)動軸的交點上；充電臂的大臂繞底盤旋轉(zhuǎn)舵機的轉(zhuǎn)動軸前傾，其舵機坐標系2表示為(x2，y2，z2)；充電臂的中臂繞大臂舵機的旋轉(zhuǎn)軸前傾，其舵機坐標系3表示為(x3，y3，z3)；充電臂的小臂繞中臂舵機的旋轉(zhuǎn)軸前傾，其舵機坐標系4表示為(x4，y4，z4)；充電槍旋轉(zhuǎn)舵機控制充電槍繞小臂的垂直方向轉(zhuǎn)動，其坐標5表示為(x5，y5，z5)；充電槍俯仰舵機控制充電槍在小臂的水平方向旋轉(zhuǎn)，其坐標6表示為(x6，y6，z6)；充電槍中心點p即為充電槍在坐標系6中的位置點；

建立相鄰兩坐標系i-1與i之間的相對關系，此坐標系變換通式如式(2)所示：

表示由z軸平移和z軸旋轉(zhuǎn)，x軸平移和x軸旋轉(zhuǎn)的四種運動變換矩陣的結果；其中，αi-1為第i坐標系繞第i-1坐標系的xi-1軸的轉(zhuǎn)角；ai-1為第i坐標系繞第i-1坐標系的xi-1軸的移動距離；θi為第i坐標系繞第zi軸的轉(zhuǎn)角；di為第i坐標系繞第zi軸的移動距離；

根據(jù)自動充電臂的坐標變化通式和充電臂臂長d—h參數(shù)表，可以得到各連桿變化矩陣，將從底盤基座到大臂、中臂、小臂直至最后的充電槍所在的關節(jié)的所有變換結合起來，就得到了自動充電機械臂的總變換矩陣，即充電槍相對于底盤固定基1的變換矩陣：

得到總變換矩陣之后，就可以進行自動充電臂的6個舵機的運動參數(shù)的計算，控制自動充電臂上的充電槍運動到電動汽車上的受電接口附近。

進一步地，所述步驟四激光準直發(fā)射器和psd進一步進行充電槍精確定位過程為，自動充電臂上的充電槍運動到電動汽車上的受電接口附近時，激光準直發(fā)射器啟動并射出激光對psd進行照射，然后從psd傳出的電流信號經(jīng)位置信號處理電路進行反相、放大、保持后送入位置計算主控芯片，由位置計算主控芯片根據(jù)式結合psd的規(guī)格和引腳分布，將相應引腳的光電流進行加減與除法運算，得到光敏面上光斑的坐標(x7，y7)，從而得到充電槍與電動汽車受電接口之間精確的位置關系(x7，y7，z0)，其中z0為充電槍槍口與電動汽車受電接口之間的距離，z0為已知值。

本發(fā)明與現(xiàn)有技術相比，具有以下優(yōu)點和效果：本發(fā)明通過六自由度機械臂的聯(lián)動控制實現(xiàn)自動充電臂上充電槍的空間運動，通過線性霍爾傳感器感應電動汽車受電接口中心處磁鐵的磁場，進行簡單定位，之后再通過電動汽車上的位置傳感器psd感應充電臂上的激光準直發(fā)射器發(fā)出的激光，進行精確定位，最終鎖定電動汽車受電接口的位置，使充電槍順利插入電動汽車上的受電接口。

附圖說明

圖1是本發(fā)明的一種電動汽車自動充電樁用六自由度機械臂的示意圖。

圖2是本發(fā)明的一種電動汽車自動充電樁用六自由度機械臂的使用狀態(tài)圖。

圖3是本發(fā)明的一種電動汽車自動充電樁用六自由度機械臂的模塊連接圖。

圖4是線性霍爾感應器特性曲線圖。

圖5是本發(fā)明的磁感應線分布示意圖。

圖6是本發(fā)明的機械臂初始狀態(tài)對應的d-h參數(shù)表。

圖7是本發(fā)明的激光準直光斑位置檢測系統(tǒng)框圖。

圖8是本發(fā)明一種電動汽車自動充電樁用六自由度機械臂的控制系統(tǒng)的流程圖。

具體實施方式

下面結合附圖并通過實施例對本發(fā)明作進一步的詳細說明，以下實施例是對本發(fā)明的解釋而本發(fā)明并不局限于以下實施例。

如圖1所示，本發(fā)明的一種電動汽車自動充電樁用六自由度機械臂，包含底盤旋轉(zhuǎn)舵機1、旋轉(zhuǎn)底盤2、大臂前傾舵機3、中臂前傾舵機4、小臂前傾舵機5、第一支架6、第二支架7、充電槍旋轉(zhuǎn)舵機8、充電槍9、充電槍支架10和充電槍俯仰舵機11，旋轉(zhuǎn)底盤2轉(zhuǎn)動設置在充電樁側(cè)面并且由底盤旋轉(zhuǎn)舵機1驅(qū)動，大臂前傾舵機3固定在旋轉(zhuǎn)底盤2外側(cè)，第一支架6一端設置在大臂前傾舵機3上由大臂前傾舵機3驅(qū)動，第一支架6另一端設置在中臂前傾舵機4上由中臂前傾舵機4驅(qū)動，第二支架7一端固定在中臂前傾舵機4端部，第二支架7另一端設置在小臂前傾舵機5上由小臂前傾舵機5驅(qū)動，充電槍旋轉(zhuǎn)舵機8固定在小臂前傾舵機5端部，充電槍支架10設置在充電槍旋轉(zhuǎn)舵機8上由充電槍旋轉(zhuǎn)舵機8驅(qū)動，充電槍9轉(zhuǎn)動設置在充電槍支架10上并且充電槍9與充電槍俯仰舵機11連接由充電槍俯仰舵機11驅(qū)動。

第一支架由兩個u型支架固定構成，第二支架為u型支架。底盤旋轉(zhuǎn)舵機、大臂前傾舵機、中臂前傾舵機、小臂前傾舵機、充電槍旋轉(zhuǎn)舵機和充電槍俯仰舵機均與自動充電臂控制系統(tǒng)連接由自動充電臂控制系統(tǒng)控制驅(qū)動。

充電槍支架上設置有充電槍定位裝置。如圖3所示，充電槍定位裝置包含無線通信模塊1、無線通信模塊2、psd、激光準直發(fā)射器、受電接口中心強磁鐵和線性霍爾傳感器，無線通信模塊1、psd和受電接口中心強磁鐵設置在電動汽車尾部，無線通信模塊2、激光準直發(fā)射器和線性霍爾傳感器設置在充電槍支架上。電動汽車尾部無線通信模塊1與車主智能手機無線連接由智能手機內(nèi)置app控制操作。

如圖7所示，激光準直光斑位置檢測系統(tǒng)包含psd、位置信號處理電路、數(shù)據(jù)采集模塊、接口電路、a/d轉(zhuǎn)換模塊和位置計算主控芯片。自動充電臂上的充電槍運動到電動汽車上的受電接口附近時，激光準直發(fā)射器啟動并射出激光對psd進行照射，然后從psd傳出的電流信號經(jīng)位置信號處理電路進行反相、放大、保持后送入位置計算主控芯片，由位置計算主控芯片將相應引腳的光電流進行加減與除法運算，得到光敏面上光斑的坐標，從而得到充電槍與電動汽車受電接口之間精確的位置關系。

車主將電動汽車開到車位上后，在手機app上按下自動充電按鈕。自動充電臂收到指令后，使充電臂由圖1所示的初始狀態(tài)逐漸運動到圖2所示的狀態(tài)，使充電臂頂部的充電槍上的線性霍爾傳感器逐漸接近電動汽車受電接口中心處的強磁鐵的磁場，線性型霍爾集成傳感器的輸出電壓與外加磁場強度呈線性關系。如圖4所示，在磁感應強度±0.25t的范圍內(nèi)，線性霍爾傳感器輸出的電壓u0與磁感應強度成正比。

如圖8所示，一種電動汽車自動充電樁用六自由度機械臂的控制方法，其特征在于包含以下步驟：

步驟一：車主將電動汽車開到車位上后，在手機app上按下自動充電按鈕。

步驟二：自動充電臂收到指令后，使充電臂頂部的充電槍上的線性霍爾傳感器逐漸接近電動汽車受電接口中心處的強磁鐵的磁場，線性霍爾傳感器通過感知電動汽車尾部位于受電接口中心處的強磁鐵的磁感應強度，對充電槍的位置進行定位。

線性霍爾傳感器通過感知電動汽車尾部位于受電接口中心處的強磁鐵的磁感應強度，對充電槍的位置進行定位。強磁鐵磁感應強度分布示意簡圖如圖5所示，由圖5可以看出，磁感應線分布模型磁場各點的磁場強度不同，因此相同位移下，由上一個運動點到下一個運動點的磁場強度差值不同，線性霍爾傳感器可以感應到磁場的細微變化，并表現(xiàn)為不同的輸出電壓，通過放大電路，可以將這種電壓差值進行放大。經(jīng)推導，磁場強度的分布通式如式(1)所示，線性霍爾傳感器在磁場中的位置(x0，y0)與線性霍爾傳感器兩端的電壓u0的關系式為：

其中u為霍爾傳感器輸出電壓。如圖5所示，dsp首先控制充電臂使線性霍爾傳感器向右移動0.5mm，此時線性霍爾傳感器兩端電壓變化為u01，接著dsp控制充電臂使線性霍爾傳感器向上移動0.5mm，此時電壓變化為u02，將u01和u02代入式(1)，即可得到充電槍當前在磁場中的位置坐標(x0，y0)。由于磁場粗略定位模式下充電槍槍口與電動汽車受電接口之間的距離z0已設定好，即此時z0為已知值，因此充電槍在空間中的位置坐標為(x0，y0，z0)，其中z0為已知值。

步驟三：通過線性霍爾傳感器定位將充電槍移動到受電接口中心處附近。

自動充電臂有底盤、大臂、中臂、小臂、充電槍旋轉(zhuǎn)和充電槍俯仰六個自由度，自動充電臂控制系統(tǒng)通過控制6個舵機，可以實現(xiàn)充電槍的空間運動。自動充電臂控制系統(tǒng)根據(jù)磁場模型和掃描控制算法計算出充電臂充電槍的位置坐標(x0，y0，z0)后，由于磁場中心即電動汽車上的受電接口中心的坐標為(0，0，0)，因此可以得到充電槍相對受電接口的位置關系(x0，y0，z0)，之后開始進行六個舵機的運動參數(shù)的計算。

將充電臂的固定基原點定位絕對坐標系1(x1，y1，z1)，其原點位置在底盤中心線與舵機轉(zhuǎn)動軸的交點上。充電臂的大臂繞底盤旋轉(zhuǎn)舵機的轉(zhuǎn)動軸前傾，其舵機坐標系2表示為(x2，y2，z2)。充電臂的中臂繞大臂舵機的旋轉(zhuǎn)軸前傾，其舵機坐標系3表示為(x3，y3，z3)。充電臂的小臂繞中臂舵機的旋轉(zhuǎn)軸前傾，其舵機坐標系4表示為(x4，y4，z4)。充電槍旋轉(zhuǎn)舵機控制充電槍繞小臂的垂直方向轉(zhuǎn)動，其坐標5表示為(x5，y5，z5)。充電槍俯仰舵機控制充電槍在小臂的水平方向旋轉(zhuǎn)，其坐標6表示為(x6，y6，z6)。充電槍中心點p即為充電槍在坐標系6中的位置點。對全部舵機規(guī)定坐標系后，可建立相鄰兩坐標系i-1與i之間的相對關系，這種關系可由表示坐標系i-1與i相對位置的四個齊次變換來描述，此坐標系變換通式表示如式(2)所示：

式(2)表示表示由z軸平移和z軸旋轉(zhuǎn)，x軸平移和x軸旋轉(zhuǎn)的四種運動變換矩陣的結果；公式中：ai-1為第i坐標系繞第i-1坐標系的xi-1軸的轉(zhuǎn)角；ai-1為第i坐標系繞第i-1坐標系的xi-1軸的移動距離；θi為第i坐標系繞第zi軸的轉(zhuǎn)角；di為第i坐標系繞第zi軸的移動距離。

當自動充電機械臂各連桿的坐標系即(x1，y1，z1)到(x6，y6，z6)被規(guī)定之后，就能夠列出各連桿的常量參數(shù)。矩陣就成為關節(jié)變量θ(底盤旋轉(zhuǎn)舵機、充電槍旋轉(zhuǎn)舵機、充電槍俯仰舵機運動參數(shù))的函數(shù)或變量d(大臂前傾舵機、中臂前傾舵機、小臂前傾舵機運動參數(shù))的函數(shù)。本發(fā)明設計的自動充電臂的初始狀態(tài)對應的d-h參數(shù)如圖6的表1所示。

根據(jù)自動充電臂的坐標系變化通式和充電臂臂長d—h參數(shù)表，可以得到各臂的變化矩陣，將從底盤基座到大臂、中臂、小臂直至最后的充電槍所在的關節(jié)的所有變換結合起來，就得到了自動充電機械臂的總變換矩陣，即充電槍相對于底盤固定基1的變換矩陣：

得到總變換矩陣之后，就可以進行自動充電臂的6個舵機的運動參數(shù)的計算。自動充電臂控制系統(tǒng)根據(jù)磁場模型和掃描控制算法計算出充電臂充電槍的位置坐標(x0，y0)后，通過式(2)和式(3)所示的自動充電臂的運動方程，計算出六個舵機的運動參數(shù)(該參數(shù)里面包含了各個舵機的運動參數(shù))，控制自動充電臂上的充電槍運動到電動汽車上的受電接口附近。

步驟四：通過激光準直發(fā)射器和psd進一步進行充電槍精確定位，并將充電槍移動到受電接口中心位置。

線性霍爾傳感器感應中心磁鐵的磁場，自動充電臂控制系統(tǒng)根據(jù)這一數(shù)據(jù)控制充電臂運動到受電接口中心處，但由于充電臂運動過程中的累積誤差的影響，充電槍不能精確地移動到正對電動汽車受電接口的位置，為實現(xiàn)充電槍與受電接口的精確定位，本發(fā)明使用光電傳感器對受電接口進行精確定位，通過無線模塊反饋受電接口的位置信息，從而構成閉環(huán)跟蹤控制系統(tǒng)。

光電傳感器主要由激光準直發(fā)射器和二維psd位置傳感器組成，激光準直發(fā)射器發(fā)出的激光照射在psd上時在psd上產(chǎn)生一個光斑，二維psd具有2個方向垂直且相互獨立的光敏層，能夠分別感知光斑在x，y兩個方向上的位置。psd光敏面上光斑的坐標(x7，y7)與電極輸出的光電流之間的關系如式(4)所示：

激光準直光斑位置檢測系統(tǒng)框圖如圖7所示，自動充電臂上的充電槍運動到電動汽車上的受電接口附近時，激光準直發(fā)射器啟動并射出激光對psd進行照射，然后從psd傳出的電流信號經(jīng)位置信號處理電路進行反相、放大、保持后送入位置計算主控芯片，由位置計算主控芯片根據(jù)式(4)，結合psd的規(guī)格和引腳分布，將相應引腳的光電流進行加減與除法運算，得到光敏面上光斑的坐標(x7，y7)，從而得到充電槍與電動汽車受電接口之間精確的位置關系(x7，y7，z0)，其中z0為充電槍槍口與電動汽車受電接口之間的距離，z0為已知值。

獲得充電槍與電動汽車受電接口之間精確的位置關系(x7，y7，z0)之后，汽車尾部的無線通信模塊將這一坐標發(fā)送給自動充電臂上的無線通信模塊。自動充電臂主控芯片得到坐標(x7，y7，z0)之后，結合式(2)和式(3)所示的六自由度運動方程計算出舵機的運動參數(shù)使充電槍運動到受電接口正前方，之后自動充電臂主控系統(tǒng)啟動充電槍與受電接口對接程序，控制充電槍前移與受電接口進行對接。當充電完成或者接收到停止充電信號后，充電槍與受電接口斷開，自動充電臂自動回到初始位置。

本說明書中所描述的以上內(nèi)容僅僅是對本發(fā)明所作的舉例說明。本發(fā)明所屬技術領域的技術人員可以對所描述的具體實施例做各種修改或補充或采用類似的方式替代，只要不偏離本發(fā)明說明書的內(nèi)容或者超越本權利要求書所定義的范圍，均應屬于本發(fā)明的保護范圍。