CN115030750A

CN115030750A - 一种喷浆机械臂、控制系统及控制方法

Info

Publication number: CN115030750A
Application number: CN202210777652.0A
Authority: CN
Inventors: 郭刚; 杨会龙; 高晓成; 闫尚彬; 高胡林
Original assignee: China Coal Electrical Co ltd; China National Coal Group Corp; China Coal Shaanxi Yulin Energy and Chemical Co Ltd
Current assignee: China Coal Electrical Co ltd; China National Coal Group Corp; China Coal Shaanxi Yulin Energy and Chemical Co Ltd
Priority date: 2022-07-04
Filing date: 2022-07-04
Publication date: 2022-09-09

Abstract

本申请涉及工程机械技术领域，公开了一种喷浆机械臂、控制系统及控制方法，用于巷道用混凝土湿喷工程车，机械臂末端连接喷枪，包括依次连接的伸缩臂、大臂、小臂；伸缩臂一端安装于工程车车体；大臂一端与伸缩臂另一端通过第一回转关节连接；大臂另一端与小臂一端通过第二回转关节连接；喷枪通过第三回转关节与小臂连接；还包括分别设于大臂的第一倾角传感器和设于小臂的第二倾角传感器，第一倾角传感器的测量轴与第一回转关节的旋转轴平行且处于同一方向，第二倾角传感器的测量轴与第二回转关节的旋转轴平行且处于同一方向。本发明的机械臂结构能够实现个关节运动和喷头位置、姿态之间的最大程度解耦，结构简单、易于实现。

Description

一种喷浆机械臂、控制系统及控制方法

技术领域

本发明属于工程机械技术领域，尤其涉及一种喷浆机械臂、控制系统及控制方法。

背景技术

混凝土喷射作业在例如采矿业、交通运输隧道施工中应用非常普遍。传统的喷浆工艺往往是通过人工操作遥控器来控制机械臂运动，来完成喷枪在水平方向或者竖直方向的直线运动，并通过喷浆在机械臂进行水平运动和竖直运动的同时不停进行画圆运动，但是该方法对操作工人的技术要求太高，且控制精度过低，故需要实现喷枪在水平方向或竖直方向的自动运动。

根据喷浆机械手的喷浆轨迹，现有的技术方案一般将机械手的结构分为三部分：竖直运动机构、水平运动机构和喷枪画圆机构。大臂升高和伸缩臂调平构成喷浆机械手竖直运动轨迹，操作人员通过手柄控制大臂升高和伸缩臂调平的油缸活塞运动，使喷枪完成竖直运动；伸缩臂伸缩运动构成喷浆机械手水平运动轨迹；喷浆画圆机构构成喷浆机械手的喷浆画圆轨迹。传统结构的机械臂各关节运动与喷头位姿之间的关系存在耦合，导致操作困难，需同时调整几个关节才能完成位姿按预想路径的改变。

由于喷浆机械臂普遍采用液压驱动关节，阀控液压系统在对负载的位移、速度及力进行控制，液压系统常常采用精度较高的位置传感器测出位置。但在实际应用中，在比例阀控缸安装位置传感器会使得系统复杂，可靠性与效率降低。

发明内容

本发明提供了一种喷浆机械臂，以解决上述技术问题的至少一个技术问题。

本发明所采用的技术方案为：

本发明提供一种喷浆机械臂，用于巷道用混凝土湿喷工程车，所述机械臂末端连接喷枪，包括依次连接的伸缩臂、大臂、小臂；

所述伸缩臂一端安装于所述工程车车体；

所述大臂一端与所述伸缩臂另一端通过第一回转关节连接；

所述大臂另一端与所述小臂一端通过第二回转关节连接；

所述喷枪通过第三回转关节与所述小臂连接；

还包括分别设于所述大臂的第一倾角传感器和设于所述小臂的第二倾角传感器，所述第一倾角传感器的测量轴与所述第一回转关节的旋转轴平行且处于同一方向，所述第二倾角传感器的测量轴与所述第二回转关节的旋转轴平行且处于同一方向。

