多節(jié)臂架的姿態(tài)檢測(cè)系統(tǒng)和方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于姿態(tài)控制與檢測(cè)技術(shù)領(lǐng)域，具體涉及一種用于作業(yè)設(shè)備上的多節(jié)臂架 的姿態(tài)檢測(cè)系統(tǒng)及方法。
【背景技術(shù)】
[0002] 作業(yè)設(shè)備的本體端和作業(yè)端之間通常設(shè)置有臂架，特別是在本體端與作業(yè)區(qū)域之 間的空間距離較遠(yuǎn)時(shí)，還需要設(shè)置多節(jié)臂架，以便于遠(yuǎn)程操作控制。多節(jié)臂架通常是由多個(gè) 單臂以可相對(duì)轉(zhuǎn)動(dòng)的方式鉸接連接而成，通過(guò)控制各機(jī)械臂的姿態(tài)和方向?qū)崿F(xiàn)對(duì)末端的作 業(yè)端的位置控制。
[0003] 作業(yè)設(shè)備工作時(shí)，正常工況下通常有靜止工作狀態(tài)和平滑移動(dòng)狀態(tài)兩種，部分設(shè) 備可能還有其他自由度的運(yùn)動(dòng)狀態(tài)。以常見(jiàn)的混凝土泵送車為例，靜止工作狀態(tài)是指在開(kāi) 始泵送混凝土前對(duì)臂架進(jìn)行移動(dòng)，擺好臂架姿態(tài)，開(kāi)始泵送混凝土后臂架始終保持不變，如 在樓房裝修時(shí)，用混凝土泵車將混凝土從1樓泵送到4樓的窗口，則屬于靜止工作狀態(tài)。平 滑移動(dòng)狀態(tài)是指泵送混凝土?xí)r，需要將臂架末端進(jìn)行平滑移動(dòng)，均勻地進(jìn)行澆筑，例如混凝 土泵車澆筑混凝土路面時(shí)，則屬于平滑移動(dòng)狀態(tài)。
[0004] 對(duì)于多節(jié)臂架的上述工況，會(huì)存在比較大的缺陷，比如靜止工況下產(chǎn)生不規(guī)律的 振動(dòng)可能會(huì)帶來(lái)共振，平移狀態(tài)下臂架會(huì)由于慣性作用發(fā)生擺動(dòng)，這些都會(huì)對(duì)作業(yè)設(shè)備整 體帶來(lái)安全風(fēng)險(xiǎn)。
[0005] 為克服上述問(wèn)題，現(xiàn)有技術(shù)中一般通過(guò)減振的方式避免可能產(chǎn)生的安全風(fēng)險(xiǎn)。減 振方式通常可以分為被動(dòng)減振和主動(dòng)減振，其中主動(dòng)減振是目前一種常見(jiàn)的減振方式，主 動(dòng)減振的關(guān)鍵或核心是準(zhǔn)確檢測(cè)出臂架的位置，以此為依據(jù)進(jìn)行位置控制從而實(shí)現(xiàn)減振的 目的。
[0006] 現(xiàn)有方案中，位置檢測(cè)通常通過(guò)在臂架上設(shè)置位置檢測(cè)系統(tǒng)實(shí)現(xiàn)，即通過(guò)在臂架 上設(shè)置加速度傳感器和傾角傳感器，通過(guò)檢測(cè)各臂架之間的夾角和臂架末端的加速度，從 而反饋給控制系統(tǒng)以對(duì)臂架進(jìn)行控制，減小振動(dòng)的影響。
[0007] 但是，上述方案存在比較明顯的缺陷，首先是因?yàn)槎喙?jié)臂架的存在，其多次累積的 誤差會(huì)導(dǎo)致最后檢測(cè)結(jié)果誤差較大；另外，由于臂架在受力時(shí)會(huì)產(chǎn)生形變，多節(jié)臂架的形 變量使得上述臂架位置會(huì)存在很大的誤差，而上述檢測(cè)系統(tǒng)無(wú)法實(shí)現(xiàn)對(duì)單節(jié)臂架的形變檢 測(cè)，從而使得臂架整體的位置檢測(cè)精度大大降低。

【發(fā)明內(nèi)容】

