1、課程設計課程名稱機器人學題目名稱基于Matlab的強化學習路徑尋優學生學院專業班級學號學生姓名指導教師目錄1 課程設計要求32 六自由度機器人設計32.1 機器人構型及其坐標系32.2 機器人dh參數表42.3 機器人關節運動范圍43 正運動學43.1 機器人工作空間54 逆運動學54.1 微分運動54.2 算法流程54.3 實驗結果65 有關機器人一些其他的工作65.1 差分進化算法辨識逆運動學解65.2 關于本次報告中使用差分算法的一些理解與想法75.3 基于強化學習的路徑尋優及程序流程圖96 有關程序以及視頻資料、其余文件的說明111課程設計要求一，按照附件模板填寫，要求有封面和目錄，除

2、簽名處不能有手寫。二，主要內容包括下面幾個部分，1,設計一款六自由度機器人，要求2,3,4,5關節中有一個是滑動關節，其余關節應為轉動關節。試構想該機器人的功能，并根據功能設定機器人的介紹參數（桿件長度及關節極限）2,建立機器人的正運動學模型，進行Matlab運動仿真。（分析機器人的工作空間，制作機器人各個運動的動畫。）注意事項：1）要求畫出機器人的關節坐標系，列出DH參數表，以及各個關節間的齊次變換矩陣。2） Matlab仿真應畫出工作空間的立體圖和剖面圖。采用機器人產品的同學應與實際說明書的工作空間做對比。自行設計的同學要做簡單的分析討論。3）直接采用例程里面的三自由度機器人該部分得0分。

3、3,實現逆運動學軌跡規劃注意事項：1）這里特指機器人末端的軌跡規劃，不是關節空間的軌跡規劃。2）要實現控制機器人末端在空間中完成某種軌跡。（如直線，圓弧，心型，寫字等）3）可以采用求解逆運動的方程或者是利用微分運動。4）寫出詳細的推導過程（公式）。5）要求有仿真截圖及動畫。6）只能使用matlab及本課程提供的例程，不能使用工具箱。7）僅僅使用3自由度例程的同學本部分分數會很低4,自由發揮項（完成這一部分的同學才能夠得到90分以上）1）機器人完整逆解的求解方式（數值解）；2）尋找奇異點，分析奇異位型5,Matlab程序作為附錄應添加在課程設計報告書的最后面。要求在第一頁附上程序流程圖，注明函數

4、調用過程，此外，程序要排好版2六自由度機器人設計這款機器人為一個六自由度機器人，其第三個關節為滑動關節，其余關節均為轉動關節關于將第三個關節設置為轉動關節，想法主要是第一關節為轉動關節，主要是控制機器人在xy面上的大范圍運動，第二關節以及第三關節主要是控制機器人在空間中的xyz上大幅度的運動，后三個關節主要是對末端姿態的調整為主。2.1 機器人構型及其坐標系圖1DH建立坐標圖2經DH建模后的在MATLAB顯示的構型圖1與圖2的機器人構型一致，它們之間的不同僅是設置的。不同。2.2 機器人dh參數表#0dzdxa1-2011500902-30200-903-4035000904-5041500-

5、905-6051500-906-70610000表1DH表2.3 機器人關節運動范圍關節Th1Th2DzTh3Th4Th5運動范圍-180180-90900500-9090-9090-9090表2機器人關節運動范圍表3正運動學由DH表變換順序9,d,y,a,a可得出相鄰兩個坐標系間的齊次變換矩陣HT_j為:cas(O)*sin(G)00hE(口)本g率3)(6)sin(a)*8s(8)ain(S)+y*d(I若要求某一個坐標使用DH表中的數據依次代入則能夠得到每個相鄰坐標系間的變換矩陣,系與基坐標系的關系，則將其兩者之間的所有坐標變換矩陣連乘即可。3.1機器人工作空間圖3工作空間等軸測圖圖4工

6、作空間左視圖4逆運動學4.1 微分運動Jocobi微分運動矩陣為：匹1X（P7-P1）灰幻H期“犯X（斯P4）n的X（衿陽）X（P7-P0）=對00的四忸幅%微分運動方程：rrTrr“rrrfT產產偽Ngr丹寸丹2ppPR.U.U1r2r3456rfrf,d334.2 算法流程以下給出微分運動的計算流程:1)給定末端關節參考位姿P_ref,R_refo2)給定6個關節變量的數組params,計算正運動學，得出每個關節相對世界坐標系的位姿。3) (P_err,R_err)=(P_ref-RRR_ref)4)通過R_err計算出關節誤差W_err,當P_err的二范數和W_err的二范數之和小于0