作为本发明的一种优选实施方式，所述第一回转关节、所述第二回转关节和所述第三回转关节的旋转轴均与所述伸缩臂延伸方向平行。

作为本发明的一种优选实施方式，所述喷枪和所述第三回转关节之间还设有摆动连杆和第四回转关节，所述摆动连杆一端连接所述第三回转关节，另一端连接所述第四回转关节，所述喷枪通过所述第四回转关节与所述摆动连杆连接，所述第四回转关节的旋转轴沿竖直方向。

作为本发明的一种优选实施方式，还包括安装于所述第三回转关节和所述喷枪之间连杆上的第三倾角传感器，所述第三倾角传感器的测量轴与所述第三回转关节的旋转轴平行且处于同一方向。

作为本发明的一种优选实施方式，所述伸缩臂与所述工程车车体之间设有第五回转关节，所述第五回转关节的旋转轴沿竖直方向。

本发明还提供一种喷浆机械臂控制系统，用于控制前述的喷浆机械臂，包括遥控器、控制器和执行机构；

所述遥控器用于向所述控制器发送机械臂末端在水平方向和竖直方向的运动速度；

所述控制器用于接收遥控器的运动速度数据、以及所述第一倾角传感器、第二倾角传感器、第三倾角传感器的测量值，并能够将运动速度数据转得到相应的目标关节角度，根据各个目标关节角度和相应的所述第一倾角传感器、第二倾角传感器和第三倾角传感器的测量值之间的误差及误差变换率输出控制信号；

所述执行机构用于根据控制器输出的控制信号驱动第一回转关节、第二回转关节和第三回转关节运动至目标关节角度。

本发明还提供一种喷浆机械臂的位姿控制方法，利用前述的喷浆机械臂控制系统，其特征在于，包括：

S1：建立巷道坐标系OXYZ，调整所述伸缩臂的延伸方向沿OX方向，其中，OX方向表示平行于巷道待喷面的方向；OY方向表示垂直于待喷面的方向；OZ方向表示垂直于地面的方向；

S2：确定控制器执行控制程序的周期t，并通过所述遥控器向所述控制器发送所述机械臂末端在OY方向上的运动速度v_y和在OZ方向的运动速度v_z；

S3：将所述机械臂末端的运动速度v_y、v_z转换成t时刻所述小臂末端相对于所述大臂前端在OY方向的位置和在OZ方向的位置；

S4：根据小臂末端相对于所述大臂前端在OY方向的位置和OZ方向的位置得到第一回转关节的目标转动角度α和第二回转关节的目标转动角度β；

S5：通过第一回转关节和第二回转关节的目标转动角度α、β分别与第一倾角传感器和第二倾角传感器的测量值jd_1′和jd_2′进行位置闭环控制，使大臂和小臂达到目标位置；

S6：根据所述第三倾角传感器的测量值jd_3’确定所述喷枪与待喷面的角度，调整所述第三回转关节使所述喷枪与待喷面垂直。

作为本发明的一种优选实施方式，在步骤S4中，解算所述第一回转关节的目标转动角度α和第二回转关节的目标转动角度β的公式为：

t时刻所述小臂末端相对所述大臂前端在OXYZ坐标系中的位置为：

Z_t＝Z₀+v_zt；

Y_t＝Y₀+v_yt；

式中：Z_t、Y_t分别表示t时刻所述小臂末端相对所述大臂前端在OXYZ坐标系中Y轴、Z轴上的距离，Z₀、Y₀为初始值，t表示时间；

由几何法可以得到：

d₁ sinα-d₂ sinβ＝Z_t；

d₁ cosα+d₂ cosβ＝Y_t；

式中：d₁为大臂的长度，d₂为小臂的长度；

当需要改变喷枪与待喷面的距离时，使v_z＝0，v_y≠0；当需要改变喷枪相对地面的高度时，v_z≠0，v_y＝0；

联立上式解之得：

当Z_t≥0时，

α＝cos^-1(s-D₁-D₂)/(2*d₁*d₂)-cos^-1(s-D₁+D₂)/(2*d₂*S)+cos^-1(Y_t/S)；

