智能臂空間運(yùn)動(dòng)控制方法及裝置制造方法
【專利摘要】本發(fā)明公開了一種智能臂空間運(yùn)動(dòng)控制方法及裝置，該方法包括：接收各角度傳感器感測及發(fā)送的智能臂處于當(dāng)前位置時(shí)各節(jié)臂架與水平面之間的夾角，并接收遙控器發(fā)送的運(yùn)動(dòng)指令；計(jì)算智能臂從當(dāng)前位置運(yùn)動(dòng)到目標(biāo)位置時(shí)的坐標(biāo)改變量；并確定約束條件，再根據(jù)預(yù)設(shè)的優(yōu)化算法，結(jié)合各臂架動(dòng)作能力和/或遙控器的手柄開度變化確定智能臂的相鄰兩節(jié)臂架之間的夾角在智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差；根據(jù)轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度控制智能臂轉(zhuǎn)臺(tái)的運(yùn)動(dòng)，以及根據(jù)相鄰兩節(jié)臂架之間夾角的角度差控制智能臂臂架的運(yùn)動(dòng)。本發(fā)明提供的控制方法能避免智能臂架空間運(yùn)動(dòng)時(shí)臂架油缸出現(xiàn)速度突變和/或避免智能臂架空間運(yùn)動(dòng)時(shí)大油缸急速換向造成的臂架末端抖動(dòng)的現(xiàn)象。
【專利說明】智能臂空間運(yùn)動(dòng)控制方法及裝置
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及智能臂控制【技術(shù)領(lǐng)域】，特別涉及智能臂空間運(yùn)動(dòng)控制方法及裝置。
【背景技術(shù)】
[0002]目前，對于混凝土泵車臂架的控制，基本上采取單個(gè)遙控手柄控制單個(gè)臂架或者轉(zhuǎn)臺(tái)。一般來說，操作者將混凝土從泵車料斗中輸送到樓盤的隨意位置，然后由工人將混凝土均勻攤鋪開，雖然相比人工運(yùn)輸混凝土節(jié)省了大量人力物力，但還是存在著繁重的勞動(dòng)，一方面泵車操作者需注意泵車臂架姿態(tài)，考慮操作的正確性，誤操作易導(dǎo)致臂架撞上障礙物；若動(dòng)作臂架幅度過大，對臂架末端軟管下的攙扶者也會(huì)帶來傷害。另一方面工人攤鋪混凝土也很麻煩。在此背景下，泵車操作者希望有一種簡單的操作方式來實(shí)現(xiàn)混凝土的典型工況均勻布料，泵車制造者也希望制造出更高性能的混凝土泵車，為其擴(kuò)大市場占有率，由此，智能臂架技術(shù)應(yīng)運(yùn)而生。
[0003]混凝土泵車臂架(如智能臂)在三維空間的坐標(biāo)(X Y Z)可以如圖1所示，該三維空間的確定方式可以如下:在泵車臂架可達(dá)的空間內(nèi)，以泵車轉(zhuǎn)臺(tái)與第I節(jié)臂架始端鉸接處作為坐標(biāo)原點(diǎn)，以車身的料斗方向?yàn)閄正半軸，以垂直于車身的方向?yàn)閥軸正半軸，根據(jù)笛卡爾坐標(biāo)系右手定則，可確定z軸；h表示垂足。當(dāng)然，根據(jù)實(shí)際需要可以以其他的點(diǎn)為坐標(biāo)原點(diǎn)，確定其他的方向?yàn)閄正半軸，并相應(yīng)確定I軸及Z軸。
[0004]對于混凝土泵車臂架末端空間位置在水平面的投影h的獲取，采用三維坐標(biāo)計(jì)算方式，即:
[0005]
【權(quán)利要求】
1.一種智能臂空間運(yùn)動(dòng)控制方法，應(yīng)用于具有η節(jié)臂架的智能臂，其中所述η大于等于2，其特征在于，所述智能臂空間運(yùn)動(dòng)控制方法包括: 接收各角度傳感器感測及發(fā)送的所述智能臂處于當(dāng)前位置時(shí)各節(jié)臂架與水平面之間的夾角，并接收用于控制所述智能臂運(yùn)動(dòng)的遙控器發(fā)送的運(yùn)動(dòng)指令； 根據(jù)所述各節(jié)臂架與水平面之間的夾角、所述智能臂各節(jié)臂架的長度及所述運(yùn)動(dòng)指令計(jì)算所述智能臂從當(dāng)前位置運(yùn)動(dòng)到目標(biāo)位置時(shí)的坐標(biāo)改變量；并根據(jù)所述坐標(biāo)改變量確定約束條件，再根據(jù)預(yù)設(shè)的優(yōu)化算法，同時(shí)結(jié)合各臂架動(dòng)作能力和/或所述遙控器的手柄開度變化確定所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差； 根據(jù)所述運(yùn)動(dòng)指令確定轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度，并根據(jù)所述轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度控制所述智能臂轉(zhuǎn)臺(tái)的運(yùn)動(dòng)，以及根據(jù)所述相鄰兩節(jié)臂架之間夾角的角度差控制所述智能臂臂架的運(yùn)動(dòng)。
2.根據(jù)權(quán)利要求1所述智能臂空間運(yùn)動(dòng)控制方法，其特征在于，所述優(yōu)化算法以使所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差的平方和最小為優(yōu)化目標(biāo)，所述優(yōu)化目標(biāo)表示為:
3.根據(jù)權(quán)利要求2所述智能臂空間運(yùn)動(dòng)控制方法，其特征在于，所述“結(jié)合各臂架動(dòng)作能力確定所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差”的步驟包括: 根據(jù)預(yù)設(shè)的所述智能臂各節(jié)臂架之間夾角的最大值及最小值，判斷所述智能臂在目標(biāo)位置時(shí)各節(jié)臂架之間夾角是否在對應(yīng)的所述最小值與所述最大值之間的范圍內(nèi)； 將臂架之間夾角處于對應(yīng)的所述最小值與所述最大值之間的范圍外的臂架對應(yīng)的系數(shù)Ki確定為零，并重新確定所述智能臂中其他節(jié)臂與其相鄰臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差。
4.根據(jù)權(quán)利要求2述智能臂空間運(yùn)動(dòng)控制方法，其特征在于，所述“結(jié)合各臂架動(dòng)作能力確定所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差”的步驟包括還包括: 根據(jù)預(yù)設(shè)的轉(zhuǎn)臺(tái)角度的最大值及最小值，判斷所述轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度是否在所述轉(zhuǎn)臺(tái)角度的最小值與所述轉(zhuǎn)臺(tái)角度的最大值之間的范圍內(nèi)； 在所述轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度位于所述轉(zhuǎn)臺(tái)角度的最小值與所述轉(zhuǎn)臺(tái)角度的最大值之外的范圍時(shí)，停止所述智能臂各節(jié)臂架的動(dòng)作。
5.根據(jù)權(quán)利要求3所述智能臂空間運(yùn)動(dòng)控制方法，其特征在于，所述“結(jié)合各臂架動(dòng)作能力確定所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差”的步驟包括還包括:將臂架之間夾角處于對應(yīng)的所述智能臂各節(jié)臂架之間夾角的最小值與最大值之間的范圍內(nèi)且位于預(yù)設(shè)的增速范圍內(nèi)的臂架對應(yīng)的系數(shù)Ki減小，將臂架之間夾角處于對應(yīng)的所述最小值與所述最大值之間的范圍內(nèi)且位于預(yù)設(shè)的減速范圍內(nèi)的臂架對應(yīng)的系數(shù)Ki增大。
6.根據(jù)權(quán)利要求2-5中任一項(xiàng)所述智能臂空間運(yùn)動(dòng)控制方法，其特征在于，所述“結(jié)合所述遙控器的手柄開度變化確定所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差”的步驟包括: 比較預(yù)存的手柄開度值與所述遙控器的手柄的當(dāng)前開度值； 在所述預(yù)存的手柄開度值與所述遙控器的手柄的當(dāng)前開度值的差值大于預(yù)設(shè)閾值時(shí)，將大臂對應(yīng)的系數(shù)Ki清零，以鎖定所述大臂，以及在間隔預(yù)設(shè)時(shí)間后，控制所述大臂對應(yīng)的系數(shù)Ki由大變小； 將所述手柄開度值更新為所述遙控器的手柄的當(dāng)前開度值。
7.一種智能臂空間運(yùn)動(dòng)控制裝置，應(yīng)用于具有η節(jié)臂架的智能臂，其中所述η大于等于2，其特征在于，所述智能臂空間運(yùn)動(dòng)控制裝置包括: 接收單元(61)，用于接收各角度傳感器感測及發(fā)送的所述智能臂處于當(dāng)前位置時(shí)各節(jié)臂架與水平面之間的夾角，并接收用于控制所述智能臂運(yùn)動(dòng)的遙控器發(fā)送的運(yùn)動(dòng)指令； 夾角差確定單元(63)，根據(jù)所述各節(jié)臂架與水平面之間的夾角、所述智能臂各節(jié)臂架的長度及所述運(yùn)動(dòng)指令 計(jì)算所述智能臂從當(dāng)前位置運(yùn)動(dòng)到目標(biāo)位置時(shí)的坐標(biāo)改變量；并根據(jù)所述坐標(biāo)改變量確定約束條件，再根據(jù)預(yù)設(shè)的優(yōu)化算法，同時(shí)結(jié)合各臂架動(dòng)作能力和/或所述遙控器的手柄開度變化確定所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差； 運(yùn)動(dòng)控制單元(65)，用于根據(jù)所述運(yùn)動(dòng)指令確定轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度，并根據(jù)所述轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度控制所述智能臂轉(zhuǎn)臺(tái)的運(yùn)動(dòng)，以及根據(jù)所述相鄰兩節(jié)臂架之間夾角的角度差控制所述智能臂臂架的運(yùn)動(dòng)。
8.根據(jù)權(quán)利要求7所述智能臂空間運(yùn)動(dòng)控制裝置，其特征在于，所述夾角差確定單元(63)包括: 優(yōu)化子單元，用于以使所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差的平方和最小為優(yōu)化目標(biāo)，所述優(yōu)化目標(biāo)表示為:
9.根據(jù)權(quán)利要求8所述智能臂空間運(yùn)動(dòng)控制裝置，其特征在于，所述優(yōu)化子單元包括: 第一處理模塊，用于根據(jù)預(yù)設(shè)的所述智能臂各節(jié)臂架之間夾角的最大值及最小值，判斷所述智能臂在目標(biāo)位置時(shí)各節(jié)臂架之間夾角是否在對應(yīng)的所述最小值與所述最大值之間的范圍內(nèi)，并將臂架之間夾角處于對應(yīng)的所述最小值與所述最大值之間的范圍外的臂架對應(yīng)的系數(shù)Ki確定為零，且重新確定所述智能臂中其他節(jié)臂與其相鄰臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差，以及將臂架之間夾角處于對應(yīng)的所述智能臂各節(jié)臂架之間夾角的最小值與最大值之間的范圍內(nèi)且位于預(yù)設(shè)的增速范圍內(nèi)的臂架對應(yīng)的系數(shù)Ki減小，將臂架之間夾角處于對應(yīng)的所述最小值與所述最大值之間的范圍內(nèi)且位于預(yù)設(shè)的減速范圍內(nèi)的臂架對應(yīng)的系數(shù)Ki增大；還用于根據(jù)預(yù)設(shè)的轉(zhuǎn)臺(tái)角度的最大值及最小值，判斷所述轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度是否在所述轉(zhuǎn)臺(tái)角度的最小值與所述轉(zhuǎn)臺(tái)角度的最大值之間的范圍內(nèi)；在所述轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度位于所述轉(zhuǎn)臺(tái)角度的最小值與所述轉(zhuǎn)臺(tái)角度的最大值之外的范圍時(shí)，將所述智能臂的相鄰兩節(jié)臂架之間的夾角在所述智能臂處于當(dāng)前位置與目標(biāo)位置時(shí)的角度差確定為零，以停止所述智能臂各節(jié)臂架的動(dòng)作。
10.根據(jù)權(quán)利要求9所述智能臂空間運(yùn)動(dòng)控制裝置，其特征在于，所述優(yōu)化子單元包括: 第二處理模塊，用于比較預(yù)存的手柄開度值與所述遙控器的手柄的當(dāng)前開度值，在所述預(yù)存的手柄開度值與所述遙控器的手柄的當(dāng)前開度值的差值大于預(yù)設(shè)閾值時(shí)，將大臂對應(yīng)的系數(shù)1清零，以鎖定所述大臂，以及在間隔預(yù)設(shè)時(shí)間后，控制所述大臂對應(yīng)的系數(shù)Ki由大變小，以及將所 述手柄開度值更新為所述遙控器的手柄的當(dāng)前開度值。
【文檔編號】E04G21/04GK103914082SQ201410117938
【公開日】2014年7月9日 申請日期:2014年3月27日 優(yōu)先權(quán)日:2014年3月27日
【發(fā)明者】王建明, 曾啟, 賈劍峰 申請人:三一汽車制造有限公司