一種空間機(jī)械臂的任務(wù)規(guī)劃方法及系統(tǒng)的制作方法
【專利說(shuō)明】一種空間機(jī)械臂的任務(wù)規(guī)劃方法及系統(tǒng) 【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及自動(dòng)化控制技術(shù)，尤其涉及一種空間機(jī)械臂的任務(wù)規(guī)劃方法及系統(tǒng)。 【【背景技術(shù)】】
[0002] 隨著空間探索的不斷深入，各類航天器的搭建、維修及服務(wù)等在軌操作任務(wù)變得 越發(fā)繁重和復(fù)雜。由于空間環(huán)境具有高真空、強(qiáng)輻射、高低溫交變等特點(diǎn)，宇航員出艙執(zhí)行 任務(wù)具有很大風(fēng)險(xiǎn)。利用空間機(jī)械臂協(xié)助甚至代替宇航員執(zhí)行任務(wù)，不僅可以保護(hù)宇航員 的生命安全，也可以提高作業(yè)效率，節(jié)省成本。
[0003] 空間機(jī)械臂多處在狹小、復(fù)雜和供給匱乏的工作環(huán)境中，能源供給和維護(hù)保養(yǎng)等 方面成本較高，因此其運(yùn)動(dòng)過程受到來(lái)自自身和外界的多重約束。另外，受計(jì)算機(jī)性能限制 和在軌控制系統(tǒng)可靠性要求，空間機(jī)械臂的任務(wù)規(guī)劃方式已無(wú)法滿足日益復(fù)雜的在軌任務(wù) 需求。
[0004] 目前，空間機(jī)械臂的控制系統(tǒng)主要進(jìn)行任務(wù)規(guī)劃、路徑規(guī)劃和運(yùn)動(dòng)控制三方面，其 中任務(wù)規(guī)劃作為控制系統(tǒng)的最高層，負(fù)責(zé)任務(wù)目標(biāo)的接收、分析，進(jìn)一步完成任務(wù)規(guī)劃，然 而，現(xiàn)有技術(shù)中只能實(shí)現(xiàn)對(duì)簡(jiǎn)單的直線任務(wù)的任務(wù)規(guī)劃，對(duì)于比較復(fù)雜的任務(wù)，如空間機(jī)械 臂在保持末端姿態(tài)恒定不變的前提下繞過障礙物等，還無(wú)法實(shí)現(xiàn)任務(wù)規(guī)劃。 【
【發(fā)明內(nèi)容】
】
[0005] 有鑒于此，本發(fā)明實(shí)施例提供了一種空間機(jī)械臂的任務(wù)規(guī)劃方法及系統(tǒng)，用以解 決現(xiàn)有技術(shù)中只能對(duì)簡(jiǎn)單的任務(wù)進(jìn)行任務(wù)規(guī)劃的問題。
[0006] 本發(fā)明實(shí)施例提供了一種空間機(jī)械臂的任務(wù)規(guī)劃方法，包括：
[0007] 獲取任務(wù)起始點(diǎn)、任務(wù)目標(biāo)點(diǎn)、約束條件和路徑規(guī)劃算法；
[0008] 根據(jù)所述任務(wù)起始點(diǎn)、所述任務(wù)目標(biāo)點(diǎn)、所述約束條件和所述路徑規(guī)劃算法，獲取 所述任務(wù)起始點(diǎn)與所述任務(wù)目標(biāo)點(diǎn)之間的非直線路徑上的至少一個(gè)任務(wù)中間點(diǎn)；
[0009] 根據(jù)所述任務(wù)起始點(diǎn)、任務(wù)目標(biāo)點(diǎn)和所述至少一個(gè)任務(wù)中間點(diǎn)，完成空間機(jī)械臂 的任務(wù)規(guī)劃。
[0010]如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述根據(jù)所 述任務(wù)起始點(diǎn)、所述任務(wù)目標(biāo)點(diǎn)、所述約束條件和所述路徑規(guī)劃算法，獲取所述任務(wù)起始點(diǎn) 與所述任務(wù)目標(biāo)點(diǎn)之間的非直線路徑上的至少一個(gè)任務(wù)中間點(diǎn)，包括：
[0011 ]根據(jù)所述任務(wù)起始點(diǎn)，循環(huán)獲取Μ個(gè)候選中間點(diǎn)，直到獲取到的Μ個(gè)候選中間點(diǎn)中 包含所述任務(wù)目標(biāo)點(diǎn)以及利用所述任務(wù)目標(biāo)點(diǎn)又獲取到Μ個(gè)候選中間點(diǎn)時(shí)為止；
[0012] 利用所述路徑規(guī)劃算法和所述約束條件，對(duì)每次獲取的所述Μ個(gè)候選中間點(diǎn)進(jìn)行 篩選，以獲得Ρ個(gè)候選中間點(diǎn)；以及，獲得并記錄所述Ρ個(gè)候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的名 義父節(jié)點(diǎn)；
[0013] 從所述任務(wù)目標(biāo)點(diǎn)開始，根據(jù)所述任務(wù)目標(biāo)點(diǎn)的名義父節(jié)點(diǎn)以及每個(gè)候選中間點(diǎn) 的名義父節(jié)點(diǎn)進(jìn)行搜索路徑回溯，以獲得所述至少一個(gè)任務(wù)中間點(diǎn)。
[0014] 如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述根據(jù)所 述任務(wù)起始點(diǎn)，循環(huán)獲取Μ個(gè)候選中間點(diǎn)，包括：
[0015] 對(duì)于上一次獲取的Μ個(gè)候選中間點(diǎn)經(jīng)過篩選后得到的Ρ個(gè)候選中間點(diǎn)，獲取所述Ρ 個(gè)候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的總代價(jià)；
[0016] 獲取所述Ρ個(gè)候選中間點(diǎn)中總代價(jià)最小的候選中間點(diǎn)；
[0017] 根據(jù)所述總代價(jià)最小的候選中間點(diǎn)、預(yù)設(shè)的搜索維度、位置搜索步長(zhǎng)和姿態(tài)搜索 步長(zhǎng)，獲取Μ個(gè)候選中間點(diǎn)。
[0018] 如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述利用所 述路徑規(guī)劃算法和所述約束條件，對(duì)每次獲取的所述Μ個(gè)候選中間點(diǎn)進(jìn)行篩選，以獲得Ρ個(gè) 候選中間點(diǎn)，包括：
[0019] 在根據(jù)所述路徑規(guī)劃算法計(jì)算所述Μ個(gè)候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的總代價(jià)的 過程中，根據(jù)所述約束條件，篩除所述Μ個(gè)候選中間點(diǎn)中不滿足所述約束條件的Ν個(gè)候選中 間點(diǎn)，以獲得Ρ個(gè)候選中間點(diǎn)。
[0020] 如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述獲得Ρ個(gè) 候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的名義父節(jié)點(diǎn)，包括：
[0021 ]對(duì)于每次獲得的Ρ個(gè)候選中間點(diǎn)中的每個(gè)候選中間點(diǎn)，獲得該候選中間點(diǎn)的原始 父節(jié)點(diǎn)到該候選中間點(diǎn)的第一總代價(jià)，以及，該候選中間點(diǎn)的原始父節(jié)點(diǎn)的名義父節(jié)點(diǎn)到 該候選中間點(diǎn)的第二總代價(jià)；
[0022] 若所述第一總代價(jià)大于或者等于所述第二總代價(jià)，將該候選中間點(diǎn)的原始父節(jié)點(diǎn) 的名義父節(jié)點(diǎn)作為該候選中間點(diǎn)的名義父節(jié)點(diǎn)；
[0023] 若所述第一總代價(jià)小于所述第二總代價(jià)，將該候選中間點(diǎn)的原始父節(jié)點(diǎn)作為該候 選中間點(diǎn)的名義父節(jié)點(diǎn)。
[0024] 如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述獲取Ρ個(gè) 候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的總代價(jià)，包括：
[0025] 利用如下公式獲得每個(gè)候選中間點(diǎn)的總代價(jià)：
[0026] f(pc) =g(pf)+C(pf,pc)+h(pc)
[0027] 其中，f(Pc)表示候選中間點(diǎn)Pc的總代價(jià);g(Pf)表示任務(wù)起始點(diǎn)到候選中間點(diǎn)Pc的 名義父節(jié)點(diǎn)Pf的實(shí)際代價(jià);C(Pf,Pc)表示候選中間點(diǎn)Pc的名義父節(jié)點(diǎn)Pf到候選中間點(diǎn)Pc的實(shí) 際代價(jià);h(P。)表示候選中間點(diǎn)到任務(wù)目標(biāo)點(diǎn)的估計(jì)代價(jià)。
[0028] 本發(fā)明實(shí)施例還提供了一種空間機(jī)械臂的任務(wù)規(guī)劃系統(tǒng)，包括：
[0029] 信息輸入單元，用于獲取任務(wù)起始點(diǎn)、任務(wù)目標(biāo)點(diǎn)、約束條件和路徑規(guī)劃算法；
[0030] 中間點(diǎn)獲取單元，用于根據(jù)所述任務(wù)起始點(diǎn)、所述任務(wù)目標(biāo)點(diǎn)、所述約束條件和所 述路徑規(guī)劃算法，獲取所述任務(wù)起始點(diǎn)與所述任務(wù)目標(biāo)點(diǎn)之間的非直線路徑上的至少一個(gè) 任務(wù)中間點(diǎn)；
[0031] 任務(wù)規(guī)劃單元，用于根據(jù)所述任務(wù)起始點(diǎn)、任務(wù)目標(biāo)點(diǎn)和所述至少一個(gè)任務(wù)中間 點(diǎn)，完成空間機(jī)械臂的任務(wù)規(guī)劃。
[0032] 如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述中間點(diǎn) 獲取單元，具體用于：
[0033] 根據(jù)所述任務(wù)起始點(diǎn)，循環(huán)獲取Μ個(gè)候選中間點(diǎn)，直到獲取到的Μ個(gè)候選中間點(diǎn)中 包含所述任務(wù)目標(biāo)點(diǎn)以及利用所述任務(wù)目標(biāo)點(diǎn)又獲取到Μ個(gè)候選中間點(diǎn)時(shí)為止；
[0034]利用所述路徑規(guī)劃算法和所述約束條件，對(duì)每次獲取的所述Μ個(gè)候選中間點(diǎn)進(jìn)行 篩選，以獲得Ρ個(gè)候選中間點(diǎn)；以及，獲得并記錄所述Ρ個(gè)候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的名 義父節(jié)點(diǎn)；
[0035]從所述任務(wù)目標(biāo)點(diǎn)開始，根據(jù)所述任務(wù)目標(biāo)點(diǎn)的名義父節(jié)點(diǎn)以及每個(gè)候選中間點(diǎn) 的名義父節(jié)點(diǎn)進(jìn)行搜索路徑回溯，以獲得所述至少一個(gè)任務(wù)中間點(diǎn)。
[0036]如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述中間點(diǎn) 獲取單元用于根據(jù)所述任務(wù)起始點(diǎn)，循環(huán)獲取Μ個(gè)候選中間點(diǎn)時(shí)，具體用于：
[0037]對(duì)于上一次獲取的Μ個(gè)候選中間點(diǎn)經(jīng)過篩選后得到的Ρ個(gè)候選中間點(diǎn)，獲取所述Ρ 個(gè)候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的總代價(jià)；
[0038]獲取所述Ρ個(gè)候選中間點(diǎn)中總代價(jià)最小的候選中間點(diǎn)；
[0039]根據(jù)所述總代價(jià)最小的候選中間點(diǎn)、預(yù)設(shè)的搜索維度、位置搜索步長(zhǎng)和姿態(tài)搜索 步長(zhǎng)，獲取Μ個(gè)候選中間點(diǎn)。
[0040]如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述中間點(diǎn) 獲取單元用于利用所述路徑規(guī)劃算法和所述約束條件，對(duì)每次獲取的所述Μ個(gè)候選中間點(diǎn) 進(jìn)行篩選，以獲得Ρ個(gè)候選中間點(diǎn)時(shí)，具體用于：
[0041]在根據(jù)所述路徑規(guī)劃算法計(jì)算所述Μ個(gè)候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的總代價(jià)的 過程中，根據(jù)所述約束條件，篩除所述Μ個(gè)候選中間點(diǎn)中不滿足所述約束條件的Ν個(gè)候選中 間點(diǎn)，以獲得Ρ個(gè)候選中間點(diǎn)。
[0042]如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述中間點(diǎn) 獲取單元用于獲得Ρ個(gè)候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的名義父節(jié)點(diǎn)時(shí)，具體用于：
[0043]對(duì)于每次獲得的Ρ個(gè)候選中間點(diǎn)中的每個(gè)候選中間點(diǎn)，獲得該候選中間點(diǎn)的原始 父節(jié)點(diǎn)到該候選中間點(diǎn)的第一總代價(jià)，以及，該候選中間點(diǎn)的原始父節(jié)點(diǎn)的名義父節(jié)點(diǎn)到 該候選中間點(diǎn)的第二總代價(jià)；
[0044]若所述第一總代價(jià)大于或者等于所述第二總代價(jià)，將該候選中間點(diǎn)的原始父節(jié)點(diǎn) 的名義父節(jié)點(diǎn)作為該候選中間點(diǎn)的名義父節(jié)點(diǎn)；
[0045]若所述第一總代價(jià)小于所述第二總代價(jià)，將該候選中間點(diǎn)的原始父節(jié)點(diǎn)作為該候 選中間點(diǎn)的名義父節(jié)點(diǎn)。
[0046]如上所述的方面和任一可能的實(shí)現(xiàn)方式，進(jìn)一步提供一種實(shí)現(xiàn)方式，所述中間點(diǎn) 獲取單元用于獲取Ρ個(gè)候選中間點(diǎn)中每個(gè)候選中間點(diǎn)的總代價(jià)時(shí)，具體用于：
[0047]利用如下公式獲得每個(gè)候選中間點(diǎn)的總代價(jià)：
[0048] f(pc) =g(pf)+C(pf,pc)+h(pc)
[0049]其中，f(p。)表示候選中間點(diǎn)p。的總代價(jià);g(Pf)表示任務(wù)起始點(diǎn)到候選中間點(diǎn)p。的 名義父節(jié)點(diǎn)Pf的實(shí)際代價(jià);C(Pf,Pc)表示候選中間點(diǎn)Pc的名義父節(jié)點(diǎn)Pf到候選中間點(diǎn)Pc的實(shí) 際代價(jià);h(P。)表示候選中間點(diǎn)到任務(wù)目標(biāo)點(diǎn)的估計(jì)代價(jià)。
[0050]由以上技術(shù)方案可以看出，本發(fā)明實(shí)施例具有以下有益效果：
[0051]根據(jù)本發(fā)明實(shí)施例提供的技術(shù)方案，可以獲取到任務(wù)起始點(diǎn)與任務(wù)目標(biāo)點(diǎn)之間的 非直線路徑上的任務(wù)中間點(diǎn)，從而能夠利用任務(wù)中間點(diǎn)完成復(fù)雜任務(wù)