本發(fā)明涉及路徑規(guī)劃，具體為一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法。

背景技術(shù)：

1、微裝配系統(tǒng)能夠通過各個(gè)子系統(tǒng)之間的精密配合完成微小零件的裝配任務(wù)，協(xié)作機(jī)器人在裝配任務(wù)中的主要作用為上下料與輔助裝配，在該過程進(jìn)行前需要進(jìn)行協(xié)作機(jī)器人的路徑規(guī)劃。協(xié)作機(jī)器人路徑規(guī)劃是指根據(jù)協(xié)作機(jī)器人本身的結(jié)構(gòu)參數(shù)、環(huán)境信息以及需要其出發(fā)的初始點(diǎn)和到達(dá)的目標(biāo)點(diǎn)，規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的無碰撞路徑。

2、rrt算法作為目前比較熱門的三維路徑規(guī)劃方法，主要思想為在限定的搜索空間內(nèi)進(jìn)行隨機(jī)采樣，并對采樣點(diǎn)進(jìn)行篩選，直到找到一條無碰撞的路徑。而rrt*算法具有漸進(jìn)最優(yōu)性，在原rrt的基礎(chǔ)上加入了路徑的優(yōu)化過程，在找到一條路徑后并不會停止采樣，在后續(xù)采樣中通過chooseparent與rewire過程進(jìn)行樹內(nèi)父子關(guān)系的更新，直到達(dá)到一定要求后停止采樣并輸出最優(yōu)路徑。

3、informed-rrt*算法對rrt*算法向最優(yōu)解收斂速度較慢的情況進(jìn)行了優(yōu)化，它能夠限制rrt*算法在獲得初始路徑后的采樣范圍，并根據(jù)當(dāng)前最優(yōu)路徑更新采樣范圍，提高采樣點(diǎn)的質(zhì)量，從而加速算法向最優(yōu)解的收斂速度。

4、但informed-rrt*算法目前主要應(yīng)用于二維平面，在三維平面微裝配協(xié)作機(jī)器人的路徑規(guī)劃應(yīng)用中，由于協(xié)作機(jī)器人工作路徑一般需要繞過大體積障礙物，導(dǎo)致路徑被一些急彎分割為了不同的片段，在此情況下暫無能夠有效限制采樣范圍從而加速算法收斂的路徑規(guī)劃方法。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的目的是解決上述現(xiàn)有路徑規(guī)劃算法的不足，提出一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，能夠有效限制采樣范圍，采樣質(zhì)量高，加速算法收斂。

2、為了達(dá)到上述目的，本發(fā)明采用如下技術(shù)方案：

3、一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，具體包括以下步驟：s1、系統(tǒng)初始化，系統(tǒng)初始化包括了協(xié)作機(jī)器人信息的初始化，環(huán)境信息的初始化，協(xié)作機(jī)器人起點(diǎn)與終點(diǎn)以及初始搜索范圍的初始化；s2、通過rrt*算法獲取初始路徑；s3、基于改進(jìn)的informed-rrt*算法引入了針對三維空間內(nèi)的自適應(yīng)斜圓柱體區(qū)域限制策略與針對路徑被急彎分割為不同部分的路徑分段限制采樣策略，在獲得初始解后根據(jù)已規(guī)劃出的最優(yōu)路徑進(jìn)行斜圓柱體區(qū)域采樣限制，提高采樣點(diǎn)質(zhì)量，加速向最優(yōu)解的收斂；s4、當(dāng)規(guī)劃至最優(yōu)路徑，或者到達(dá)設(shè)定的迭代時(shí)間或次數(shù)上限，停止路徑規(guī)劃并輸出當(dāng)時(shí)的最短路徑作為規(guī)劃路徑。s5、將路徑中的路徑點(diǎn)位置信息傳遞給協(xié)作機(jī)器人，協(xié)作機(jī)器人通過位置信息求逆解得到各個(gè)關(guān)節(jié)的角度，完成在該路徑下的運(yùn)動(dòng)規(guī)劃。

4、進(jìn)一步，s1中的系統(tǒng)初始化還包括對每次迭代步長、終點(diǎn)的范圍判斷、最大迭代次數(shù)或迭代時(shí)間、初始采樣范圍、路徑分割段數(shù)n的初始化。

