本发明涉及一种装载机末端机具姿态自动控制方法及装载机，属于工程机械液压控制。
背景技术：
1、装载机是一种广泛用于公路、铁路、建筑、水电、港口、矿山等建设工程的土石方施工机械，它主要用于铲装土壤、砂石、石灰、煤炭等散状物料，也可对矿石、硬土等作轻度铲挖作业。随着市场的不断发展，精细化、多用性成了衡量装载机产品力的重要指标。在小中吨位机型中，装载机的前端机具一般具有可更换性，更换为铲斗、货叉等不同机具从而满足不同场合的作业需求，在以上作业场景中通常进行平动举升、下落等操作，而装载机是由液压系统驱动的空间多连杆机构，如若须精细控制装载机末端机具姿态，就需要进行复合作业，即同时控制动臂及摇臂油缸来保证末端姿态，这增加了驾驶员的操作难度，也为安全作业埋下了隐患。
2、公开号为cn117488892a的发明专利，提供了装载机铲斗姿态的控制方法、装置、电子设备和装载机，通过预设的铲斗角度映射关系确定装载机铲斗的连杆倾角，通过调整连杆倾角使得装载机铲斗保持水平。该发明虽然占用的资源较少，运算速度快，但是不具有通用性，而且预设的铲斗角度映射关系表是通过获取倾角传感器针对某机型某辆车进行试验测试，而没有考虑到因为加工、传感器安装以及角度测量等误差，极易导致产品在铲斗姿态控制出现一致性问题。
3、公开号为cn 115992537a的发明专利，提供了装载机属具平移举升控制方法及装载机，根据属具平移举升前的摇臂夹角和动臂夹角确定属具平移举升过程中动臂转动角度与摇臂转动角度的转动角度对应关系；实时检测摇臂夹角和动臂夹角，计算当前动臂转动角度，根据当前动臂转动角度和转动角度对应关系确定摇臂目标转动角度；控制器控制转斗联主阀使摇臂转动角度趋近摇臂目标转动角度。该发明是搜寻摇臂夹角与动臂转动角度在水平时的对应关系，同样是依靠插值表进行角度控制，在实际操作中因为期望角度往往差别较小，而液压系统为非线性系统，容易反复调节是系统进入不稳定状态，极易危害员安全，造成财产损失。
4、公开号为cn117364873a的发明专利，提供了装载机控制方法、装置、装载机及存储介质，获取动臂相对于水平方向的第一姿态角以及倾斜斗杆相对于水平方向的第二姿态角；基于第一姿态角、第二姿态角以及装载机的结构参数，得到动臂与铲斗液压推杆之间的第一关节角，以及铲斗与动臂之间的第二关节角；基于第一关节角和第二关节角，对动臂和铲斗进行控制。该发明计算过程复杂，对控制器计算性能有要求，且对液压工作部分进行角度控制，未考虑液压系统本身控制特性，容易导致过调节现象。
5、现有技术中铲斗姿态控制中使用动臂-摇臂角度插值表来达到平动控制的效果，该方法不能正向解决姿态控制的问题，不同吨位不同车型需要逐个采集动臂-摇臂角度才能找到动臂转动角度与摇臂转动角度的转动角度对应关系，且因为加工、装配以及传感器等误差，不能进行大批量生产应用。
6、现有技术中对摇臂油缸角度进行控制，而未考虑液压系统本身非线性特征，容易引起过调节的现象，导致油缸反复调节，造成系统失稳，货物掉落甚至整车倾覆，造成人员伤亡。
7、现有技术中未考虑因为制造误差引起传感器角度误差，未考虑对传感器进行极限位置标定。
技术实现思路
1、本发明的目的在于克服现有技术中的不足，提供一种装载机末端机具姿态自动控制方法及装载机，能够自动控制装载机末端机具姿态，辅助工作人员实现精准作业。为达到上述目的，本发明是采用下述技术方案实现的：
2、第一方面，本发明提供一种装载机末端机具姿态自动控制方法，包括：
3、响应于启动装载机末端机具姿态自动控制的指令信号，获取动臂手柄的信号和动臂角度传感器的信号；
4、响应于获取到动臂手柄的信号和动臂角度传感器的信号，获取动臂角度传感器采集的实时角度数据、摇臂角度传感器采集的实时角度数据；
5、基于获取到的实时角度数据，通过机构学推导计算装载机末端机具的尖端与水平面的实时夹角；
6、响应于实时夹角发生变化，通过机构学推导计算装载机末端机具在当前动臂油缸行程下保持水平所需的期望摇臂油缸行程；
7、基于期望摇臂油缸行程和摇臂油缸的直径计算所需流量；
8、对计算得到的所需流量进行反向推导插值，得到摇臂油缸运行的先导压力，基于所述先导压力输出摇臂油缸的电比例减压阀的pwm信号，通过所述摇臂油缸的电比例减压阀调整摇臂油缸运行以控制末端机具姿态。
9、结合第一方面，可选地，所述基于获取到的实时角度数据，通过机构学推导计算装载机末端机具的尖端与水平面的实时夹角，包括：
10、使用d-h法为装载机工作机构建立坐标系，包括车架与动臂上端铰接点的参考坐标系axy、动臂上部凸出部与摇臂油缸缸筒底部铰接点的第零坐标系cx0y0、动臂上部凸出部与摇臂油缸缸筒底部铰接点的第一坐标系cx1y1、摇臂上端与摇臂油缸活塞杆顶部铰接点的第二坐标系ex2y2、动臂中部上突出部与摇臂中部铰接点的第三坐标系fx3y3、摇臂下端与连杆上端铰接点的第四坐标系gx4y4、连杆下端与末端机具铰接点的第五坐标系hx5y5；
11、计算动臂下端与末端机具铰接点i、动臂中部上突出部与摇臂中部铰接点f、动臂中部下突出部与动臂油缸活缸筒底部铰接点d在参考坐标系axy中与机架所在平面的夹角；
12、基于计算得到铰接点i、铰接点f、铰接点d与机架所在平面的夹角，将铰接点i、铰接点f、铰接点d的坐标变换为参考坐标系axy中的坐标，通过方位角公式计算得到动臂的铰接点连线ai与水平面的夹角αai；
13、基于铰接点d的坐标、机架与动臂油缸活塞杆顶部铰接点b的坐标，通过余弦定理计算得到动臂油缸姿态角αbd；
14、计算相邻编号坐标系之间的旋转角，根据计算得到的旋转角得到相邻坐标系转换的齐次变换矩阵；
15、利用得到的齐次变换矩阵，将摇臂上端与摇臂油缸活塞杆顶部铰接点e在第二坐标系ex2y2中的坐标、动臂中部上突出部与摇臂中部铰接点f在第三坐标系fx3y3中的坐标、摇臂下端与连杆上端铰接点g在第四坐标系gx4y4中的坐标、连杆下端与末端机具铰接点h在第五坐标系hx5y5中的坐标转换为在第一坐标系cx1y1中的坐标；
16、利用得到的齐次变换矩阵，将铰接点e、铰接点f、铰接点g、铰接点h在第一坐标系cx1y1中的坐标转换为在参考坐标系axy中的坐标，通过方位角公式计算得到摇臂油缸的铰接点连线ce与水平面的夹角αce、摇臂的铰接点连线ef与水平面的夹角αef、连杆的铰接点连线gh与水平面的夹角αgh；
17、基于铰接点h、铰接点i在参考坐标系axy中的坐标，计算铰接点连线hi与水平面的夹角；结合末端机具的尺寸与余弦定理，计算得到装载机末端机具的尖端与水平面的实时夹角。
18、结合第一方面，可选地，所述基于期望摇臂油缸行程和摇臂油缸的直径计算所需流量，包括：
19、获取实时摇臂油缸行程；
20、计算在预设调节时间内期望摇臂油缸行程与实时摇臂油缸行程的差值；
21、基于计算得到的差值，计算摇臂油缸的伸出运行速度v1或缩回运行速度v2；
22、获取摇臂油缸的缸筒直径d、活塞杆直径d；
23、通过下式计算摇臂油缸的伸出运行流量q1或缩回运行流量q2：
24、
25、结合第一方面，可选地，所述对计算得到的所需流量进行反向推导插值，得到摇臂油缸运行的先导压力，基于所述先导压力输出摇臂油缸的电比例减压阀的pwm信号，包括：
26、根据计算得到的所需流量，计算电控液压泵的所需排量；
27、通过排量与电流的对应关系，得到电控液压泵的pwm5信号，所述pwm5信号用于调节电控液压泵的斜盘的倾角使电控液压泵达到所需排量；
28、获取电控液压泵的实时排量；
29、基于获取到的实时排量和计算得到的所需流量，反向差值出摇臂油缸所需的先导压力；
30、通过先导压力与电流的对应关系，反推得到并输出摇臂油缸的电比例减压阀的pwm信号。
31、结合第一方面，可选地，所述pwm信号包括pwm1信号和pwm2信号；
32、所述pwm1信号用于控制摇臂油缸伸出，使末端机具内收；
33、所述pwm2信号用于控制摇臂油缸缩回，使末端机具外摆。
34、结合第一方面，可选地，还包括安全启动，通过以下步骤实现：
35、响应于启动装载机末端机具姿态自动控制的指令信号，获取动臂手柄的信号和动臂角度传感器的信号；
36、若未获取到动臂手柄的信号或未获取到动臂角度传感器的信号，不进行装载机末端机具姿态自动控制，输出摇臂油缸的电比例减压阀的pwm信号为0。
37、结合第一方面，可选地，还包括基于设置的末端机具的姿态角进行平动微调控制，通过以下步骤实现：
38、获取摇臂手柄的信号；
39、响应于摇臂油缸根据摇臂手柄的信号运行，获取摇臂油缸的电比例减压阀的当前pwm信号；
40、响应于当前pwm信号使摇臂油缸运行的方向与当前方向相同，记录摇臂油缸运行后末端机具的姿态角；
41、通过机构学推导计算装载机末端机具在当前动臂油缸行程下保持姿态角所需的期望摇臂油缸行程；
42、基于期望摇臂油缸行程和摇臂油缸的直径计算所需流量；
43、对计算得到的所需流量进行反向推导插值，得到摇臂油缸运行的先导压力，基于所述先导压力输出下一时刻摇臂油缸的电比例减压阀的pwm信号，通过所述摇臂油缸的电比例减压阀调整摇臂油缸运行以控制末端机具姿态。
44、结合第一方面，可选地，若当前pwm信号使摇臂油缸运行的方向与当前方向不相同时，停止输出当前pwm信号。
45、结合第一方面，可选地，还包括对动臂角度传感器和摇臂角度传感器进行标定，通过以下步骤实现：
46、将动臂置于最低点，记录动臂最低点时动臂的最小角度，通过机构学推导计算得到动臂油缸的最小长度，记录动臂油缸的最小长度；
47、在动臂置于最低点时，伸出摇臂油缸至极限值，通过机构学推导计算得到摇臂油缸的第一伸长长度；缩回摇臂油缸至极限值，通过机构学推导计算得到摇臂油缸的第一缩回长度；
48、将动臂置于最高点，记录动臂最高点时动臂的最大角度，通过机构学推导计算得到动臂油缸的最大长度，记录动臂油缸的最大长度；
49、在动臂置于最高点时，伸出摇臂油缸至极限值，通过机构学推导计算得到摇臂油缸的第二伸长长度；缩回摇臂油缸至极限值，通过机构学推导计算得到摇臂油缸的第二缩回长度；
50、比较第一伸长长度和第二伸长长度，较大值为摇臂油缸的最大长度，记录摇臂油缸的最大长度；比较第一缩回长度和第二缩回长度，较小值为摇臂油缸的最小长度，记录摇臂油缸的最小长度。
51、第二方面，本发明提供一种装载机，包括执行组件、控制组件和采集组件，
52、所述执行组件包括动臂、摇臂、摇臂油缸、连杆和末端机具；所述动臂的上部凸出部与所述摇臂油缸的缸筒底部铰接；所述摇臂油缸的活塞杆顶部与所述摇臂的上端铰接；所述动臂的中部上突出部与所述摇臂的中部铰接；所述摇臂的下端与所述连杆的上端铰接，所述连杆的下端与所述末端机具铰接；
53、所述采集组件包括动臂角度传感器和摇臂角度传感器；所述动臂角度传感器固接在所述动臂的上端与装载机的车架铰接点上；所述动臂角度传感器的输出端连接所述控制器的输入端，用于将采集到的动臂角度数据输出至所述控制器；所述摇臂角度传感器固接在所述动臂的中部上突出部与所述摇臂的中部铰接点上；所述摇臂角度传感器的输出端连接所述控制器的输入端，用于将采集到的摇臂角度数据输出至所述控制器；
54、所述控制组件包括动臂手柄、薄膜开关、控制器和摇臂油缸的电比例减压阀；
55、所述动臂手柄的输出端连接所述控制器的输入端，用于将动臂手柄的信号传输至所述控制器；所述薄膜开关的输出端连接所述控制器的输入端，用于将装载机末端机具姿态自动控制的启动信号和关闭信号传输至所述控制器；
56、所述控制器用于根据输入的角度数据和信号，进行装载机末端机具姿态自动控制，输出摇臂油缸的电比例减压阀的pwm1信号/pwm2信号；
57、所述摇臂油缸的电比例减压阀的输入端连接所述控制器的输出端，用于根据所述pwm1信号/pwm2信号调整摇臂油缸运行。
58、结合第二方面，可选地，所述执行组件还包括动臂油缸，所述动臂的上端与装载机的车架铰接，所述动臂的下端与所述末端机具铰接；所述动臂的中部下突出部与所述动臂油缸的缸筒底部铰接，所述动臂油缸的活塞杆顶部与装载机的车架连接。
59、结合第二方面，可选地，所述控制组件还包括摇臂手柄，所述摇臂手柄的输出端连接所述控制器的输入端，用于将摇臂手柄的信号传输至所述控制器；所述控制器用于根据输入的角度数据和摇臂手柄的信号进行基于设置的末端机具的姿态角进行平动微调控制，输出摇臂油缸的电比例减压阀的pwm1信号/pwm2信号。
60、结合第二方面，可选地，所述控制组件还包括电控液压泵，所述电控液压泵的输入端连接所述控制器的输出端，所述控制器根据所需流量输出电控液压泵的pwm5信号，所述电控液压泵用于根据pwm5信号调整排量。
61、结合第二方面，可选地，所述采集组件还包括电控液压泵排量传感器，所述电控液压泵排量传感器设置在所述电控液压泵的出口管路上，所述电控液压泵排量传感器的输出端连接所述控制器的输入端，用于将采集到的电控液压泵的排量数据输出至所述控制器。
62、与现有技术相比，本发明实施例所提供的一种装载机末端机具姿态自动控制方法及装载机所达到的有益效果包括：
63、本发明基于获取到的实时角度数据，通过机构学推导计算装载机末端机具的尖端与水平面的实时夹角；响应于实时夹角发生变化，通过机构学推导计算装载机末端机具在当前动臂油缸行程下保持水平所需的期望摇臂油缸行程；基于期望摇臂油缸行程和摇臂油缸的直径计算所需流量；本发明通过前馈正向计算所需流量，实现流量精准控制，减小了平动过程中的系统冲击；
64、本发明对计算得到的所需流量进行反向推导插值，得到摇臂油缸运行的先导压力，基于所述先导压力输出摇臂油缸的电比例减压阀的pwm信号，通过所述摇臂油缸的电比例减压阀调整摇臂油缸运行以控制末端机具姿态；本发明充分考虑液压系统本身非线性特征，通过准确计算流量反推应该请求的先导压力，实现上升及下落的平动控制，减小了系统调节过程中的冲击；准确地控制了摇臂油缸的电比例减压阀的阀口开度，使整个平动调节过程迅速且准确，提高了系统的操控性；
65、本发明采用前馈+反馈控制，通过前馈正向计算所需流量，从而调整电控液压泵排量，输出电流给主阀调整主阀开度，同时通过角度传感器计算摇臂油缸期望伸出量，对摇臂油缸行程进行闭环控制，进一步提高了平动的控制精度；能够避免过调节的现象，避免由油缸反复调节造成的失稳，防止货物掉落、整车倾覆，避免造成人员伤亡；
66、本发明采用电控液压自动控制，实时根据流量需求调节电控液压泵的排量，降低能量损失；
67、本发明针对平动调节过程进行设计，使调平在动臂运动过程中进行使能，保证了整个调平过程的安全性，减小了人员财产损失；
68、本发明还提供安全启动，未获取到动臂手柄的信号或未获取到动臂角度传感器的信号，不进行装载机末端机具姿态自动控制；能够保证作业过程的安全，减小系统的不稳定性；
69、本发明还提供基于设置的末端机具的姿态角进行平动微调控制，保证了整个作业过程流畅；
70、本发明对动臂角度传感器和摇臂角度传感器进行标定，能够避免因为制造误差引起传感器角度误差。
1.一种装载机末端机具姿态自动控制方法，其特征在于，包括：
2.根据权利要求1所述的装载机末端机具姿态自动控制方法，其特征在于，所述基于获取到的实时角度数据，通过机构学推导计算装载机末端机具的尖端与水平面的实时夹角，包括：
3.根据权利要求1所述的装载机末端机具姿态自动控制方法，其特征在于，所述基于期望摇臂油缸行程和摇臂油缸的直径计算所需流量，包括：
4.根据权利要求1所述的装载机末端机具姿态自动控制方法，其特征在于，所述对计算得到的所需流量进行反向推导插值，得到摇臂油缸运行的先导压力，基于所述先导压力输出摇臂油缸的电比例减压阀的pwm信号，包括：
5.根据权利要求1所述的装载机末端机具姿态自动控制方法，其特征在于，所述pwm信号包括pwm1信号和pwm2信号；
6.根据权利要求1所述的装载机末端机具姿态自动控制方法，其特征在于，还包括安全启动，通过以下步骤实现：
7.根据权利要求1所述的装载机末端机具姿态自动控制方法，其特征在于，还包括基于设置的末端机具的姿态角进行平动微调控制，通过以下步骤实现：
8.根据权利要求7所述的装载机末端机具姿态自动控制方法，其特征在于，若当前pwm信号使摇臂油缸运行的方向与当前方向不相同时，停止输出当前pwm信号。
9.根据权利要求1所述的装载机末端机具姿态自动控制方法，其特征在于，还包括对动臂角度传感器和摇臂角度传感器进行标定，通过以下步骤实现：
10.一种装载机，其特征在于，包括执行组件、控制组件和采集组件，
11.根据权利要求10所述的装载机，其特征在于，所述执行组件还包括动臂油缸，所述动臂的上端与装载机的车架铰接，所述动臂的下端与所述末端机具铰接；所述动臂的中部下突出部与所述动臂油缸的缸筒底部铰接，所述动臂油缸的活塞杆顶部与装载机的车架连接。
12.根据权利要求10所述的装载机，其特征在于，所述控制组件还包括摇臂手柄，所述摇臂手柄的输出端连接所述控制器的输入端，用于将摇臂手柄的信号传输至所述控制器；所述控制器用于根据输入的角度数据和摇臂手柄的信号进行基于设置的末端机具的姿态角进行平动微调控制，输出摇臂油缸的电比例减压阀的pwm1信号/pwm2信号。
13.根据权利要求10所述的装载机，其特征在于，所述控制组件还包括电控液压泵，所述电控液压泵的输入端连接所述控制器的输出端，所述控制器根据所需流量输出电控液压泵的pwm5信号，所述电控液压泵用于根据pwm5信号调整排量。
14.根据权利要求13所述的装载机，其特征在于，所述采集组件还包括电控液压泵排量传感器，所述电控液压泵排量传感器设置在所述电控液压泵的出口管路上，所述电控液压泵排量传感器的输出端连接所述控制器的输入端，用于将采集到的电控液压泵的排量数据输出至所述控制器。