[0008] 針對(duì)現(xiàn)有技術(shù)的以上缺陷或改進(jìn)需求，本發(fā)明提供了一種多臂架的姿態(tài)檢測(cè)系統(tǒng) 及方法，通過(guò)在多節(jié)臂架的每節(jié)臂架上設(shè)置傳感器模塊并分別檢測(cè)每節(jié)臂架的形變量，并 進(jìn)而實(shí)現(xiàn)對(duì)每節(jié)臂架的形變量檢測(cè)并進(jìn)而實(shí)現(xiàn)對(duì)臂架空間位置的不斷修正，完成對(duì)臂架姿 態(tài)的準(zhǔn)確檢測(cè)。
[0009] 為實(shí)現(xiàn)上述目的，按照本發(fā)明一個(gè)方面，提供一種多臂架姿態(tài)檢測(cè)系統(tǒng)，用于實(shí)現(xiàn) 對(duì)作業(yè)設(shè)備上依次連接的多臂架的空間位置準(zhǔn)確檢測(cè)，其特征在于，該姿態(tài)檢測(cè)系統(tǒng)包括 多個(gè)傳感器模塊，其中每節(jié)臂架兩端均設(shè)置有一個(gè)所述傳感器模塊，該傳感器模塊中具有 三軸加速度傳感器，以用于分別測(cè)量對(duì)應(yīng)臂架上相應(yīng)端部的Χ、γ和Z方向上的加速度值，根 據(jù)該加速度值可獲得各臂架的彎曲角度以及兩相鄰臂架之間的夾角，進(jìn)而利用上述彎曲角 度以及夾角可得到臂架因變形產(chǎn)生的偏移，根據(jù)所述偏移即可獲得對(duì)各臂架節(jié)點(diǎn)空間位置 坐標(biāo)，從而即可實(shí)現(xiàn)對(duì)臂架姿態(tài)的準(zhǔn)確測(cè)定。
[0010] 作為本發(fā)明的改進(jìn)，所述根據(jù)偏移獲得各臂架節(jié)點(diǎn)空間位置坐標(biāo)具體計(jì)算過(guò)程
【主權(quán)項(xiàng)】
1. 一種多節(jié)臂架姿態(tài)檢測(cè)系統(tǒng)，用于實(shí)現(xiàn)對(duì)作業(yè)設(shè)備上依次連接的多臂架的空間位置 準(zhǔn)確檢測(cè)，其特征在于，該姿態(tài)檢測(cè)系統(tǒng)包括多個(gè)傳感器模塊，其中每節(jié)臂架兩端均設(shè)置有 一個(gè)所述傳感器模塊，該傳感器模塊中具有三軸加速度傳感器，以用于分別測(cè)量對(duì)應(yīng)臂架 上相應(yīng)端部的X、Y和Z方向上的加速度值，根據(jù)該加速度值可獲得各臂架的彎曲角度以及 兩相鄰臂架之間的夾角，進(jìn)而利用上述彎曲角度以及夾角可得到臂架因變形產(chǎn)生的偏移， 根據(jù)所述偏移即可獲得對(duì)各臂架節(jié)點(diǎn)空間位置坐標(biāo)，從而即可實(shí)現(xiàn)對(duì)臂架姿態(tài)的準(zhǔn)確測(cè) 定。
2. 根據(jù)權(quán)利要求1所述的一種多節(jié)臂架姿態(tài)檢測(cè)系統(tǒng)，其中，所述根據(jù)偏移獲得各臂 架節(jié)點(diǎn)空間位置坐標(biāo)具體計(jì)算過(guò)程為：
式中，（Xs，Ys)為任一臂架n的起始坐標(biāo)（Xs，Ys)，（Xe，Ye)為該節(jié)臂架末端坐標(biāo)，L為 該任一臂架的長(zhǎng)度，k為偏移，0 "是該臂架n與臂架移動(dòng)平面上的X軸的夾角。
3. 根據(jù)權(quán)利要求2所述的一種多節(jié)臂架姿態(tài)檢測(cè)系統(tǒng)，其中，所述偏移k通過(guò)如下公式 計(jì)算得到：
式中，（X，bend(X))為曲線bend(x)上使bend' (x) =tan(abs(Ain[2n]))的點(diǎn)的坐 標(biāo)，曲線bend(x) =asinh(tan(x/2))/sin(x/2)，Ain[2n]為任一臂架n因變形而彎曲的彎 曲角度。
4. 根據(jù)權(quán)利要求3所述的一種多節(jié)臂架姿態(tài)檢測(cè)系統(tǒng)，其中，所述任一第n節(jié)臂架因變 形而彎曲的彎曲角度Ain[2n]通過(guò)如下過(guò)程計(jì)算得到： Ain[2n] = 0211-0211-! 式中，9 ；&為位于第n節(jié)臂架末端的傳感器的傾角值，9^1為位于該第n節(jié)臂架起始 端傳感器的傾角值。
5. 根據(jù)權(quán)利要求4所述的一種多節(jié)臂架姿態(tài)檢測(cè)系統(tǒng)，其中，所述任一傳感器的空間 傾斜角9i具體為： 9i=arctan2(ACCZi/ACCXi) -yi 式中，0為傳感器i的空間傾斜角，ACCYi為傳感器i檢測(cè)得到的Y軸加速度，ACCXi為傳感器i檢測(cè)得到的X軸加速度，Yi為傳感器的重力加速度偏移值。
6. -種多節(jié)臂架姿態(tài)檢測(cè)方法，用于實(shí)現(xiàn)對(duì)作業(yè)設(shè)備上依次連接的多臂架的空間位置 準(zhǔn)確檢測(cè)，其特征在于，該姿態(tài)檢測(cè)方法包括： (1) 通過(guò)各臂架首末兩端設(shè)置的傳感器模塊中的三軸加速度傳感器，分別測(cè)量對(duì)應(yīng)臂 架上相應(yīng)端部的X、Y和Z方向上的加速度值； (2) 根據(jù)該加速度值可獲得各臂架的彎曲角度以及兩相鄰臂架之間的夾角； (3) 利用上述彎曲角度以及夾角可得到臂架因變形產(chǎn)生的偏移； (4) 根據(jù)所述偏移即可獲得對(duì)各臂架節(jié)點(diǎn)空間位置坐標(biāo)，從而即可實(shí)現(xiàn)對(duì)臂架姿態(tài)的 準(zhǔn)確測(cè)定。
7. 根據(jù)權(quán)利要求6所述的一種多節(jié)臂架姿態(tài)檢測(cè)方法，其中，所述根據(jù)偏移獲得各臂 架節(jié)點(diǎn)空間位置坐標(biāo)具體計(jì)算過(guò)程為：
式中，（Xs，Ys)為任一臂架起始坐標(biāo)（Xs，Ys)，（Xe，Ye)為該節(jié)臂架末端坐標(biāo)，L為該任 一臂架的長(zhǎng)度，k為偏移，0 "是該臂架n與臂架移動(dòng)平面上的X軸的夾角。
8. 根據(jù)權(quán)利要求7所述的一種多節(jié)臂架姿態(tài)檢測(cè)方法，其中，所述偏移k通過(guò)如下公 式計(jì)算得到：
式中，（X，bend(X))為曲線bend(x)上使bend' (x) =tan(abs(Ain[2n]))的點(diǎn)的坐 標(biāo)，曲線bend(x) =asinh(tan(x/2))/sin(x/2)，Ain[2n]為任一臂架n因變形而彎曲的彎 曲角度。
9. 根據(jù)權(quán)利要求8所述的一種多節(jié)臂架姿態(tài)檢測(cè)方法，其中，所述任一第n節(jié)臂架因變 形而彎曲的彎曲角度Ain[2n]通過(guò)如下過(guò)程計(jì)算得到： Ain[2n] = 0211-0211-! 式中，9 ；&為位于第n節(jié)臂架末端的傳感器的傾角值，9^1為位于該第n節(jié)臂架起始 端傳感器的傾角值。
10. 根據(jù)權(quán)利要求9所述的一種多節(jié)臂架姿態(tài)檢測(cè)方法，其中，所述任一傳感器的空間 傾斜角9i具體為： 9i=arctan2(ACCZi/ACCXi) -yi 式中，0為傳感器i的空間傾斜角，ACCYi為傳感器i檢測(cè)得到的Y軸加速度，ACCXi為傳感器i檢測(cè)得到的X軸加速度，Yi為傳感器的重力加速度偏移值。
【專利摘要】本發(fā)明公開(kāi)了一種多節(jié)臂架姿態(tài)檢測(cè)方法，包括：(1)通過(guò)各臂架首末兩端設(shè)置的傳感器模塊中的三軸加速度傳感器，分別測(cè)量對(duì)應(yīng)臂架上相應(yīng)端部的X、Y和Z方向上的加速度值；(2)根據(jù)該加速度值可獲得各臂架的彎曲角度以及兩相鄰臂架之間的夾角；(3)利用上述彎曲角度以及夾角可得到臂架因變形產(chǎn)生的偏移，(4)根據(jù)所述偏移即可獲得對(duì)各臂架節(jié)點(diǎn)空間位置坐標(biāo)，從而即可實(shí)現(xiàn)對(duì)臂架姿態(tài)的準(zhǔn)確測(cè)定。本發(fā)明還公開(kāi)了相應(yīng)的姿態(tài)檢測(cè)系統(tǒng)。本發(fā)明可以實(shí)現(xiàn)對(duì)每節(jié)臂架的形變量檢測(cè)并進(jìn)而實(shí)現(xiàn)對(duì)臂架空間位置的不斷修正，完成對(duì)臂架姿態(tài)的準(zhǔn)確檢測(cè)。
【IPC分類】G01B21-00, G01B21-32
【公開(kāi)號(hào)】CN104776822
【申請(qǐng)?zhí)枴緾N201510177605
【發(fā)明人】游海濤, 尹棟, 楊兆萬(wàn)
【申請(qǐng)人】武漢地和智能有限公司, 武漢智異科技開(kāi)發(fā)有限公司
【公開(kāi)日】2015年7月15日
【申請(qǐng)日】2015年4月15日