β＝cos^-1(s-D₁+D₂)/(2*d₂*S)-cos^-1(Y_t/S)；

当Z_t<0时，

α＝cos^-1(s-D₁-D₂)/(2*d₁*d₂)-cos^-1(s-D₁+D₂)/(2*d₂*S)-cos^-1(Y_t/S)；

β＝cos^-1(s-D₁+D₂)/(2*d₂*S)+cos^-1(Y_t/S)；

其中，D₁＝d₁ ²；D₂＝d₂ ²；s＝Z_t ²+Y_t ²；

作为本发明的一种优选实施方式，在步骤S5中，根据第一回转关节的目标转动角度α和第二回转关节的目标转动角度β解算所述第一倾角传感器和第二倾角传感器的目标角度jd₁和jd₂，通过所述第一倾角传感器和第二倾角传感器的目标角度jd₁和jd₂与当前测量值jd_1’和jd_2’之间的误差进行误差角度补偿以进行位置闭环控制；若所述第一倾角传感器和第二倾角传感器的当前测量值jd_1’和jd_2’等于目标角度jd₁和jd₂，则所述大臂、小臂到达目标位置；若所述第一倾角传感器和第二倾角传感器的当前测量值jd_1’和jd_2’不等于目标角度jd₁和jd₂，继续进行误差角度补偿，直至当前测量值jd_1’和jd_2’等于目标角度jd₁和jd₂。

由于采用了上述技术方案，本发明所取得的有益效果为：

1.本发明通过进行机械臂结构的改变，使得各关节运动最大程度实现解耦，操作更加简单明了，降低对操作者的技术要求。具体的，将伸缩臂安装于工程车车体上，伸缩臂进行前后伸缩在平行于巷道两侧待喷面的方向上运动，第一回转关节和第二回转关节，可通过液压回转驱动，使大臂和小臂分别绕第一回转关节和第二回转关节的旋转轴旋转，从而控制喷枪与待喷面的垂直距离和喷枪与地面之间的竖直高度，利用第三回转关节带动喷枪旋转至能够与待喷面垂直，防止喷浆浆料回弹。

可通过第一倾角传感器和第二倾角传感器测量无法解耦的大臂与小臂角度，具体的，根据第一回转关节和第二回转关节在车体坐标系中的相对位置求得相应角度，与第一倾角传感器和第二倾角传感器测量值形成位置闭环，进而控制喷枪在水平方向或竖直方向运动，同时利用喷头上的第三倾角传感器实现了喷头姿态角度自动保持，从而实现了各关节运动与喷头位置、姿态之间的最大程度解耦，同时结构简单，易于实现。

2.整个喷浆机械臂的控制系统，通过遥控器向控制器发送机械臂末端在水平或竖直方向的运动速度，按周期执行控制程序，在控制程序中将机械臂末端的运动速度转换成目标位置，并且根据该位置解算得到相应的目标关节角度，与第一倾角传感器和第二倾角传感器的测量值之间的误差以及误差变换率经模糊控制得到控制量作为PWM的占空比，进而控制执行机构驱动关节运动，使机械臂末端到达目标位置。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本申请，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例提供的喷浆机械臂初始位置的结构图；

图2为本发明实施例提供的喷浆机械臂作业位置的结构图；

图3a为本发明实施例提供的大臂第一位置的结构图；

图3b为本发明实施例提供的大臂第二位置的结构图；

图3c为本发明实施例提供的大臂第三位置的结构图；

图3d为本发明实施例提供的大臂第四位置的结构图；

图4为本发明实施例提供的喷浆机械臂的位姿控制方法的流程示意图。

其中，

1-机械臂；11-伸缩臂；12-大臂；13-小臂；14-喷枪；

2-第一倾角传感器；3-第二倾角传感器；4-第三倾角传感器；

5-第一回转关节；6-第二回转关节；7-第三回转关节；8-第四回转关节。

具体实施方式

为了更清楚的阐释本申请的整体构思，下面结合说明书附图以示例的方式进行详细说明。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

