1.一種多冗余度機械臂系統(tǒng)的協(xié)同控制方法,其特征在于,所述多冗余度機械臂系統(tǒng)包括兩個以上的冗余度機械臂,所述兩個以上的冗余度機械臂在通信拓?fù)鋱D中連通;
所述控制方法包括:
確定所述兩個以上的冗余度機械臂中至少一個冗余度機械臂為目標(biāo)冗余度機械臂;
獲取所述目標(biāo)冗余度機械臂的相鄰冗余度機械臂的位置和速度信息,所述相鄰冗余度機械臂與所述目標(biāo)冗余度機械臂在通信拓?fù)鋱D中相鄰連接;
獲取所述目標(biāo)冗余度機械臂的參考點軌跡信息,所述參考點軌跡信息由預(yù)設(shè)的目標(biāo)參考點和所述目標(biāo)參考點的期望軌跡確定;
根據(jù)所述位置和速度信息,以及所述參考點軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機械臂對應(yīng)的雅可比矩陣等式;
在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下,根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機械臂的控制信號;
根據(jù)所述控制信號控制所述目標(biāo)冗余度機械臂,使得所述目標(biāo)冗余度機械臂實現(xiàn)重復(fù)運動。
2.根據(jù)權(quán)利要求1所述的協(xié)同控制方法,其特征在于,所述根據(jù)所述位置和速度信息,以及所述參考點軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機械臂對應(yīng)的雅可比矩陣等式具體包括:
構(gòu)建所述目標(biāo)冗余度機械臂對應(yīng)的權(quán)重矩陣,所述權(quán)重矩陣中的各個元素為所述目標(biāo)冗余度機械臂與所述兩個以上的冗余度機械臂中的各個機械臂之間分別對應(yīng)的第一連通權(quán)值;
根據(jù)所述參考點軌跡信息的獲取狀態(tài)確定所述目標(biāo)冗余度機械臂的第二連通權(quán)值;
根據(jù)所述位置和速度信息、所述參考點軌跡信息、所述權(quán)重矩陣和所述第二連通權(quán)值構(gòu)建所述目標(biāo)冗余度機械臂對應(yīng)的雅可比矩陣等式。
3.根據(jù)權(quán)利要求2所述的協(xié)同控制方法,其特征在于,所述第一連通權(quán)值通過以下步驟確定:
將所述目標(biāo)冗余度機械臂與所述目標(biāo)冗余度機械臂自身之間的第一連通權(quán)值設(shè)置為1;
將所述目標(biāo)冗余度機械臂與相鄰冗余度機械臂之間的第一連通權(quán)值設(shè)置為1;
將所述目標(biāo)冗余度機械臂與所述兩個以上的冗余度機械臂中的非自身且非相鄰冗余度機械臂之間的第一連通權(quán)值設(shè)置為0;
所述根據(jù)所述參考點軌跡信息的獲取狀態(tài)確定所述目標(biāo)冗余度機械臂的第二連通權(quán)值具體包括:
判斷所述目標(biāo)冗余度機械臂是否獲取到所述參考點軌跡信息;
若所述目標(biāo)冗余度機械臂獲取到所述參考點軌跡信息,則將所述第二連通權(quán)值設(shè)置為1;
若所述目標(biāo)冗余度機械臂未獲取到所述參考點軌跡信息,則將所述第二連通權(quán)值設(shè)置為0;
所述雅可比矩陣等式的通用表達(dá)式為:
其中,Ji表示目標(biāo)冗余度機械臂i的雅可比矩陣,為Ji的時間導(dǎo)數(shù);表示目標(biāo)冗余度機械臂i的關(guān)節(jié)速度向量,為的時間導(dǎo)數(shù);表示與目標(biāo)冗余度機械臂i之間的第一連通權(quán)值為1的機械臂的編號;Aij表示目標(biāo)冗余度機械臂i的權(quán)重矩陣,所述權(quán)重矩陣中的各個元素為目標(biāo)冗余度機械臂i與冗余度機械臂j之間的第一連通權(quán)值;ρi表示目標(biāo)冗余度機械臂i的第二連通權(quán)值;δi=ri-rirp為所述位置信息,ri為目標(biāo)冗余度機械臂i的末端執(zhí)行器位置,rirp為預(yù)設(shè)的目標(biāo)冗余度機械臂i的末端執(zhí)行器相對于所述目標(biāo)參考點的距離向量;為所述速度信息,為目標(biāo)冗余度機械臂i的末端執(zhí)行器速度,rd為所述目標(biāo)參考點的期望位置,為所述目標(biāo)參考點的期望速度;c0>0為控制算法收斂速度的參數(shù),其越大則代表收斂速度越快。
4.根據(jù)權(quán)利要求1至3中任一項所述的協(xié)同控制方法,其特征在于,所述在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下,根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機械臂的控制信號具體包括:
設(shè)置最小化性能指標(biāo)為重復(fù)運動,在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下確定二次型優(yōu)化結(jié)果;
將所述二次型優(yōu)化結(jié)果轉(zhuǎn)換為標(biāo)準(zhǔn)二次規(guī)劃;
對所述標(biāo)準(zhǔn)二次規(guī)劃進(jìn)行求解,得到求解結(jié)果;
根據(jù)所述求解結(jié)果確定所述目標(biāo)冗余度機械臂的控制信號。
5.根據(jù)權(quán)利要求3所述的協(xié)同控制方法,其特征在于,所述二次型優(yōu)化為:
在第一約束條件下使得第一式子最小化;
所述第一約束條件包括:
雅可比矩陣等式約束條件
關(guān)節(jié)角度極限約束條件
關(guān)節(jié)速度極限約束條件
關(guān)節(jié)加速度極限約束條件
所述第一式子為
其中,上標(biāo)T表示矩陣和向量的轉(zhuǎn)置;c1>0和c2>0是用來控制關(guān)節(jié)位移幅值和收斂率的設(shè)計參數(shù),θi(0)表示目標(biāo)冗余度機械臂i的初始關(guān)節(jié)角;表示目標(biāo)冗余度機械臂i的關(guān)節(jié)角上下限;表示目標(biāo)冗余度機械臂i的關(guān)節(jié)速度上下限,表示目標(biāo)冗余度機械臂i的關(guān)節(jié)加速度上下限。
6.一種多冗余度機械臂系統(tǒng)的協(xié)同控制裝置,其特征在于,所述多冗余度機械臂系統(tǒng)包括兩個以上的冗余度機械臂,所述兩個以上的冗余度機械臂在通信拓?fù)鋱D中連通;
所述控制裝置包括:
目標(biāo)機械臂確定模塊,用于確定所述兩個以上的冗余度機械臂中至少一個冗余度機械臂為目標(biāo)冗余度機械臂;
信息獲取模塊,用于獲取所述目標(biāo)冗余度機械臂的相鄰冗余度機械臂的位置和速度信息,所述相鄰冗余度機械臂與所述目標(biāo)冗余度機械臂在通信拓?fù)鋱D中相鄰連接;
參考點軌跡信息獲取模塊,用于獲取所述目標(biāo)冗余度機械臂的參考點軌跡信息,所述參考點軌跡信息由預(yù)設(shè)的目標(biāo)參考點和所述目標(biāo)參考點的期望軌跡確定;
等式構(gòu)建模塊,用于根據(jù)所述位置和速度信息,以及所述參考點軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機械臂對應(yīng)的雅可比矩陣等式;
控制信號確定模塊,用于在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下,根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機械臂的控制信號;
機械臂控制模塊,用于根據(jù)所述控制信號控制所述目標(biāo)冗余度機械臂,使得所述目標(biāo)冗余度機械臂實現(xiàn)重復(fù)運動。
7.根據(jù)權(quán)利要求6所述的協(xié)同控制裝置,其特征在于,所述等式構(gòu)建模塊具體包括:
權(quán)重矩陣構(gòu)建單元,用于構(gòu)建所述目標(biāo)冗余度機械臂對應(yīng)的權(quán)重矩陣,所述權(quán)重矩陣中的各個元素為所述目標(biāo)冗余度機械臂與所述兩個以上的冗余度機械臂中的各個機械臂之間分別對應(yīng)的第一連通權(quán)值;
第一確定單元,用于根據(jù)所述參考點軌跡信息的獲取狀態(tài)確定所述目標(biāo)冗余度機械臂的第二連通權(quán)值;
等式構(gòu)建單元,用于根據(jù)所述位置和速度信息、所述參考點軌跡信息、所述權(quán)重矩陣和所述第二連通權(quán)值構(gòu)建所述目標(biāo)冗余度機械臂對應(yīng)的雅可比矩陣等式。
8.根據(jù)權(quán)利要求7所述的協(xié)同控制裝置,其特征在于,所述權(quán)重矩陣構(gòu)建單元具體包括:
第一設(shè)置模塊,用于將所述目標(biāo)冗余度機械臂與所述目標(biāo)冗余度機械臂自身之間的第一連通權(quán)值設(shè)置為1;
第二設(shè)置模塊,用于將所述目標(biāo)冗余度機械臂與相鄰冗余度機械臂之間的第一連通權(quán)值設(shè)置為1;
第三設(shè)置模塊,用于將所述目標(biāo)冗余度機械臂與所述兩個以上的冗余度機械臂中的非自身且非相鄰冗余度機械臂之間的第一連通權(quán)值設(shè)置為0;
所述第一確定單元具體包括:
判斷模塊,用于判斷所述目標(biāo)冗余度機械臂是否獲取到所述參考點軌跡信息;
第四設(shè)置模塊,用于若所述目標(biāo)冗余度機械臂獲取到所述參考點軌跡信息,則將所述第二連通權(quán)值設(shè)置為1;
第五設(shè)置模塊,用于若所述目標(biāo)冗余度機械臂未獲取到所述參考點軌跡信息,則將所述第二連通權(quán)值設(shè)置為0;
所述等式構(gòu)建單元構(gòu)建的雅可比矩陣等式的通用表達(dá)式為:
其中,Ji表示目標(biāo)冗余度機械臂i的雅可比矩陣,為Ji的時間導(dǎo)數(shù);表示目標(biāo)冗余度機械臂i的關(guān)節(jié)速度向量,為的時間導(dǎo)數(shù);表示與目標(biāo)冗余度機械臂i之間的第一連通權(quán)值為1的機械臂的編號;Aij表示目標(biāo)冗余度機械臂i的權(quán)重矩陣,所述權(quán)重矩陣中的各個元素為目標(biāo)冗余度機械臂i與冗余度機械臂j之間的第一連通權(quán)值;ρi表示目標(biāo)冗余度機械臂i的第二連通權(quán)值;δi=ri-rirp為所述位置信息,ri為目標(biāo)冗余度機械臂i的末端執(zhí)行器位置,rirp為預(yù)設(shè)的目標(biāo)冗余度機械臂i的末端執(zhí)行器相對于所述目標(biāo)參考點的距離向量;為所述速度信息,為目標(biāo)冗余度機械臂i的末端執(zhí)行器速度,rd為所述目標(biāo)參考點的期望位置,為所述目標(biāo)參考點的期望速度;c0>0為控制算法收斂速度的參數(shù),其越大則代表收斂速度越快。
9.根據(jù)權(quán)利要求6至8中任一項所述的協(xié)同控制裝置,其特征在于,所述控制信號確定模塊具體包括:
第二確定單元,用于設(shè)置最小化性能指標(biāo)為重復(fù)運動,在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下確定二次型優(yōu)化結(jié)果;
轉(zhuǎn)換單元,用于將所述二次型優(yōu)化結(jié)果轉(zhuǎn)換為標(biāo)準(zhǔn)二次規(guī)劃;
求解單元,用于對所述標(biāo)準(zhǔn)二次規(guī)劃進(jìn)行求解,得到求解結(jié)果;
控制信號確定單元,用于根據(jù)所述求解結(jié)果確定所述目標(biāo)冗余度機械臂的控制信號。
10.根據(jù)權(quán)利要求8所述的協(xié)同控制裝置,其特征在于,所述控制信號確定模塊采用的二次型優(yōu)化為:
在第一約束條件下使得第一式子最小化;
所述第一約束條件包括:
雅可比矩陣等式約束條件
關(guān)節(jié)角度極限約束條件
關(guān)節(jié)速度極限約束條件
關(guān)節(jié)加速度極限約束條件
所述第一式子為
其中,上標(biāo)T表示矩陣和向量的轉(zhuǎn)置;c1>0和c2>0是用來控制關(guān)節(jié)位移幅值和收斂率的設(shè)計參數(shù),θi(0)表示目標(biāo)冗余度機械臂i的初始關(guān)節(jié)角;表示目標(biāo)冗余度機械臂i的關(guān)節(jié)角上下限;表示目標(biāo)冗余度機械臂i的關(guān)節(jié)速度上下限,表示目標(biāo)冗余度機械臂i的關(guān)節(jié)加速度上下限。