7、.1時,結束流程。否則：5)計算Jocobi矩陣，計算關節微量delta,delta=inv(J)*P_err,W_err；將其疊加到原關節變量數組params上。6)循環至(2)。4.3 實驗結果2mIdo-ioo-a-aw混圖5數值法畫直線圖6數值法畫正弦曲線圖5的數值法畫直線，其初始位置不定，但是其姿態是固定的，所以設定末端的終點(-350,-200,400),姿態不變，最終效果如圖5所示，為一條直線圖6為一條正弦曲線，初始位置為(-350,-200,400),姿態R=100;010;001,這條正弦曲線是z=400,R不變的情況上畫的，具體情況是：1.建立正弦曲線函數2.對函數數據點進

8、行采樣，采120個樣本3.然后用數值法盡心畫線。最終效果如圖6所示。5有關機器人一些其他的工作5.1 差分進化算法辨識逆運動學解百度介紹：差分進化算法(DifferentialEvolutionAlgorithm,DE)是一種高效的全局優化算法。它也是基于群體的啟發式搜索算法，群中的每個個體對應一個解向量。差分進化算法的進化流程則與遺傳算法非常類似，都包括變異、雜交和選擇操作，但這些操作的具體定義與遺傳算法有所不同。差分進化算法(DifferentialEvolution,DE)由Storn和Price于1995年首次提出。主要用于求解實數優化問題。該算法是一類基于群體的自適應全局優化算法，屬

9、于演化算法的一種，由于其具有結構簡單、容易實現、收斂快速、魯棒性強等特點，因而被廣泛應用在數據挖掘、模式識別、數字濾波器設計、人工神經網絡、電磁學等各個領域。1996年在日本名古屋舉行的第一屆國際演化計算（ICEO直賽中，差分進化算法被證明是速度最快的進化算法。和遺傳算法一樣，差分進化算法也是一種基于現代智能理論的優化算法，通過群體內個體之間的相互合作與競爭產生的群體智能來指導優化搜索的方向。該算法的基本思想是：從一個隨機產生的初始種群開始，通過把種群中任意兩個個體的向量差與第三個個體求和來產生新個體，然后將新個體與當代種群中相應的個體相比較，如果新個體的適應度優于當前個體的適應度，則在下一代

10、中就用新個體取代舊個體，否則仍保存舊個體。通過不斷地進化，保留優良個體，淘汰劣質個體，引導搜索向最優解逼近。圖7差分進化算法運算流程()=0+F*(P)-)圖8差分進化算法的三個操作5.2 關于本次報告中使用差分算法的一些理解與想法1 .首先對圖8差分進化算法的三個操作進行基本的講解2 .對下面的圖9、圖10差分進化算法的每代最佳適應度的曲線的一些講解以及理解3 .對差分進化算法應用在求逆運動學解上的一些想法，即對差分進化算法的一些改進的想法一、首先對圖8差分進化算法的三個操作進行基本的講解圖8中第一個式子：差分進化算法的變異，其中F為變異因子，變異因子的大小的選取在后面（二）中會有解釋。其中

11、1/2/3為隨機的三個粒子，根據S_N=S_1+F*（S_2-S_3米產生新的粒子S_N。第二個式子：差分進化算法的操作，上下兩行為兩個粒子里面的數據，其中new為剛生成的粒子、now為當前size循環下的粒子，通過一些隨機概率，將兩粒子之間的數據進行交換，生成新的粒子v,。第三個式子：將當前粒子與新生成的粒子v進行適應度fi對比，適應度高的留下，低的被淘汰。圖9F=0.1時的fi曲線圖10F=0.3時的fi曲線二、對下面的圖9、圖10差分進化算法的每代最佳適應度的曲線的一些講解以及理解F為變異參數，F設置的較小時，群體的差異度減小，收斂過早從圖9來看F=0.1時收斂過早，并且適應度低，效果不