另外，在本发明的描述中，需要理解的是，术语“顶”、“底”、“内”、“外”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是通信；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。

本发明提供一种喷浆机械臂，用于巷道用混凝土湿喷工程车，机械臂1末端连接喷枪，包括依次连接的伸缩臂11、大臂12、小臂12。伸缩臂11一端安装于所述工程车车体，使用过程中，伸缩臂11的延伸方向与巷道待喷面平行，进行前后伸缩运动。大臂12一端与伸缩臂11另一端通过第一回转关节5连接，大臂12另一端与小臂13一端通过第二回转关节6连接，喷枪14通过第三回转关节7与小臂13连接。

在一个实施方式中，第一回转关节5、第二回转关节6和第三回转关节7的旋转轴均与伸缩臂11延伸方向平行。假设朝向待喷面为末端，背离待喷面为前端。大臂12和小臂13一般沿“之”字形运动，大臂12末端能够绕伸缩臂11的方向旋转，使得大臂12末端能够朝向待喷面。小臂13前端在第二回转关节6驱动下，使得小臂13末端到达目标喷浆位置，从而控制喷枪14与待喷面的垂直距离和喷枪14与地面之间的竖直高度，然后通过第三回转关节7调整喷枪14垂直于待喷面，防止喷浆浆料回弹率高的问题。即在巷道坐标系中，通过微调控制喷枪14与待喷面的垂直距离和喷枪14与地面之间的竖直高度，然后沿前后运动完成一次前后移动，完成某一区域内喷浆作业，如此反复循环完成待喷面的作业。

机械臂1还包括分别设于大臂12的第一倾角传感器2和设于小臂13的第二倾角传感器3，第一倾角传感器2的测量轴与第一回转关节5的旋转轴平行且处于同一方向，第二倾角传感器3的测量轴与第二回转关节6的旋转轴平行且处于同一方向。第一倾角传感器2和第二倾角传感器3的测量值即为第一回转关节5、第二回转关节6的转动角度即大臂12、小臂13与初始位置的夹角。

在一个实施方式，喷枪14和第三回转关节7之间还设有摆动连杆和第四回转关节8，摆动连杆一端连接第三回转关节7，另一端连接第四回转关节8，喷枪14通过第四回转关节8与摆动连杆连接，第四回转关节8的旋转轴沿竖直方向。第四回转关节8能够相对于巷道待喷面进行前后摆动，用于调整喷枪14垂直于待喷面，或者沿车体运动方向向前喷射。

在一个实施方式，喷浆机械臂1还包括安装于第三回转关节7和喷枪14之间连杆上的第三倾角传感器4，第三倾角传感器4的测量轴与第三回转关节7的旋转轴平行且处于同一方向。利用第三倾角传感器4测量喷枪14与待喷面之间的角度，保证喷枪14能够转动至垂直于待喷面，实现喷枪14姿态角度的自动保持。

在一个实施方式，伸缩臂11与工程车车体之间设有第五回转关节，第五回转关节的旋转轴沿竖直方向。第五回转关节能够驱动伸缩臂11旋转至与待喷面平行。当然可以理解的是，在不设置第五回转关节的情况下，也可通过控制车体的运动，实现伸缩臂11与待喷面垂直。

在一个实施例中，本发明提供一种喷浆机械臂1的位姿控制方法，利用前述的喷浆机械臂控制系统，其特征在于，包括：

S1：建立巷道坐标系OXYZ，调整所述伸缩臂11的延伸方向沿OX方向，其中，OX方向表示平行于巷道待喷面的方向；OY方向表示垂直于待喷面的方向；OZ方向表示垂直于地面的方向；

S2：确定控制器执行控制程序的周期t，并通过所述遥控器向所述控制器发送所述机械臂1末端在OY方向上的运动速度v_y和在OZ方向的运动速度v_z；

S3：将所述机械臂1末端的运动速度v_y、v_z转换成t时刻所述小臂13末端相对于所述大臂12前端在OY方向的位置和OZ方向的位置；