5、進(jìn)一步，s3具體包括以下步驟：s3.1、針對三維空間內(nèi)的自適應(yīng)斜圓柱體區(qū)域限制策略與針對路徑被急彎分割為不同部分的路徑分段限制采樣策略，應(yīng)當(dāng)根據(jù)設(shè)置的路徑分割段數(shù)n將當(dāng)前最優(yōu)路徑分割成n段，并在1到n之間隨機(jī)選取一個(gè)整數(shù)m，根據(jù)該整數(shù)與規(guī)定的片段選擇的規(guī)則選取路徑限制的兩段片段組合；s3.2、分別在兩個(gè)片段組合內(nèi)隨機(jī)抽取兩個(gè)路徑中的點(diǎn)，將兩點(diǎn)中間含有的路徑片段作為斜橢圓體限制范圍的參考，兩點(diǎn)分別作為片段的起點(diǎn)與終點(diǎn)，過兩點(diǎn)的直線作為斜圓柱體的中軸；s3.3、求解路徑片段的起點(diǎn)與終點(diǎn)的坐標(biāo)系差的絕對值，斜圓柱體上下底面垂直于差的絕對值最大值所對應(yīng)的坐標(biāo)系，將路徑在該坐標(biāo)系的最大與最小值作為斜圓柱體上下底面位置；s3.4、將片段中包含的點(diǎn)依次求在平行于斜圓柱體上下底面的二維平面與中軸的距離，選擇距離的最大值作為斜圓柱體的半徑rmax，構(gòu)建斜圓柱體采樣限制區(qū)域。其中rmax的求解公式為：

6、rmax＝argmax{ρ(σ(x))|σ(x0)＝x1，σ(xnum-1)＝x2}

7、其中，式中σ表示截取的路徑片段，num表示路徑片段中含有的路徑點(diǎn)數(shù)量，x表示路徑點(diǎn)，x1與x2為對應(yīng)選取路徑片段的起點(diǎn)與終點(diǎn)。s3.5、在斜圓柱體采樣限制區(qū)域內(nèi)進(jìn)行隨機(jī)采樣與路徑優(yōu)化。

8、進(jìn)一步，s3中的根據(jù)采樣點(diǎn)進(jìn)行路徑優(yōu)化對應(yīng)的方法即為rrt*算法的路徑優(yōu)化方法，包含了chooseparent與rewire的路徑優(yōu)化過程。

9、進(jìn)一步，s3.1中對應(yīng)參數(shù)的選取；具體包括以下步驟：s3.11、根據(jù)路徑大致走向確定路徑分割段數(shù)n，取路徑被急拐彎分割段數(shù)加1；s3.12、根據(jù)m值與規(guī)定的片段選擇的規(guī)則進(jìn)行路徑片段的選取，其中片段選擇的規(guī)則如下：設(shè)xstart為目前已規(guī)劃出的最優(yōu)路徑起點(diǎn)，xgoal為目前已規(guī)劃出的最優(yōu)路徑的終點(diǎn)，cmin為目前已規(guī)劃出的最優(yōu)路徑的路徑長度，x1與x2為路徑片段中隨機(jī)選取的路徑點(diǎn)，對應(yīng)選取路徑片段的起點(diǎn)與終點(diǎn)，當(dāng)m＝1時(shí)，x1＝xstart，從[cmin/n，2*cmin/n]的路徑片段中隨機(jī)選取一路徑點(diǎn)作為x2；當(dāng)m＝2時(shí)，從[cmin/n，2*cmin/n]的路徑片段中隨機(jī)選取一路徑點(diǎn)作為x1，從[2*cmin/n，3*cmin/n]的路徑片段中隨機(jī)選取一路徑點(diǎn)作為x2；以此類推，直到當(dāng)m＝n-1時(shí)，從[(n-2)*cmin/n，(n-1)*cmin/n]的路徑片段中隨機(jī)選取一路徑點(diǎn)作為x1，x2＝xgoal；當(dāng)m＝n時(shí)，令x1＝xstart，x2＝xgoal，以保證算法的概率完備性。

10、進(jìn)一步，s3.5的在在斜圓柱體采樣限制區(qū)域內(nèi)進(jìn)行隨機(jī)采樣；具體包括以下步驟：s3.51、在斜圓柱體的上下底面所垂直的坐標(biāo)系軸上的上下底面范圍內(nèi)隨機(jī)取一點(diǎn)，該點(diǎn)在該軸對應(yīng)的坐標(biāo)值作為采樣點(diǎn)對應(yīng)的一個(gè)坐標(biāo)值。s3.52、在0到斜圓柱體的半徑rmax中隨機(jī)取值r；s3.53、在0到360度中隨機(jī)取值angle；s3.54、在平行于斜圓柱體上下底面且過s3.51中隨機(jī)點(diǎn)平面中，以斜圓柱體的中軸與平面的交點(diǎn)為圓心，r為半徑畫圓，在圓上對應(yīng)angle角度的點(diǎn)所在位置即為采樣點(diǎn)。

