技術(shù)特征：

1.一種空間移動多臂機器人的移動軌跡規(guī)劃方法，其特征在于包含以下步驟：

(1)根據(jù)空間移動多臂機器人具體位置轉(zhuǎn)移任務(wù)要求，確定第一機械臂兩端所需到達的位置以及第二機械臂兩端所需到達的位置；

(2)根據(jù)步驟(1)中確定的第一機械臂兩端所需到達的位置以及第二機械臂兩端所需到達的位置，解算出第一機械臂兩端的位移、速度和加速度的時間歷程以及第二機械臂兩端的位移、速度和加速度的時間歷程；

(3)規(guī)劃第一機械臂的運動，根據(jù)機械臂運動學(xué)方程以及步驟(2)中確定的第一機械臂兩端的移動軌跡，解算出第一機械臂各個關(guān)節(jié)角位移、速度和加速度的時間歷程；

(4)規(guī)劃第二機械臂的運動，根據(jù)機械臂運動學(xué)方程以及步驟(2)中確定的第二機械臂兩端的移動軌跡，解算出第二機械臂各個關(guān)節(jié)角位移、速度和加速度的時間歷程；

(5)根據(jù)步驟(3)中確定的第一機械臂各個關(guān)節(jié)角位移以及步驟(4)中確定的第二機械臂各個關(guān)節(jié)角位移，確定空間移動多臂機器人的移動軌跡；

(6)根據(jù)所述空間移動多臂機器人的移動軌跡，判斷兩個機械臂之間、機械臂與機器人平臺之間、機械臂與目標(biāo)航天器之間是否發(fā)生碰撞，若均未發(fā)生碰撞，則空間移動多臂機器人的移動軌跡安全，否則表示移動軌跡不安全，返回步驟(1)重新規(guī)劃在多臂移動過程中第一機械臂兩端所需到達的位置以及第二機械臂兩端所需到達的位置。

2.根據(jù)權(quán)利要求1所述的一種空間移動多臂機器人的移動軌跡規(guī)劃方法，其特征在于：所述第一機械臂是指同時連接目標(biāo)航天器和機器人平臺的機械臂，第二機械臂是指僅連接機器人平臺的機械臂。

3.根據(jù)權(quán)利要求1或2所述的一種空間移動多臂機器人的移動軌跡規(guī)劃方法，其特征在于：所述第一機械臂和第二機械臂均為七自由度機械臂，即均包含七個關(guān)節(jié)和七節(jié)機械臂。

4.根據(jù)權(quán)利要求1所述的一種空間移動多臂機器人的移動軌跡規(guī)劃方法，其特征在于：所述步驟(2)根據(jù)第一機械臂兩端所需到達的位置以及第二機械臂兩端所需到達的位置，解算出第一機械臂兩端的位移、速度和加速度的時間歷程以及第二機械臂兩端的位移、速度和加速度的時間歷程，具體為：

$r_w\left(t\right)=r_{w0}\left(t\right)+\frac{6(r_{wf}-r_{w0})}{{t_f}^5}{t}^5-\frac{15(r_{wf}-r_{w0})}{{t_f}^4}{t}^4+\frac{10(r_{wf}-r_{w0})}{{t_f}^3}{t}^3$

${\stackrel{\·}{r}}_w\left(t\right)=\frac{30(r_{wf}-r_{w0})}{{t_f}^5}{t}^4-\frac{60(r_{wf}-r_{w0})}{{t_f}^4}{t}^3+\frac{30(r_{wf}-r_{w0})}{{t_f}^3}{t}^2$

${\stackrel{\·\·}{r}}_w\left(t\right)=\frac{120(r_{wf}-r_{w0})}{{t_f}^5}{t}^3-\frac{180(r_{wf}-r_{w0})}{{t_f}^4}{t}^2+\frac{60(r_{wf}-r_{w0})}{{t_f}^3}t$

