本發(fā)明涉及冗余機(jī)械臂的路徑規(guī)劃領(lǐng)域，尤其涉及一種帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法。

背景技術(shù)：

1、在已知目標(biāo)的情況下進(jìn)行蛇形機(jī)械臂的運(yùn)動(dòng)規(guī)劃時(shí)，可以根據(jù)蛇形機(jī)械臂與目標(biāo)的相對(duì)位置規(guī)劃一條安全路徑，通過(guò)運(yùn)動(dòng)控制方法使得蛇形機(jī)械臂按給定的路徑運(yùn)動(dòng)最終達(dá)到目標(biāo)點(diǎn)。當(dāng)蛇形機(jī)械臂按指定路徑運(yùn)動(dòng)時(shí)，通過(guò)控制蛇形機(jī)械臂使其所有連桿均以較小的誤差在給定路徑上運(yùn)動(dòng)，將這種運(yùn)動(dòng)方式稱為蛇行路徑跟蹤。然而，蛇形機(jī)械臂其本身具有物理轉(zhuǎn)角極限，在跟隨給定路徑過(guò)程中機(jī)械臂關(guān)節(jié)轉(zhuǎn)角可能會(huì)超過(guò)專輯極限導(dǎo)致機(jī)械臂無(wú)法繼續(xù)運(yùn)動(dòng)，現(xiàn)在常見的末端跟隨算法無(wú)法解決轉(zhuǎn)角超極限問(wèn)題，如果在機(jī)械臂超過(guò)轉(zhuǎn)角極限時(shí)簡(jiǎn)單的將轉(zhuǎn)角限制為關(guān)節(jié)最大轉(zhuǎn)角會(huì)導(dǎo)致機(jī)械臂與嚴(yán)重偏離理想路徑。而采用數(shù)值計(jì)算法進(jìn)行機(jī)械臂路徑規(guī)劃雖可以解決轉(zhuǎn)角超極限的問(wèn)題，但是其計(jì)算量大、求解耗時(shí)長(zhǎng)，無(wú)法滿足機(jī)械臂實(shí)時(shí)控制的要求。因此需要一種高效的路徑規(guī)劃方法能夠讓蛇形機(jī)械臂在轉(zhuǎn)角限制內(nèi)以盡可能小的誤差跟蹤理想路徑。

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

1、針對(duì)現(xiàn)有技術(shù)的不足，本發(fā)明提出一種帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法。

2、具體技術(shù)方案如下：

3、一種帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法，包括以下步驟：

4、s1：設(shè)定蛇形機(jī)械臂的目標(biāo)路徑，所述目標(biāo)路徑包括多個(gè)目標(biāo)點(diǎn)，其中g(shù)j為第j個(gè)目標(biāo)點(diǎn)；所述機(jī)械臂包括基座、連桿、關(guān)節(jié)，相鄰兩連桿之間通過(guò)關(guān)節(jié)連接，a1為機(jī)械臂的初始關(guān)節(jié)，連接基座與連桿1；an為末端關(guān)節(jié)，ai為第i個(gè)關(guān)節(jié)，i＝1,2,…,n；ai-1與ai之間的連桿長(zhǎng)度為li；

5、s2：采用順序搜索法，從a1到an依次粗略獲取各關(guān)節(jié)點(diǎn)的位置范圍；

6、s3：采用插值法獲取各關(guān)節(jié)點(diǎn)的精確位置；

7、s4：在與目標(biāo)路徑的偏差最小的情況下，以迭代的方式重新規(guī)劃節(jié)點(diǎn)位置，直至各關(guān)節(jié)轉(zhuǎn)角均在轉(zhuǎn)角極限以內(nèi)；對(duì)節(jié)點(diǎn)位置進(jìn)行重新規(guī)劃的原理為：

