基于Matlab的配網帶電作業機器人正逆運動學仿真

文/李帥 張金祿 么軍 李威 何玉龍

1 引言

電力工業是國民經濟的基礎，配電網又是其關鍵環節。當線路發生突發故障時，停電檢修對社會經濟影響巨大，嚴重影響百姓日常生活；另外，停電以及檢修完畢之后的上電都會對系統內的設備造成沖擊傷害，影響設備的使用壽命。傳統意義上的帶電作業，作業人員通過絕緣作業工具與帶電設備接觸，檢修作業強度大，危險性高，人員受傷情況難以避免。

因此，通過研發一種配網帶電作業機器人，不僅可減少作業時間即停電時間，同時還可以提高作業的可靠性，保護帶電作業人員，就顯得尤為重要。

2 機器人D-H法

Denavit-Hartenbery法簡稱D-H法。該方法于1956年由上述兩位學者提出，絕大多數機械臂的數學模型可由該方法構建。絕大部分的關節型機械手臂，按順序將轉動副和移動副固定在連桿上，連桿不在同一平面，并且旋轉軸彼此不平行。連桿坐標建立方式如圖1所示。

Ci-1、Ci、Ci+1三個不同連桿對應各自關節i-1，i，i+1，Ji、Ji+1為關節i和i+1的軸線，作其公垂線與對應坐標系OiXiYiZi中的Ji+1交點為坐標原點，Zi軸與Ji+1軸平行且向上，Xi軸平行于公垂線，遠離Ji軸為正方向。Ji對應坐標系Oi-1Xi-1Yi-1Zi-1。

參數aiαiθidi各自代表不同含義，在單個連桿關節中包括兩個參數連桿長度ai表示單連桿長度，即公垂線上Ji、Ji+1間距離。αi表示Zi與Zi+1之間的偏轉角度，逆時針為正。Di表示相鄰兩個連桿公垂線間距離。θi表示相鄰兩個連桿之間的扭轉角，逆時針為正。

在連桿坐標系中，相鄰兩連桿可通過平移與旋轉互相轉換，分為以下四步：

表1：機械臂連桿D-H參數

圖1：連桿坐標建立方式

圖2：配網帶電作業機器人機械臂模型

（1）將初始連桿繞坐標系Z軸旋轉，旋轉角度為θ，使初始連桿與目標連桿的X軸共面；

（2）將初始連桿繞坐標系Z軸平移，平移距離為d，使初始連桿與目標連桿的X軸共線；

（3）將初始連桿繞坐標系X軸平移，平移角度為a，使得初始連桿與目標連桿坐標原點重合；

（4）將初始連桿繞坐標系X軸轉動，轉動角度為α，使得初始連桿與目標連桿Z軸共線；

將連桿坐標轉換通過矩陣定義如下：

坐標系變換：

機器人運動求解過程中，機械臂末端的位姿可以通過連桿的坐標系變換得到。機械臂數學模型的建立關鍵在于找到局部坐標系在全局坐標系中的映射關系。發生平移時，其變換矩陣為Tr(dx, dy, dz)：

旋轉角度為θ的坐標系3個方向的變換矩陣分別為：

令PU表示點P相對于坐標系U的描述；TUR表示坐標系R與U之間的變換矩陣；即點P與坐標系R之間的轉換關系如下PR：

圖3：配電機器人機械臂起始狀態

圖4：機器人運動學仿真界面

圖5：運動學正解

PR=PU×TUR

3 機器人數學模型建立

3.1 機械臂數學模型

為了完成機器人的仿真，第一步是創建其數學模型。Robotics工具箱的Link可用于構建其模型。

其各個參數定義如下：

（1）a為連桿長度，Z軸在X軸上的偏移；

（2）α為連桿扭角，Z軸繞X軸轉角，逆時針為正；

（3）d為連桿偏置，X軸在Z軸上的偏移；

（4）θ為關節角度，X軸繞Z軸轉角，逆時針為正；

（5）σ代表關節類型：0表示旋轉關節，1表示移動關節；

（6）φ代表移動關節偏移量。

本文所用機械臂如圖2所示。該機械手由六個旋轉關節組成。各桿件的D-H參數如表1所示。

3.2 機器人正運動求解