12、好圖10F=0.3時最后迭代完也還未收斂，但是適應度相較于圖9的效果好。從上述兩圖的fi適應度曲線來看，因為我將適應度取期望位姿的倒數，所以根據適應度的上升趨勢來看，如果適應度曲線上升趨勢比較平滑，證明若出現這種情況，末端的位姿變化不會過于突兀，如果在中間具有小橫線然后再上升，末端的迭代運動可能會停止在某一位姿，然后再突然運動，這樣的效果并不好，所以，我們最希望的在求取機械臂逆解的適應度曲線應該是一條快速上升的，并且平滑的曲線，這樣末端在迭代時速度快且運動比較平滑，不抖。三、對差分進化算法應用在求逆運動學解上的一些想法，即對差分進化算法的一些改進的想雖然上述圖9、圖10的曲線是還可以的，但這僅

13、僅反映了末端位姿，在其其余關節上可能會發生非常大的突變，這對于實際的控制是非常不可取的，這主要是因為在上述三個操作中的交叉操作出現的問題，對粒子間的數據進行按一定概率的交換，這樣可能會導致角度的突變。針對這一問題，提出了以下幾點改進的想法1 .將牛頓-拉夫遜法與差分進化算法進行結合，利用牛頓拉夫遜法在前面范圍的快速運動，在靠近期望位姿時牛頓拉夫遜法卻迭代緩慢，這時利用差分進化算法進行搜索，快速找到期望位姿2 .在差分進化算法的初始粒子的設置方面，將牛頓拉夫遜法的最后的迭代的各關節的角度數據輸出，在此角度數據的基礎下進行隨機的增加或減少一點增量，因為靠近期望位姿，所以增加這一點的增量范圍的角度應

14、該在期望位姿附近，然后利用差分進化算法進行快速搜索，這樣，就可以得到理想位姿，并且，因為角度的數值差異不大，所以機械手各關節的變動不會很大，在可以接受的范圍內。這樣我們不僅可以避免掉牛頓拉夫遜法在靠近期望位姿的末端時緩慢的迭代，我們還可以利用差分進化算法的快速性，快速搜索期望位姿，也避免了其各關節角度的突變。總的來說，就是兩種算法之間的互補。3 .在參數設置方面，我們對F的參數盡量設的小，因為對粒子差異度的需求并不大，最優解在初始粒子附近，所以F參數的設置不大。在交叉因子CR上的設置，我們可以加大交叉因子的CR的值，使發生交叉的概率增大，提高粒子的多樣性，這樣一邊更快的尋找到最優粒子。5.3基

15、于強化學習的路徑尋優及程序流程圖n圖11原始問題圖12簡化后的問題圖13問題的無向圖本次學習的路徑就是要從0/1/2/3/4房間走出去，即走到5,上述圖11位原始問題，圖12位簡化后的問題，圖13位這個問題的無向圖，相連代表可以通行。我們根據上述的問題，采用的是強化學習的方法去解決。AgentActiona1RewardrStatest+1ReinforcementLearningSetup圖14強化學習的一些概念上述的agent為智能體，這個智能體可以看做一個小機器人，在上圖智能體有兩輸入、一輸出，跟上圖一樣。environment為環境，在上圖環境有一輸入、兩輸出。actionatat=a

16、1,.,an,action為行為也可稱動作，即智能體采取的行為，智能體采取的行為可以有n種，為a1/a2/./an。reward為獎勵，獎勵可以是離散的，也可以是線性的。如果獎勵是離散的，離散的獎勵可以形成一個獎勵矩陣，也可以用分段函數表示等等，線性的獎勵可以是一個獎勵函數，如果獎勵是一個函數，這樣是即為方便的。還有一個是state,狀態，也可以叫觀測observation,有很多種情況。現在我們就根據強化學習中的其中一種方法做路徑尋優，叫Q-learning。Qlearning的核心是根據上述的那些概念生成一個Q表，而機器人的action根據Q表選擇在算法實現前，我們先做一些準備工作：1 .