S4：根据小臂13末端相对于所述大臂12前端在OY方向的位置和OZ方向的位置得到第一回转关节5的目标转动角度α和第二回转关节6的目标转动角度β；

S5：通过第一回转关节5和第二回转关节6的目标转动角度α、β分别与第一倾角传感器2和第二倾角传感器3的测量值jd_1’和jd_2’进行位置闭环控制，使大臂12和小臂13达到目标位置；

S6：根据所述第三倾角传感器4的测量值jd_3’确定所述喷枪13与待喷面的角度，调整所述第三回转关节7使所述喷枪13与待喷面垂直。

作为本发明的一种优选实施方式，在步骤S3中，解算所述第一回转关节5的目标转动角度α和第二回转关节6的目标转动角度β的公式为：

t时刻所述小臂13末端相对所述大臂12前端在OXYZ坐标系中的位置为：

Z_t＝Z₀+v_zt；

Y_t＝Y₀+v_yt；

式中：Z_t、Y_t分别表示t时刻所述小臂13末端相对所述大臂12前端在OXYZ坐标系中Y轴、Z轴上的距离，Z₀、Y₀为初始值，t表示时间；

由几何法可以得到：

d₁ sinα-d₂ sinβ＝Z_t；

d₁ cosα+d₂ cosβ＝Y_t；

式中：d₁为大臂12的长度，d₂为小臂13的长度；

当需要改变喷枪13与待喷面的距离时，使v_z＝0，v_y≠0；当需要改变喷枪13相对地面的高度时，v_z≠0，v_y＝0；

联立上式解之得：

当Z_t≥0时，

β＝cos^-1(s-D₁+D₂)/(2*d₂*S)-cos^-1(Y_t/S)；

当Z_t<0时，

β＝cos^-1(s-D₁+D₂)/(2*d₂*S)+cos^-1(Y_t/S)；

其中，D₁＝d₁ ²；D₂＝d₂ ²；s＝Z_t ²+Y_t ²；

作为本发明的一种优选实施方式，在步骤S5中，根据第一回转关节5的目标转动角度α和第二回转关节6的目标转动角度β解算所述第一倾角传感器2和第二倾角传感器3的目标角度jd₁和jd₂，通过所述第一倾角传感器2和第二倾角传感器3的目标角度jd₁和jd₂与当前测量值jd_1’和jd_2’之间的误差进行误差角度补偿以进行位置闭环控制；若所述第一倾角传感器2和第二倾角传感器3的当前测量值jd_1’和jd_2’等于目标角度jd₁和jd₂，则所述大臂12、小臂13到达目标位置；若所述第一倾角传感器2和第二倾角传感器3的当前测量值jd_1’和jd_2’不等于目标角度jd₁和jd₂，继续进行误差角度补偿，直至当前测量值jd_1’和jd_2′等于目标角度jd₁和jd₂。

将求解得到的目标转动角度α、β转换成其所对应的第一倾角传感器2和第二倾角传感器3测量的目标角度jd₁和jd₂的公式为：

当-90°≤α<270°，jd₁＝90°-α；当270°≤α<360°，jd₁＝450°-α；

当-360°≤α<-270°，jd₁＝-270°-α；

当-90°≤β<270°，jd₂＝90°-β；当270°≤β<360°，jd₂＝450°-β；

当-360°≤β<-270°，jd₂＝-270°-β。

以大臂12为例，对于α，当α>0时，图3a中，0°≤α<90°，jd₁＝90°-α，此时0°<jd₁≤90°；

图3b中，90°≤α<180°，jd₁＝90°-α，此时-90°<jd₁≤0°；

图3c中，180°≤α<270°，jd₁＝90°-α，此时-180°<jd₁≤-90°；

图3d中，270°≤α<360°，jd₁＝450°-α，此时90°<jd₁≤180°。

本发明中未述及的地方采用或借鉴已有技术即可实现。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。

以上所述仅为本发明的实施例而已，并不用于限制本发明。对于本领域技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims

1.一种喷浆机械臂，用于巷道用混凝土湿喷工程车，所述机械臂末端连接喷枪，其特征在于，包括依次连接的伸缩臂、大臂、小臂；