8、對(duì)于三個(gè)已知位置的相鄰關(guān)節(jié)點(diǎn)ai-1、ai、ai+1，計(jì)算關(guān)節(jié)ai對(duì)應(yīng)的關(guān)節(jié)轉(zhuǎn)角，若其超過(guò)轉(zhuǎn)角極限，則保持關(guān)節(jié)點(diǎn)ai-1位置不變，重新規(guī)劃ai和ai+1的位置至a′i和a′i+1；a′i和a′i+1均位于由ai-1、ai和ai+1三點(diǎn)形成的平面上，且a′i+1位于給定的目標(biāo)路徑上；根據(jù)幾何關(guān)系計(jì)算線段ai-1a′i+1的長(zhǎng)度，采用s2、s3所述的方法計(jì)算a′i+1的位置，基于空間幾何計(jì)算a′i的位置；

9、將和的夾角作為設(shè)定角度qset，設(shè)定角度qset小于轉(zhuǎn)角極限qmax，且兩者之差為迭代容差δ，根據(jù)實(shí)際需求進(jìn)行設(shè)定；以qset為目標(biāo)進(jìn)行迭代。

10、進(jìn)一步地，所述s2的具體操作為：當(dāng)ai-1已知時(shí)，從其后最近的目標(biāo)點(diǎn)開始，依次計(jì)算ai-1和目標(biāo)點(diǎn)之間的歐拉距離，并與li進(jìn)行比較，若|ai-1gj|<li且|ai-1gj+1|>li，則ai位于gj和gj+1之間。

11、進(jìn)一步地，所述s3中，插值法選用三角法，具體操作如下：

12、將gj和gj+1之間的路徑曲線近似為直線段，gj與ai-1的距離為pj，gj+1與ai-1的距離為pj+1，

13、在δai-1gjgj+1中，根據(jù)余弦定理，得到：

14、

15、在δaiai-1gj+1中，根據(jù)余弦定理，得到方程：

16、

17、設(shè)定約束10·|gjgj+1|<|ai-1ai|，|ai-1ai|>|aigj+1|，pj+1>|aigj+1|；得到|aigj+1|為：

18、

19、根據(jù)幾何關(guān)系得到ai的位置表達(dá)式如下：

20、

21、進(jìn)一步地，所述s3中，插值法選用比例近似法，具體操作如下：

22、將gj和gj+1之間的路徑曲線近似為直線段，gj與ai-1的距離為pj，gj+1與ai-1的距離為pj+1；以關(guān)節(jié)點(diǎn)ai-1為圓心，pj為弧長(zhǎng)做圓弧，該圓弧與ai-1ai連線的交點(diǎn)記為a點(diǎn)，與aigj+1連線的交點(diǎn)記為b點(diǎn)；以關(guān)節(jié)點(diǎn)ai-1為圓心，li為弧長(zhǎng)做圓弧，該圓弧與aigj+1連線的交點(diǎn)記為c點(diǎn)；

23、根據(jù)空間幾何，得到b點(diǎn)的坐標(biāo)表達(dá)式如下：

24、b＝ai-1+λ1(g?j+1-ai-1)

25、

26、假定δaiagj相似于δgj+1bgj，根據(jù)三角形相似原理，得到：

27、

28、進(jìn)而得到ai的坐標(biāo)，表達(dá)式如下：

29、ai＝ai-1+λ3(a-ai-1)＝ai-1+λ3·(gj-ai-1+λ2·(ai-1-gj+λ1·(gj-1-ai-1)))

30、a＝gj+λ2(b-gj)

31、

32、進(jìn)一步地，所述s4中，以迭代的方式重新規(guī)劃節(jié)點(diǎn)位置具體通過(guò)如下子步驟實(shí)現(xiàn)：

33、(4.1)按順序計(jì)算新構(gòu)型中各個(gè)關(guān)節(jié)的關(guān)節(jié)轉(zhuǎn)角；其中第i個(gè)關(guān)節(jié)的關(guān)節(jié)轉(zhuǎn)角qi+1表達(dá)式如下：

34、

35、(4.2)順序判斷各關(guān)節(jié)轉(zhuǎn)角是否超過(guò)轉(zhuǎn)角極限，若是，則重新規(guī)劃關(guān)節(jié)點(diǎn)的位置；若否，則進(jìn)行下一關(guān)節(jié)轉(zhuǎn)角的判斷；