17、根據圖11或圖13生成獎勵矩陣2 .生成初始的Q表，以備更新3 .設定一些基本的參數數值下面講一下Q-learning的算法步驟：1 .隨機生成一個觀測（狀態）Q表中的位置。2 .將當前觀測中可通行的行為，全部存起來，并一一對應的3 .然后我們隨機選擇一個可行的動作，注意是可行的動作4 .然后我們根據公式：Q(state,action)=R(state,action)+Gamma*MaxQ(nextstate,allactions)進行計算5 .根據3.中的動作，可預見下一個觀測6 .如果不滿足觀測=5,回到2在上述算法中是一個回合的迭代，這種情況下不足以算出所有的情況，所以在上述的算法步驟的

18、基礎上，在外面增加一個大循環，每次都可以隨機生成觀測，將大循環次數增加至一定數量即可迭代出完整的Q表。最后根據獎勵矩陣R和算法，迭代出的Q表如下：0123450TQ電20,-1,-1,3dl40,-1,-1,0,-11,100L5-1,0T-1,-1,OJOOJ0123450000800;000640100;0006400;080510800;6400640100;000000;圖15獎勵矩陣R圖16經迭代后的Q表出來了Q表，選取最優的Q值，進行行為選擇，就是看著這些值，哪個值大選哪個。然后根據上面的圖以及選取策略，我們可以有以下圖圖17觀測與行為dot0代表觀測0,而down(1)代表相應的

19、行為，1表示相應行為的編號，方便后面編程有圖17和圖16我們可以進行編程，有如下流程圖圖18test3主程序流程圖最終效果可以在視頻中觀看。相應的主程序test3也在程序里，一些程序功能也編寫在相應的說明文件中。在本文中為彳f么生成Q表使用的是Python程序，而不是MATLAB程序，以及為什么不在途中訓練機器人，主要原因有如下：I.Python的運行效率較MATLAB高2.加入機器人后的訓練，這樣雖然訓練過程非常直觀，但是同時因為大量的程序的嵌入，導致運行效率將會非常低。考慮到上述兩點，所以將問題抽象出來在Python中訓練得到Q表，最后再對策略進行編程，加大時間效用。如果想讓機器人訓練過程

20、比較直觀，只需在Q-learning算法步驟中添加機器人的程序再作部分修改即可。6有關程序以及視頻資料、其余文件的說明如春續之置原碼、報告、演示視頻、PPT等資料，請聯系扣扣亦我飛也機器人：action_func:將實際行為賦予編號，方便編程calculate_Matrix_DH_Ln:計算各關節的變換矩陣，與Matrix_DH_Ln無異，僅增加了一行代碼用于輸疝各關節變版矩陣compute_err：主要用于計算當前末端與期望末端的誤差Connect3D：畫連點之間連線圖des_pose:用于直線的期望位姿循環迭代的DrawCylinder：畫圓柱體init_dh_list:DH表input_

21、obser：輸入觀測，并輸出行為，同時改變當前觀測至下一觀測j_dh_draw:畫機器人圖Matrix_DH_Ln：計算變換矩陣pos_up、pos_down、pos_left、pos_right:機器人運動程序，上下左右shuzhi：數值解simp_jacobian:雅可比矩陣test、testl、test2、test3：機器人工作空間、畫直線、畫正弦曲線、機器人路徑尋優差分進化算法：chap8_16:差分進化算法的主程序chap8_16obj:適應度程序上述程序中，差分進化算法程序在一個文件、工作空間程序在另外一個文件夾，文件夾。有關注釋在程序內。Q-learning程序：q-learni