所述伸缩臂一端安装于所述工程车车体；

所述大臂一端与所述伸缩臂另一端通过第一回转关节连接；

所述大臂另一端与所述小臂一端通过第二回转关节连接；

所述喷枪通过第三回转关节与所述小臂连接；

2.根据权利要求1所述的一种喷浆机械臂，其特征在于，

所述第一回转关节、所述第二回转关节和所述第三回转关节的旋转轴均与所述伸缩臂延伸方向平行。

3.根据权利要求2所述的一种喷浆机械臂，其特征在于，

所述喷枪和所述第三回转关节之间还设有摆动连杆和第四回转关节，所述摆动连杆一端连接所述第三回转关节，另一端连接所述第四回转关节，所述喷枪通过所述第四回转关节与所述摆动连杆连接，所述第四回转关节的旋转轴沿竖直方向。

4.根据权利要求1所述的一种喷浆机械臂，其特征在于，

还包括安装于所述第三回转关节和所述喷枪之间连杆上的第三倾角传感器，所述第三倾角传感器的测量轴与所述第三回转关节的旋转轴平行且处于同一方向。

5.根据权利要求1所述的一种喷浆机械臂，其特征在于，

所述伸缩臂与所述工程车车体之间设有第五回转关节，所述第五回转关节的旋转轴沿竖直方向。

6.一种喷浆机械臂控制系统，用于控制如权利要求1-5任一项所述的喷浆机械臂，其特征在于，包括遥控器、控制器和执行机构；

7.一种喷浆机械臂的位姿控制方法，利用如权利要求6所述的喷浆机械臂控制系统，其特征在于，包括：

S3：将所述机械臂末端的运动速度v_y、v_z转换成t时刻所述小臂末端相对于所述大臂前端在OY方向的位置和OZ方向的位置；

S5：通过第一回转关节和第二回转关节的目标转动角度α、β分别与第一倾角传感器和第二倾角传感器的测量值jd₁’和jd₂’进行位置闭环控制，使大臂和小臂达到目标位置；

S6：根据所述第三倾角传感器的测量值jd₃’确定所述喷枪与待喷面的角度，调整所述第三回转关节使所述喷枪与待喷面垂直。

8.根据权利要求7所述的一种喷浆机械臂的位姿控制方法，其特征在于，在步骤S3中，解算所述第一回转关节的目标转动角度α和第二回转关节的目标转动角度β的公式为：

Z_t＝Z₀+v_zt；

Y_t＝Y₀+v_yt；

由几何法可以得到：

d₁sinα-d₂sinβ＝Z_t；

d₁cosα+d₂cosβ＝Y_t；

式中：d₁为大臂的长度，d₂为小臂的长度；

联立上式解之得：

当Z_t≥0时，

β＝cos^-1(s-D₁+D₂)/(2*d₂*S)-cos^-1(Y_t/S)；

当Z_t<0时，

β＝cos^-1(s-D₁+D₂)/(2*d₂*S)+cos^-1(Y_t/S)；

其中，D₁＝d₁ ²；D₂＝d₂ ²；s＝Z_t ²+Y_t ²；

9.根据权利要求7所述的一种喷浆机械臂的位姿控制方法，其特征在于，

在步骤S5中，根据第一回转关节的目标转动角度α和第二回转关节的目标转动角度β解算所述第一倾角传感器和第二倾角传感器的目标角度jd₁和jd₂，通过所述第一倾角传感器和第二倾角传感器的目标角度jd₁和jd₂与当前测量值jd₁’和jd₂’之间的误差进行误差角度补偿以进行位置闭环控制；若所述第一倾角传感器和第二倾角传感器的当前测量值jd₁’和jd₂’等于目标角度jd₁和jd₂，则所述大臂、小臂到达目标位置；若所述第一倾角传感器和第二倾角传感器的当前测量值jd₁’和jd₂’不等于目标角度jd₁和jd₂，继续进行误差角度补偿，直至当前测量值jd₁’和jd₂’等于目标角度jd₁和jd₂。