36、(4.3)在順序完成所有關(guān)節(jié)點(diǎn)的重新規(guī)劃之后，重新計(jì)算每個(gè)關(guān)節(jié)的等效轉(zhuǎn)角，如果任一關(guān)節(jié)轉(zhuǎn)角超過(guò)轉(zhuǎn)角極限，則將當(dāng)前關(guān)節(jié)點(diǎn)和剩余目標(biāo)點(diǎn)作為后續(xù)跟隨路徑，重復(fù)步驟(4.2)，以設(shè)定角度qset為目標(biāo)進(jìn)行多次迭代，直到所有關(guān)節(jié)等效轉(zhuǎn)角都在轉(zhuǎn)角極限qmax之內(nèi)。

37、本發(fā)明的有益效果是：

38、本發(fā)明方法通過(guò)啟發(fā)式算法快速搜索關(guān)節(jié)點(diǎn)位置，然后通過(guò)迭代不斷微調(diào)關(guān)節(jié)點(diǎn)位置實(shí)現(xiàn)轉(zhuǎn)角限制，使得機(jī)械臂在最大轉(zhuǎn)角內(nèi)以較小的偏差跟蹤目標(biāo)路徑，具有計(jì)算速度快、路徑跟蹤精度高、可移植性好、通用性強(qiáng)的特點(diǎn)，本方法能夠推廣用于任意多關(guān)節(jié)串聯(lián)型機(jī)械臂中，實(shí)現(xiàn)帶有轉(zhuǎn)角限制的實(shí)時(shí)的路徑規(guī)劃。

技術(shù)特征：

1.一種帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法，其特征在于，包括以下步驟：

2.根據(jù)權(quán)利要求1所述的帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法，其特征在于，所述s2的具體操作為：當(dāng)ai-1已知時(shí)，從其后最近的目標(biāo)點(diǎn)開始，依次計(jì)算ai-1和目標(biāo)點(diǎn)之間的歐拉距離，并與li進(jìn)行比較，若|ai-1gj|<li且|ai-1gj+1|>li，則ai位于gj和gj+1之間。

3.根據(jù)權(quán)利要求2所述的帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法，其特征在于，所述s3中，插值法選用三角法，具體操作如下：

4.根據(jù)權(quán)利要求2所述的帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法，其特征在于，所述s3中，插值法選用比例近似法，具體操作如下：

5.根據(jù)權(quán)利要求1所述的帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法，其特征在于，所述s4中，以迭代的方式重新規(guī)劃節(jié)點(diǎn)位置具體通過(guò)如下子步驟實(shí)現(xiàn)：

技術(shù)總結(jié)
本發(fā)明公開了一種帶有轉(zhuǎn)角限制的蛇行路徑跟蹤方法，包括：設(shè)定蛇形機(jī)械臂的目標(biāo)路徑；采用順序搜索法依次粗略獲取各關(guān)節(jié)點(diǎn)的位置范圍；采用插值法獲取各關(guān)節(jié)點(diǎn)的精確位置；在與目標(biāo)路徑的偏差最小的情況下，以迭代的方式重新規(guī)劃節(jié)點(diǎn)位置，直至各關(guān)節(jié)轉(zhuǎn)角均在轉(zhuǎn)角極限以內(nèi)；以設(shè)定角度q<subgt;set</subgt;為目標(biāo)進(jìn)行迭代，q<subgt;set</subgt;小于轉(zhuǎn)角極限q<subgt;max</subgt;，且兩者之差為迭代容差δ，δ根據(jù)實(shí)際需求進(jìn)行設(shè)定。本發(fā)明方法能高效地實(shí)現(xiàn)蛇形機(jī)械臂在轉(zhuǎn)角限制內(nèi)以較小的偏差跟蹤目標(biāo)路徑。

技術(shù)研發(fā)人員：王程,謝海波,王承震,侯永哲,沈洪垚,張斌,楊華勇
受保護(hù)的技術(shù)使用者：浙江大學(xué)高端裝備研究院
技術(shù)研發(fā)日：
技術(shù)公布日：2024/12/19