22、ng:生成Q表畫直線程序test1其余在一個%功能:畫直線%程序描述：先到達期望位姿，然后再畫直線clc;clearall;globalLinkglobalth1globalth2globaldzglobalth4globalth5globalth6%設置初始角度，并且到達期望位姿th1=1;th2=90;dz=10;th4=1;th5=-90;th6=1;j_dh_draw(th1,th2,dz,th4,th5,th6,0);pause;holdoff;x_des=-350;y_des=-200;z_des=400;R_des=1,0,0;0,1,0;0,0,1;des_pose(x_des

23、,y_des,z_des,R_des);view(-37.5,30);pause;holdoff;%畫圖%a望x砌望y%期望z%期望姿態%到達期望姿態%視角改變為等軸側視角%上述的一些列操作是為了達到目標點，現在開始畫直線x_des=x_des-200;%僅對期望x修改des_pose(x_des,y_des,z_des,R_des);%開始畫直線結束畫圖程序j_dh_drawfunctionj_dh_draw(th1,th2,dz,th4,th5,th6,fcla)%closeallglobalLinkinit_dh_list;radius=20;%25len=60;%60joint_col

24、=0;plot3(0,0,0,ro);%基本信息半徑長度%坐標原點Link(2).th=th1*pi/180;Link(3).th=th2*pi/180;Link(4).dz=dz+250;%link(2)=th,%link(3)=th,90*ToRad0*ToRad,0.01745/度1-20.01745/度2-3%初值設dz+250,+250這個設置，為最大值速的到達空間中的點，具體思想參照二分法500的中間值，方便空間中伸縮活動，可以更快Link(5).th=th4*pi/180;Link(6).th=th5*pi/180;%link(5)=th,0*ToRad%link(6)=th,0

25、*ToRad0.01745/度4-50.01745/度5-6Link(7).th=th6*pi/180;%forinitialpositionth,0*ToRad6-Efori=1:7Matrix_DH_Ln(i);%這個是構建DH變換矩陣和無位置信息的旋轉矩陣，各關節變換矩陣endfori=2:7Link(i).A=Link(i-1).A*Link(i).A;%乘變換矩陣Link(i).p=Link(i).A(:,4);Link(i).n=Link(i).A(:,1);Link(i).o=Link(i).A(:,2);Link(i).a=Link(i).A(:,3);Link(i).R=Li

