[發明專利]一種移乘機器人

有效

• 申請號: 201810609935.8
• 申請日: 2018-06-13
• 公開（公告）號: CN108724213B
• 公開（公告）日: 2021-03-30
• 發明（設計）人: 鐘鳴;姚玉峰;林川
• 申請（專利權）人: 哈爾濱工業大學（威海）
• 主分類號: B25J11/00
• 分類號: B25J11/00
• 代理公司: 威海科星專利事務所 37202
• 代理人: 孫小棟

1.一種使用移乘機器人的移乘方法，其特征在于，所述移乘機器人包括左機械臂、右機械臂和底盤，所述左機械臂包括左底座、左箱體一、左箱體二、左小臂驅動電機、左小臂減速器、左小臂傳動機構、左小臂驅動連桿、左大臂驅動電機、左大臂減速器、左大臂傳動機構、左大臂、左小臂傳動連桿、左小臂、旋轉關節一、旋轉關節二、旋轉關節三、旋轉關節四、旋轉關節五、旋轉關節六、左三角形保持架、左末端姿態保持連桿、左三角形保持架連桿、旋轉關節七、左承重托板、左末端連桿和旋轉關節九，所述左箱體一和左箱體二連接于左底座上，所述左小臂驅動電機和左小臂減速器連接于左箱體一上，所述左大臂驅動電機和左大臂減速器連接于左箱體二上；所述左小臂驅動電機的輸出軸通過左小臂傳動機構與左小臂減速器的輸入端連接，左小臂驅動連桿的一端與左小臂減速器的輸出端連接；所述左大臂驅動電機的輸出軸通過左大臂傳動機構與左大臂減速器的輸入端連接，左大臂的一端與左大臂減速器的輸出端連接，左小臂傳動連桿的一端通過旋轉關節一與左小臂驅動連桿的另一端連接；左小臂的一端通過旋轉關節九與左末端連桿的一端連接，左小臂的另一端設有左小臂旋轉關節連接部，左小臂旋轉關節連接部通過旋轉關節三與左三角形保持架連接，左小臂傳動連桿的另一端通過旋轉關節二與左小臂旋轉關節連接部連接；左大臂的另一端通過旋轉關節與左三角形保持架連接，左大臂和左小臂在左三角形保持架上的轉動軸線是重合的，左末端姿態保持連桿的一端通過旋轉關節四與左三角形保持架連接，左末端姿態保持連桿的另一端通過旋轉關節六與左末端連桿的另一端連接；左三角形保持架連桿的一端通過旋轉關節五與左三角形保持架連接，左三角形保持架連桿的另一端通過旋轉關節七與左箱體二連接，所述左承重托板與左末端連桿連接；

所述右機械臂包括右底座、右箱體一、右箱體二、右小臂驅動電機、右小臂減速器、右小臂傳動機構、右小臂驅動連桿、右大臂驅動電機、右大臂減速器、右大臂傳動機構、右大臂、右小臂傳動連桿、右小臂、旋轉關節八、旋轉關節十一、旋轉關節十三、右三角形保持架、右末端姿態保持連桿、右三角形保持架連桿、右承重托板和右末端連桿，所述右箱體一和右箱體二連接于右底座上，所述右小臂驅動電機和右小臂減速器連接于右箱體一上，所述右大臂驅動電機和右大臂減速器連接于右箱體二上；所述右小臂驅動電機的輸出軸通過右小臂傳動機構與右小臂減速器的輸入端連接，右小臂驅動連桿的一端與右小臂減速器的輸出端連接；右大臂驅動電機的輸出軸通過右大臂傳動機構與右大臂減速器的輸入端連接，右大臂的一端與右大臂減速器的輸出端連接，右小臂傳動連桿的一端通過旋轉關節八與右小臂驅動連桿的另一端連接；右小臂的一端通過旋轉關節與右末端連桿的一端連接，右小臂的另一端設有右小臂旋轉關節連接部，右小臂旋轉關節連接部通過旋轉關節與右三角形保持架連接，右小臂傳動連桿的另一端通過旋轉關節與右小臂旋轉關節連接部連接；右大臂的另一端通過一個旋轉關節與右三角形保持架連接，右大臂和右小臂在右三角形保持架上的轉動軸線是重合的；右末端姿態保持連桿的一端通過旋轉關節十一與右三角形保持架連接，右末端姿態保持連桿的另一端通過旋轉關節十三與右末端連桿的另一端連接；右三角形保持架連桿的一端通過旋轉關節與右三角形保持架連接，右三角形保持架連桿的另一端通過旋轉關節與右箱體二連接，右承重托板與右末端連桿連接；