其中，r_w表示第一機械臂或第二機械臂某一端的平動位移，表示第一機械臂或第二機械臂某一端的速度,表示第一機械臂或第二機械臂某一端的加速度,r_w0表示第一機械臂或第二機械臂某一端的初始位置,r_wf表示第一機械臂或第二機械臂某一端所需到達的位置，w＝t1時對應(yīng)第一機械臂末端，w＝t2時對應(yīng)第二機械臂末端，w＝b時對應(yīng)第一機械臂或第二機械臂連接機器人平臺的一端，t_f是總移動時間，t∈[0，t_f]。

5.根據(jù)權(quán)利要求1所述的一種空間移動多臂機器人的移動軌跡規(guī)劃方法，其特征在于：所述步驟(3)解算出第一機械臂各個關(guān)節(jié)角位移、速度和加速度的時間歷程，具體為：

(5.1)結(jié)合機械臂的運動學(xué)關(guān)系，寫出第一機械臂末端位置，機器人平臺位置與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系

$r_{t1}\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{7}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^1\right)r_i$

其中，r_t1表示第一機械臂末端相對機器人平臺質(zhì)心的位置，r_b表示機器人平臺質(zhì)心的平動位移，r_b1表示第一機械臂的安裝位置，r_i表示第i節(jié)機械臂體坐標(biāo)系中坐標(biāo)原點相對該節(jié)末端的矢徑，i＝1,2,..,7；是根據(jù)第一機械臂各關(guān)節(jié)轉(zhuǎn)角得到的第i節(jié)機械臂體坐標(biāo)系與機器人平臺坐標(biāo)系之間的轉(zhuǎn)換矩陣；

對(5.1)中的等式關(guān)系求導(dǎo)，得到第一機械臂末端速度與其他部位間的相對關(guān)系

${\stackrel{\·}{r}}_{t1}\left(t\right)={\stackrel{\·}{r}}_b\left(t\right)-\underset{i=1}{\overset{7}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^1\right){\tilde{r}}_{i7}{\mathrm{\Γ}}_i{\stackrel{\·}{\mathrm{\θ}}}_i^1$

其中，和為第一機械臂末端和機器人平臺的速度，機器人平臺的速度即為機械臂另一端的速度，r_i7表示在第i節(jié)機械臂坐標(biāo)系中第i節(jié)機械臂坐標(biāo)系原點到機械臂末端的矢徑，Γ_i表示第i節(jié)關(guān)節(jié)的旋轉(zhuǎn)方向，表示第一機械臂的第i個關(guān)節(jié)角速度；

(5.2)結(jié)合機械臂的運動學(xué)方程，寫出第一機械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系

${\mathrm{\ω}}_{t1}=\underset{i=1}{\overset{7}{\Σ}}{A}_{7i}\left({\mathrm{\θ}}_i^1\right){\mathrm{\Γ}}_i{\stackrel{\·}{\mathrm{\θ}}}_i^1$

其中，是根據(jù)第一機械臂各關(guān)節(jié)轉(zhuǎn)角得到的第i節(jié)機械臂體坐標(biāo)系與第7節(jié)機械臂體坐標(biāo)系之間的轉(zhuǎn)換矩陣；ω_t1是第一機械臂末端姿態(tài)角速度，且通過五次多項式規(guī)劃得到，具體為：

${\stackrel{\·}{\mathrm{\σ}}}_1\left(t\right)=\frac{30{\mathrm{\σ}}_{m1}}{{t_f}^5}{t}^4-\frac{60{\mathrm{\σ}}_{m1}}{{t_f}^4}{t}^3+\frac{30{\mathrm{\σ}}_{m1}}{{t_f}^3}{t}^2$

σ_m1和n_e1由第一機械臂末端初始姿態(tài)與期望姿態(tài)之間的姿態(tài)誤差四元數(shù)得出

${\mathrm{\σ}}_{m1}=2arccos\left(q_{e0}^1\right),n_{e1}=\frac{1}{sin\frac{{\mathrm{\σ}}_{m1}}{2}}{\stackrel{\‾}{q}}_e^1;$