11、進(jìn)一步，s3.54對應(yīng)angle角度的采樣點(diǎn)中，angle角對應(yīng)的參考0度可以為隨機(jī)位置或固定位置，都能滿足隨機(jī)采樣的要求。

12、總的說來，本發(fā)明具有如下優(yōu)點(diǎn)：

13、1、本發(fā)明通過對informed-rrt*算法進(jìn)行改進(jìn)，讓其能夠更加適用于微裝配協(xié)作機(jī)器人的三維路徑規(guī)劃中，使協(xié)作機(jī)器人在應(yīng)對不同三維復(fù)雜環(huán)境中都能規(guī)劃出一條漸進(jìn)最優(yōu)的無碰撞路徑。

14、2、本發(fā)明通過在informed-rrt*算法中引入了針對三維空間內(nèi)的自適應(yīng)斜圓柱體區(qū)域限制策略，有效解決了協(xié)作機(jī)器人在三維路徑規(guī)劃中的采樣范圍限制不佳問題，提高了采樣點(diǎn)的質(zhì)量，加速了三維路徑規(guī)劃中算法向最優(yōu)解的收斂。

15、3、本發(fā)明通過在上述的改進(jìn)informed-rrt*算法中引入了針對路徑被急彎分割為不同部分的路徑分段限制采樣策略，有效解決了在協(xié)作機(jī)器人繞過大體積障礙物的路徑中不能很好限制采樣范圍的問題，在不失概率完備性的前提下提高采樣點(diǎn)的質(zhì)量。

技術(shù)特征：

1.一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，其特征在于：包括以下步驟：

2.根據(jù)權(quán)利要求1所述的一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，其特征在于：所述s1中的系統(tǒng)初始化還包括對每次迭代步長、終點(diǎn)的范圍判斷、最大迭代次數(shù)或迭代時(shí)間、初始采樣范圍、路徑分割段數(shù)n的初始化。

3.根據(jù)權(quán)利要求1所述的一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，其特征在于：所述s3包括以下步驟：

4.根據(jù)權(quán)利要求1所述的一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，其特征在于：所述s3中的根據(jù)采樣點(diǎn)進(jìn)行路徑優(yōu)化對應(yīng)的方法即為rrt*算法的路徑優(yōu)化方法，包含了chooseparent與rewire的路徑優(yōu)化過程。

5.根據(jù)權(quán)利要求3所述的一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，其特征在于：所述s3.1包括以下步驟：

6.根據(jù)權(quán)利要求3所述的一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，其特征在于：所述s3.5的在斜圓柱體采樣限制區(qū)域內(nèi)進(jìn)行隨機(jī)采樣包括以下步驟：

7.根據(jù)權(quán)利要求6所述的一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，其特征在于：所述s3.54對應(yīng)angle角度的采樣點(diǎn)中，angle角對應(yīng)的參考0度為隨機(jī)位置或固定位置，都能滿足隨機(jī)采樣的要求。

技術(shù)總結(jié)
本發(fā)明提出一種微裝配協(xié)作機(jī)器人三維路徑規(guī)劃方法，包括以下步驟：S1、系統(tǒng)初始化，系統(tǒng)初始化包括了確定路徑起點(diǎn)和終點(diǎn)的位置坐標(biāo)，每次連接的步長與最大迭代時(shí)間或次數(shù)，初始搜索范圍；S2、進(jìn)行初始路徑的采樣搜索；S3、進(jìn)行改進(jìn)的Informed?RRT*算法采樣，包括針對三維空間內(nèi)的自適應(yīng)斜圓柱體區(qū)域限制策略與針對路徑被急彎分割為不同部分的路徑分段限制采樣策略；S4、若搜索出最優(yōu)路徑、達(dá)到搜索時(shí)間或迭代次數(shù)的上限，將目前的最優(yōu)路徑輸出；通過改進(jìn)的Informed?RRT*算法能夠使協(xié)作機(jī)器人在三維復(fù)雜環(huán)境中進(jìn)行最優(yōu)路徑規(guī)劃，能夠提高采樣點(diǎn)的質(zhì)量，加速了傳統(tǒng)Informed?RRT*算法在環(huán)境復(fù)雜的情況下向最優(yōu)路徑的收斂速度。

技術(shù)研發(fā)人員：張憲民,陳智霖,李海,曾霆俊
受保護(hù)的技術(shù)使用者：華南理工大學(xué)
技術(shù)研發(fā)日：
技術(shù)公布日：2024/12/19