26、nk(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3);%旋轉矩陣Connect3D(Link(i-1).p,Link(i).p,b,2);holdon;%畫杠的，就是畫杠桿的長度部分plot3(Link(i).p(1),Link(i).p(2),Link(i).p(3),rx);holdon;%這個是在杠桿的連接處加星星ifi0.000001)x_now=Link.p(1);y_now=Link.p(2);z_now=Link(7).p(3);R=Link.R;now_P=x_now,y_now,z_nowJ=simp_jacobian(th1,th2,dz,t

27、h4,th5,th6);learning_rate=0.1;D=det(J);ifD=0fprintf(D=%2.4f,D);fprintf(n);fprintf(Passthesingilarity!);fprintf(n);pause;endp_err=x_taget-x_now;y_taget-y_now;z_taget-z_now;R_err=R*R_taget;Err_th=acos(R_err(1,1)+R_err(2,2)+R_err(3,3)-1)/2);r_err=(Err_th/(2*sin(Err_th)A2*.R_err(3,2)-R_err(2,3);R_err(1,

28、3)-R_err(3,1);R_err(2,1)-R_err(1,2);dth=learning_rate*inv(J)*p_err;r_err;th1=th1+dth(1)/pi*180;th2=th2+dth(2)/pi*180;dz=dz+dth(3);th4=th4+dth(4)/pi*180;th5=th5+dth(5)/pi*180;th6=th6+dth(6)/pi*180;j_dh_draw(th1,th2,dz,th4,th5,th6,1);x(num)=Link(7).p(1);y(num)=Link(7).p(2);z(num)=Link(7).p(3);num=num+

29、1;plot3(x,y,z,r.);gridon;holdon;Loss=compute_err(x_taget,y_taget,z_taget,R_taget);loop_num=loop_num+1end結束采樣畫正弦曲線程序test2clc;clearall;globalLinkglobalth1globalth2globaldzglobalth4globalth5globalth6%設置各關節初值th1=7.8994;th2=56.2116;dz=19.7235;th4=33.7884;th5=-90;th6=82.0943;j_dh_draw(th1,th2,dz,th4,th5,t

30、h6,0);pause;holdoff;R_des=1,0,0;0,1,0;0,0,1;%期望姿態x=;y=;z=;%采樣ts=1;fork=1:1:120time(k)=k*ts;u(k)=80*sin(0.02*pi*k*ts);z1(k)=-400;end%數據處理fork1=1:1:120time(k1)=-350-time(k1);u(k1)=-200-u(k1);endnum=1;R_taget=R_des;loop_num=0;%while計數for_num=0;%for計數fori=1:1:120while(1)x_now=Link.p(1);y_now=Link.p(2);z

31、_now=Link.p(3);R=Link(7).R;R=R_des;now_P=x_now,y_now,z_nowJ=simp_jacobian(th1,th2,dz,th4,th5,th6);learning_rate=0.1;D=det(J);ifD=0fprintf(D=%2.4f,D);fprintf(n);fprintf(Passthesingilarity!);fprintf(n);endp_err=time(i)-x_now;u(i)-y_now;400-z_now;R_err=R*R_taget;Err_th=acos(R_err(1,1)+R_err(2,2)+R_err(

32、3,3)-1)/2);r_err=(Err_th/(2*sin(Err_th)+10A(-10)A2*.R_err(3,2)-R_err(2,3);R_err(1,3)-R_err(3,1);R_err(2,1)-R_err(1,2);%發生數值下溢，所以添加了一個非常小的值，對最后效果影響非常小err=norm(r_err)+norm(p_err);dth=learning_rate*inv(J)*p_err;r_err;th1=th1+dth(1)/pi*180;th2=th2+dth(2)/pi*180;dz=dz+dth(3);th4=th4+dth(4)/pi*180;th5=th5

33、+dth(5)/pi*180;th6=th6+dth(6)/pi*180;j_dh_draw(th1,th2,dz,th4,th5,th6,1);x(num)=Link(7).p(1);y(num)=Link(7).p(2);z(num尸Link(7).p(3);num=num+1;plot3(x,y,z,r.);gridon;holdon;loop_num=loop_num+1;iferr4ifnow_obser=0iffur_obser=4action=2;%執行0-4的行為編號obser=fur_obser;%因為執行這個行為后，房間號將會改變為fur_obser%所以當前房間編號變更為

34、fur_obser,fur_obser此處為4endendifnow_obser=2iffur_obser=3action=2;obser=fur_obser;endendifnow_obser=1iffur_obser=5action=4;obser=fur_obser;endiffur_obser=3action=3;obser=fur_obser;endendifnow_obser=3iffur_obser=1action=4;obser=fur_obser;endiffur_obser=2action=1;obser=fur_obser;endiffur_obser=4action=3

35、;obser=fur_obser;endendifnow_obser=4iffur_obser=0action=1;obser=fur_obser;endiffur_obser=3action=4;obser=fur_obser;endiffur_obser=5action=3;obser=fur_obser;endend結束行為執行函數action_funcfunctionaction_func(action)ifaction=1pos_up(up);%當行為編號action為1時，執行末端向上運動endifaction=2pos_down(down);%向下endifaction=3pos

36、_left(left);%向左endifaction=4pos_right(right);%向右end結束向上函數pos_upfunctionpos_up(act)globalLinkglobalth1globalth2globaldzglobalth4globalth5globalth6globalnum%獲取當前位置%各姿態固定x_now=Link.p(1);y_now=Link(7).p(2);z_now=Link(7).p(3);R_des=1,0,0;0,1,0;0,0,1;R_taget=R_des;ifup=acty_taget=y_now+100;%丫方向移動+100shuzh

37、i(x_now,y_taget,z_now,R_taget)act=up;endx_now=Link(7).p(1);y_now=Link(7).p(2);z_now=Link(7).p(3);x=x_now;y=y_now;z=z_now;結束數值解程序shuzhifunctionshuzhi(x_taget,y_taget,z_taget,R_taget)%這個程序與des_pos程序相似，在誤差上有少許不同，一個采用范數，一個采用二次代價globalLinkglobalth1globalth2globaldzglobalth4globalth5globalth6globalnumwhile(1)x_now=Link(7).p(1);y_now=Link(7).p