(5.3)將步驟(5.1)中得到的第一機械臂末端速度與其他部位間的相對關(guān)系與步驟(5.2)中得到的第一機械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系聯(lián)立，通過廣義雅可比矩陣J描述空間機器人平臺質(zhì)心和第一機械臂末端運動速度與關(guān)節(jié)速度之間的關(guān)系，寫出第一機械臂的運動學(xué)方程

${\stackrel{\·}{X}}_{E1}=J{\stackrel{\·}{\mathrm{\η}}}_1$

其中

(5.4)對步驟(5.3)中得到的第一機械臂的運動學(xué)方程利用逆運動學(xué)反解，得到第一機器臂各關(guān)節(jié)的移動軌跡，即得到第一機械臂各個關(guān)節(jié)角位移、速度和加速度的時間歷程，具體為：

${\mathrm{\η}}_1=\∫{\stackrel{\·}{\mathrm{\η}}}_{1}dt;$

${\stackrel{\·}{\mathrm{\η}}}_1=J^{+}{\stackrel{\·}{X}}_{E1};$

${\stackrel{\·\·}{\mathrm{\η}}}_1=J^{+}{\stackrel{\·\·}{X}}_{E1}+{\stackrel{\·}{J}}^{+}{\stackrel{\·}{X}}_{E1};$

其中J⁺為廣義雅可比矩陣J的Moore-Pseudo廣義逆，記為J⁺＝J^T(JJ^T)^-1；和分別是和J⁺的一階導(dǎo)數(shù)。

6.根據(jù)權(quán)利要求1所述的一種空間移動多臂機器人的移動軌跡規(guī)劃方法，其特征在于：所述步驟(4)解算出第二機械臂各個關(guān)節(jié)角位移、速度和加速度的時間歷程，具體為：

(6.1)結(jié)合機械臂的運動學(xué)關(guān)系，寫出第二機械臂末端位置，機器人平臺位置與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系

$r_{t2}\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{7}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^2\right)r_i$

其中，r_t2表示第二機械臂末端相對機器人平臺質(zhì)心的位置，r_b表示機器人平臺質(zhì)心的平動位移，r_b2表示第二機械臂的安裝位置，r_i表示第i節(jié)機械臂體坐標(biāo)系中坐標(biāo)原點相對該節(jié)末端的矢徑，i＝1,2,..,7；是根據(jù)第二機械臂各關(guān)節(jié)轉(zhuǎn)角得到的第i節(jié)機械臂體坐標(biāo)系與機器人平臺坐標(biāo)系之間的轉(zhuǎn)換矩陣；

對(6.1)中的等式關(guān)系求導(dǎo)，得到第二機械臂末端速度與其他部位間的相對關(guān)系

${\stackrel{\·}{r}}_{t2}\left(t\right)={\stackrel{\·}{r}}_b\left(t\right)-\underset{i=1}{\overset{7}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^2\right){\tilde{r}}_{i7}{\mathrm{\Γ}}_i{\stackrel{\·}{\mathrm{\θ}}}_i^2$

其中，和為第二機械臂末端和機器人平臺的速度，機器人平臺的速度即為機械臂另一端的速度，r_i7表示在第i節(jié)機械臂坐標(biāo)系中第i節(jié)機械臂坐標(biāo)系原點到機械臂末端的矢徑，Γ_i表示第i節(jié)關(guān)節(jié)的旋轉(zhuǎn)方向，表示第二機械臂的第i個關(guān)節(jié)角速度；

(6.2)結(jié)合機械臂的運動學(xué)方程，寫出第二機械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系

${\mathrm{\ω}}_{t2}=\underset{i=1}{\overset{7}{\Σ}}{A}_{7i}\left({\mathrm{\θ}}_i^2\right){\mathrm{\Γ}}_i{\stackrel{\·}{\mathrm{\θ}}}_i^2$

其中，是根據(jù)第二機械臂各關(guān)節(jié)轉(zhuǎn)角得到的第i節(jié)機械臂體坐標(biāo)系與第7節(jié)機械臂體坐標(biāo)系之間的轉(zhuǎn)換矩陣；ω_t2是第二機械臂末端姿態(tài)角速度，且通過五次多項式規(guī)劃得到，具體為：