所述左底座與底盤連接，所述右底座與底盤連接；

所述左底座和右底座與底盤之間設有絲杠副，絲杠副連接有絲杠副驅動電機，絲杠副與底盤連接，所述絲杠副設有螺母座一和螺母座二，所述左底座與螺母座一連接，所述右底座與螺母座二連接；所述底盤連接有兩個腳輪和兩個麥克納姆輪；

所述移乘機器人還包括控制系統，所述控制系統包括上位機、控制器一、控制器二、驅動器一、驅動器二、編碼器一、編碼器二、驅動器三、驅動器四、編碼器三和編碼器四，所述控制器一與上位機連接，控制器二與上位機1連接；所述控制器一設有串口一，所述驅動器一的輸入端與串口一連接，驅動器一的輸出端與左小臂驅動電機連接；所述驅動器二的輸入端與串口一連接，左大臂驅動電機與驅動器二的輸出端連接，所述編碼器一與左小臂驅動電機連接，編碼器一的信號輸出端與控制器一連接；編碼器二與左大臂驅動電機連接，編碼器二的信號輸出端與控制器一連接；所述控制器二設有串口二，驅動器三的輸入端與串口二連接，驅動器三的輸出端與右小臂驅動電機連接；驅動器四的輸入端與串口二連接，右大臂驅動電機與驅動器四的輸出端連接；編碼器三與右小臂驅動電機連接，編碼器三的信號輸出端與控制器二連接，編碼器四與右大臂驅動電機連接，編碼器四的信號輸出端與控制器二連接；所述上位機設有ROS系統和顯示器，ROS系統設有RViz仿真模塊、RViz顯示模塊、控制模塊、數據處理模塊，所述數據處理模塊與RViz仿真模塊連接，控制模塊與數據處理模塊連接通信；

所述移乘方法包括以下步驟：

先建立好三維模型，顯示與實際左機械臂和右機械臂相同尺寸的三維模型；

利用ROS中的RViz仿真模塊調用三維模型，實現三維模型和實際機械臂各關節實時關聯；當上位機控制RViz仿真模塊中三維模型的運動時，左機械臂和右機械臂跟隨運動并向RViz仿真模塊實時反饋各關節狀態信息，當實際機械臂的反饋狀態未達到三維模型狀態時，上位機計算差值后繼續發送指令控制機械臂運動，直至左機械臂和右機械臂運動到達設定的目標位置；

當上位機得到外部輸入的運動到指定位置的運動指令，經過數據處理模塊進行逆解得到各關節運動參數，并將運動參數數據發給RViz仿真模塊；

逆解的具體過程是：機械臂的正運動學問題是已知關節變量θ₁₁、θ₂₁，求解末端執行器左承重托板或右承重托板相對于基坐標系的位姿；當移乘機器人的底盤固定不動時，左機械臂或右機械臂僅在平面中運動，可以將正運動學問題進行簡化，以左機械臂為例，先確定左末端連桿的下端在平面中的位置E(x₀,y₀)，然后結合底盤的運動便可以確定左承重托板在空間中的位姿，采用向量法進行計算求解：

在固定參考系A-xy下構造閉環方程：

公式(2-1)中

為C點的位矢，k₁₁、k₁₂、k₂₁、k₂₂分別代表AD桿、CD桿、AB桿、BC桿的長度，分別表示原動件AD桿、AB桿的單位矢量，分別表示從動件CD桿、BC桿的單位矢量；

將公式(2-1)兩邊取模的平方得：

i＝1,2時，將(2-2)、(2-3)相加后分別得到：

即：

公式(2-4)和(2-5)相減后得到：

公式(2-7)展開得：

y₂＝Dx₂+E (2-8)

公式(2-8)中，

將式(2-8)代入式(2-6)得：

公式(2-9)中，F＝D²+1，G_i＝DE-k_i1cosθ₂₁-Dk_i1sinθ₂₁，

由此得出兩組正解，根據裝配模式，取

將(2-10)代入(2-8)便可確定y₂值，得到位矢C(x₂,y₂)^T；