正運動求解，即把關節變量、機器人末端初始位姿、各關節變化角度當作已知，把機器人末端的最終位姿當作未知的求解過程。表示為連桿i相對于連桿i-1的坐標位置。的變換通式為：

本文機器人機械臂末端位姿可表示為：

3.3 機器人逆運動求解

逆運動求解，即把關節變量、機器人末端初始位姿、最終位姿當作已知，把關節變化角度當作未知并求解的過程。逆運動問題實際上是運動方程的解。

給定末端執行器的位姿，即已知n,o,a,p的坐標。對于每個變量，只需將所對應的連桿進行逆變換，即可求解θ1, θ2, θ3, θ4, θ5, θ6。

變量公式分別如下：

3.4 初始狀態仿真

在本文作業任務中，需兩條機械臂相互協調配合，方可完成接引流線任務，兩機械臂初始狀態角θ分別為[90° 100° -50° 30° -90° 0]和[90° 70° 30°-20° -90° 0]。結果如圖3所示。

3.5 仿真界面設計

對雙臂數學模型的運動學和動力學仿真，其實質為每一條壁單獨仿真，出于模擬工況較多的考慮，需進行大量重復編寫或更改代碼，每一次修改都在源代碼里修改，容易出現疏漏，不夠直觀，且造成模擬效率的下降，將程序模塊化，整合為如圖4所示界面。

仿真界面左側為模型建立部分，首先輸入連桿的各個參數，包含關節角度θ，連桿扭角α，連桿偏置d，連桿長度a，連桿的初始長度和變化范圍。為簡化輸入步驟，設置了默認參數一鍵輸入功能。待各連桿參數確定后，點擊建立模型，后臺程序即可生成數學模型，并可通過繪圖和動態演示可視化模型狀態。

仿真界面右側為運動學分析部分，可輸入各連桿的初始位置、正求解位置和逆求解位置。為簡化輸入，設置了默認參數一鍵輸入功能。待各連桿初始和終止位置確定后，點擊正求解，即可在下方正解位置矩陣處顯示結果，并可通過繪制軌跡曲線可視化模型位置。反求解環節亦同上。

4 機械手臂運動學仿真

正運動學問題（DKP）和逆運動學問題（IKP）是機械臂運動學仿真中的主要問題。

4.1 正運動學問題仿真

調用MATLAB中Robotics工具箱對機械臂數學模型正運動求解過程如下：

T=fkine(robot,q);

Robot為所建立的機械臂數學模型，q為關節變化角度，T為多得正運動求解。選取左側機械臂為例，數學模型的各關節初始狀態設為q0，各坐標變化量為qz，其正運動學求解過程為：

運行后，由MATLAB計算得到的運動學正解如圖5，其中最后一列為機械臂末端位置坐標（-164.0000 -631.4900 26.9651），正解模型如圖6。用jtraj函數仿真其1秒運動軌跡，如圖7所示。

4.2 逆運動學問題仿真

調用MATLAB中Robotics工具箱對機械臂數學模型逆運動求解過程如下：

q=ikine(robot, T, q0, m)

其中，q是逆運動學問題的解，即每個關節的變化，robot是機械手的數學模型，q0是關節初始角，T為反求解的變換矩陣，m是4×4矩陣。

T1是正運動解的結果。逆運動求解過程如下：

qz2=ikine(bot, T1, q0, m)

運行后，由MATLAB計算得到的運動學逆解為：

qz2=[1.0749-2.2721 -1.7441 -0.5394 1.6551 -0.4807]

與qz1不同，將逆解qz2帶入正求解方程，得末端位置坐標[-164.0000 -631.4901 26.9652]與正問題求解工程中給定的末端位置坐標相同，即證明相同位置坐標可通過不同坐標變換得到，同時也證明逆運動求解的不唯一性。

5 結論

使用D-H方法，建立了用于配電網帶電作業的機械臂的結構參數，并在MATLAB Robotics工具箱中建立了其模型，完成了機械臂正逆問題的求解和仿真，并完成其軌跡規劃，

為日后作業過程中可能遇到不同復雜工況下的軌跡規劃、離線編程和動態設計等研究奠定了基礎。

圖6：運動學正解模型

圖7：正求解各關節運動過程