${\stackrel{\·}{\mathrm{\σ}}}_2\left(t\right)=\frac{30{\mathrm{\σ}}_{m2}}{{t_f}^5}{t}^4-\frac{60{\mathrm{\σ}}_{m2}}{{t_f}^4}{t}^3+\frac{30{\mathrm{\σ}}_{m2}}{{t_f}^3}{t}^2$

σ_m2和n_e2由第二機械臂末端初始姿態(tài)與期望姿態(tài)之間的姿態(tài)誤差四元數(shù)得出

${\mathrm{\σ}}_{m2}=2\arccos \left(q_{e0}^2\right),n_{e2}=\frac{1}{sin\frac{{\mathrm{\σ}}_{m2}}{2}}{\stackrel{\‾}{q}}_e^2$

(6.3)將步驟(6.1)中得到的第二機械臂末端速度與其他部位間的相對關(guān)系與步驟(6.2)中得到的第二機械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系聯(lián)立，通過廣義雅可比矩陣J描述空間機器人平臺質(zhì)心和第二機械臂末端運動速度與關(guān)節(jié)速度之間的關(guān)系，寫出第二機械臂的運動學(xué)方程

${\stackrel{\·}{X}}_{E2}=J{\stackrel{\·}{\mathrm{\η}}}_2$

其中

(6.4)對步驟(6.3)中得到的第二機械臂的運動學(xué)方程利用逆運動學(xué)反解，得到第二機器臂各關(guān)節(jié)的移動軌跡，即得到第二機械臂各個關(guān)節(jié)角位移、速度和加速度的時間歷程，具體為：

${\mathrm{\η}}_2=\∫{\stackrel{\·}{\mathrm{\η}}}_{2}dt$

${\stackrel{\·}{\mathrm{\η}}}_2=J^{+}{\stackrel{\·}{X}}_{E2}$

${\stackrel{\·\·}{\mathrm{\η}}}_2=J^{+}{\stackrel{\·\·}{X}}_{E2}+{\stackrel{\·}{J}}^{+}{\stackrel{\·}{X}}_{E2}$

其中J⁺為廣義雅可比矩陣J的Moore-Pseudo廣義逆，記為J⁺＝J^T(JJ^T)^-1；和分別是和J⁺的一階導(dǎo)數(shù)。

7.根據(jù)權(quán)利要求1所述的一種空間移動多臂機器人的移動軌跡規(guī)劃方法，其特征在于：所述步驟(5)根據(jù)第一機械臂各個關(guān)節(jié)角位移以及第二機械臂各個關(guān)節(jié)角位移確定空間移動多臂機器人的移動軌跡，即各節(jié)機械臂的移動軌跡，具體為：

$\begin{array}{cc}{r}_1^1\left(t\right)=r_b\left(t\right)+r_{b1}+A_{b1}\left({\mathrm{\θ}}_1^1\right)r_1& r_1^2\left(t\right)=r_b\left(t\right)+r_{b2}+A_{b1}\left({\mathrm{\θ}}_1^2\right)r_1\end{array}$

$\begin{array}{cc}{r}_2^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{2}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^1\right)r_i& r_2^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{2}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^2\right)r_i\end{array}$

$\begin{array}{cc}{r}_3^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{3}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^1\right)r_i& r_3^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{3}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^2\right)r_i\end{array}$

$r_4^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{4}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^1\right)r_i,r_4^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{4}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^2\right)r_i;$

$\begin{array}{cc}{r}_5^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{5}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^1\right)r_i& r_5^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{5}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^2\right)r_i\end{array}$

$\begin{array}{cc}{r}_6^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{6}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^1\right)r_i& r_6^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{6}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^2\right)r_i\end{array}$

$\begin{array}{cc}{r}_7^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{7}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^1\right)r_i& r_7^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{7}{\Σ}}{A}_{bi}\left({\mathrm{\θ}}_i^2\right)r_i\end{array}$