為保證機械臂的穩定工作，∠GED、∠HGE、∠ABC角度值不能低于所以需要將無效解剔除，首先驗證∠GED、∠HGE的有效性，

x₁＝l₁cosθ₁₁ (2-11)

y₁＝l₁sinθ₁₁ (2-12)

x₃＝l₁cosθ₁₁+l₇cos∠HDI (2-13)

y₃＝l₁sinθ₁₁+l₇sin∠HDI (2-14)

求得D(x₁,y₁)、H(x₃,y₃)；

由于∠GED+∠HGE＝π，∠GED＝∠CDH，所以剔除不滿足的C(x₂,y₂)坐標；

對于∠ABC，已知A(0,0)，C(x₂,y₂)，B點坐標可求

x₄＝l₂cosθ₂₁ (2-15)

y₄＝l₂sinθ₂₁ (2-16)

剔除不滿足的C(x₂,y₂)坐標；

至此，有效的C(x₂,y₂)坐標便可唯一確定；

已知C點、D點、E點共線，則必存在不與直線CD共線的點Q(x₅,y₅)，以及實數u，使得：

則有

x₀＝ux₂+(1-u)x₁ (2-17)

y₀＝uy₂+(1-u)y₁ (2-18)

由DE桿為定長l₅可得

將式(2-17)、式(2-18)代入式(2-19)中，可得

式(2-20)代入式(2-17)、式(2-18)便可求得E點位矢(x₀,y₀)^T；

機械臂的逆運動學問題是已知末端執行器的位姿，來求解該位姿對應的關節轉角θ₁₁、θ₂₁，求解機器人運動學反解是為了得到各運動關節的變化參數；

已知A(0,0)、E(x₀,y₀)，且桿DE、AD分別為定長l₅、l₁，則有

式(2-21)、式(2-22)聯立后可得

y₁＝ax₁+b (2-23)

其中，

式(2-21)、式(2-23)聯立后可得

式(2-24)代入式(2-23)后便可求得y₁，取較大的y₁值及其所對應的x₁；為保證機械臂的穩定工作，∠GED、∠EDH角度值不能低于所以需要將無效解剔除。由于∠GED+∠EDH＝π，所以剔除不滿足

的D(x₁,y₁)；

左大臂的轉角x₁≠0；

x₁＝0時，大臂與水平面垂直，此時有唯一解

已知C點、D點、E點共線，則必存在不與直線ED共線的點P(x₆,y₆)，以及實數λ，使得

則有:

x₂＝λx₁+(1-λ)x₀ (2-25)

y₂＝λy₁+(1-λ)y₀ (2-26)

由CD桿為定長l₄可得

將式(2-25)、式(2-26)和式(2-27)聯立可得

取

將式(2-28)代入式(2-25)、式(2-26)即可求得C點位矢(x₂,y₂)^T；

在固定參考系A-xy下構造閉環方程

兩邊取模的平方后相加得

得到三角方程

Asinθ₂₁+Bcosθ₂₁＝C (2-31)

其中，A＝2y₂k₂₁，B＝2x₂k₂₁，

利用三角代換A＝ρ·cosφ，B＝ρ·sinφ，則φ＝atan2(B,A)。

式(2-31)可化簡為則

所以

小臂轉角

為避免機械臂工作過程中發生紊亂，還需要剔除不滿足

的θ₂₁；

由此便求得移乘機器人機械臂的運動學逆解；

上位機通過solidwork軟件生成三維模型，生成三維模型的各個連桿的參數模型，上位機完成各個關節參數信息的定義；

將關節變量θ₁₁、θ₂₁數據發給RViz仿真模塊；

RViz仿真模塊調用已經建立好的三維模型；

RViz顯示模塊讀取三維模型的各個連桿的參數信息；

RViz顯示模塊將各個連桿的參數信息發送給結構轉換樹生成模塊；

結構轉換樹生成模塊建立RViz基坐標和機械臂基坐標之間的聯系；

結構轉換樹生成模塊建立機械臂各關節坐標之間聯系；

結構轉換樹生成模塊生成結構轉換樹；

RViz顯示模塊調用結構轉換樹顯示理想的虛擬機械臂；

編碼器一和編碼器二發送的運動角度數據給數據處理模塊，數據處理模塊計算出各個連桿可旋轉的關節的運動角度數據，RViz仿真模塊調用各個連桿可旋轉的關節的運動角度數據，RViz顯示模塊顯示實際的虛擬機械臂。