其中，依次為第一機械臂的七節(jié)機械臂的移動軌跡，依次為第二機械臂的七節(jié)機械臂的移動軌跡。

8.根據(jù)權(quán)利要求1所述的一種空間移動多臂機器人的移動軌跡規(guī)劃方法，其特征在于：所述步驟(6)根據(jù)所述空間移動多臂機器人的移動軌跡，判斷兩個機械臂之間、機械臂與機器人平臺之間、機械臂與目標(biāo)航天器之間是否發(fā)生碰撞，具體為：

(8.1)機械臂與機器人平臺碰撞檢測

將各節(jié)機械臂和機器人平臺投影到機器人平臺y-z截面，若機器人平臺y-z截面中心O到各節(jié)機械臂的最小距離OP大于最小安全距離a，則保證機械臂與機器人平臺不碰撞，具體為：

記第j機械臂的第i節(jié)機械臂兩端和在y-z截面的坐標(biāo)為和j＝1,2，i＝1,2,..,7；中間變量

若0≤λ₁≤1，不發(fā)生碰撞；則發(fā)生碰撞；

若λ₁>1，不發(fā)生碰撞；則發(fā)生碰撞；

若λ₁<0，不發(fā)生碰撞；則發(fā)生碰撞；

(8.2)機械臂與目標(biāo)航天器碰撞檢測

將各節(jié)機械臂和目標(biāo)航天器投影到目標(biāo)航天器x-y截面，若目標(biāo)航天器x-y截面中心O′到各節(jié)機械臂的最小距離O′Q大于最小安全距離r，則保證機械臂與目標(biāo)航天器不碰撞，具體為：

記第j機械臂的第i節(jié)機械臂兩端r_i-1，j和在x-y截面的坐標(biāo)為和j＝1，2，i＝1，2，..，7；中間變量

若0≤λ₂≤1，不發(fā)生碰撞；則發(fā)生碰撞；

若λ₂＞1，不發(fā)生碰撞；則發(fā)生碰撞；

若λ₂＜0，不發(fā)生碰撞；則發(fā)生碰撞；

(8.3)兩個機械臂之間碰撞檢測

任意兩節(jié)機械臂之間的最小距離L大于0，則保證機械臂之間不碰撞，具體為：

記中間變量u、v：

$u=\frac{{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_k^{j_2}-r_{k-1}^{j_2}){(r_k^{j_2}-r_{k-1}^{j_2})}^T(r_{i-1}^{j_1}-r_{k-1}^{j_2})-\left|\right|r_k^{j_2}-r_{k-1}^{j_2}|{|}^2{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_{i-1}^{j_1}-r_{k-1}^{j_2})}{\left|\right|r_i^{j_1}-r_{i-1}^{j_1}\left|{|}^2\right||r_k^{j_2}-r_{k-1}^{j_2}|{|}^2-{\&lsqb;{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_k^{j_2}-r_{k-1}^{j_2})\&rsqb;}^2}$

$v=\frac{{(r_k^{j_2}-r_{k-1}^{j_2})}^T(r_{i-1}^{j_1}-r_{k-1}^{j_2})\left|\right|r_i^{j_1}-r_{i-1}^{j_1}|{|}^2-{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_{i-1}^{j_1}-r_{k-1}^{j_2}){(r_k^{j_2}-r_{k-1}^{j_2})}^T(r_i^{j_1}-r_{i-1}^{j_1})}{\left|\right|r_i^{j_1}-r_{i-1}^{j_1}\left|{|}^2\right||r_k^{j_2}-r_{k-1}^{j_2}|{|}^2-{\&lsqb;{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_k^{j_2}-r_{k-1}^{j_2})\&rsqb;}^2}$

其中j₁＝1，2，j₂＝1，2，i＝1，2，..，7，k＝1，2，..，7，且j₁＝j(luò)₂和i＝k不同時成立。

當(dāng)且僅當(dāng)0≤u≤1、0≤v≤1、時，兩節(jié)機械臂發(fā)生碰撞，否則不發(fā)生碰撞。