一种露天钻机基于运动学逆解的定位目标值计算方法和系统与流程

本技术属于工程机械控制，尤其涉及一种露天钻机基于运动学逆解的定位目标值计算方法和系统。

背景技术：

1、露天钻机是露天矿山穿孔爆破作业中的核心设备，其钻孔定位精度直接影响爆破效果与后续开采效率。随着矿山智能化建设的推进，对钻机自动定位功能的需求日益迫切。

2、现有露天钻机的大臂和推进梁构成串联式多关节结构，具体地，钻机包括串联连接的基座、大臂和推进梁；大臂包括第一摆动关节和第一俯仰关节；推进梁包括第二摆动关节和第二俯仰关节；钻机的钻头设置于推进梁末端；以大臂左右摆动、大臂上下俯仰、推进梁左右摆动、推进梁上下俯仰共四个自由度实现钻头在空间中的位姿调整。在定位作业中，操作人员根据目标孔位信息，通过手动操控各关节逐一调整钻头位置与方向，使钻头对准目标孔位。

3、然而，上述手动定位方式中，定位精度和效率高度依赖操作人员的经验与熟练程度，不同操作者之间差异大，位置精度和姿态精度均不稳定。

技术实现思路

1、本技术提供一种露天钻机基于运动学逆解的定位目标值计算方法和系统，通过建立钻机大臂与推进梁的运动学模型及融合车辆实时位姿信息的坐标变换体系，将目标孔位从世界坐标系精确转换至车辆坐标系，并以各关节角度为变量构建最小化钻头实际位置与目标位置偏差的优化问题求解目标值，从而以系统化的数学建模和数值优化手段取代传统人工手动逐一调节方式，解决手动定位依赖操作人员经验、精度与效率不稳定的问题。

2、第一方面，本技术提供一种露天钻机基于运动学逆解的定位目标值计算方法，包括：

3、构建钻机大臂和推进梁各关节的运动学模型，并基于运动学模型构建钻头相对于车辆坐标系的位置和姿态关系矩阵；

4、获取车辆的位置信息和姿态信息，以构建车辆坐标系相对于世界坐标系的变换矩阵；姿态信息包括世界坐标系下的横滚角、俯仰角和航向角；

5、将位置和姿态关系矩阵与变换矩阵的乘积作为钻头相对于世界坐标系的位置和姿态关系矩阵，以表征钻头在世界坐标系下的实际位姿与车辆位姿及各关节角度之间的映射关系；

6、获取目标孔位在世界坐标系下的位置信息和姿态信息，以根据目标孔位的位姿信息、变换矩阵和变换矩阵的逆矩阵将目标孔位的位姿转换至车辆坐标系下，得到目标孔位相对于车辆坐标系的目标位置信息xbit、ybit和zbit；其中xbit、ybit和zbit分别表示目标孔位在车辆坐标系的x轴、y轴和z轴方向上的坐标；

7、将各关节角度作为待求解变量，根据映射关系，以目标位置信息xbit、ybit和zbit为目标，构建使钻头实际位置与目标位置之间的偏差最小化的优化问题，求解优化问题得到各关节角度目标值。

8、可选地，获取车辆的位置信息和姿态信息，以构建车辆坐标系相对于世界坐标系的变换矩阵，包括：

9、构建车辆坐标系相对于世界坐标系的变换矩阵的表达式：

10、；

11、其中，为车辆坐标系相对于世界坐标系之间的旋转矩阵；pgps为车辆在世界坐标系下的位置向量；θ为车辆绕世界坐标系x轴旋转的横滚角；γ为车辆绕世界坐标系y轴旋转的俯仰角；φ为车辆绕世界坐标系z轴旋转的航向角。

12、可选地，获取目标孔位在世界坐标系下的位置信息和姿态信息，以根据目标孔位的位姿信息、变换矩阵和变换矩阵的逆矩阵将目标孔位的位姿转换至车辆坐标系下，得到目标孔位相对于车辆坐标系的目标位置信息xbit、ybit和zbit；其中，xbit、ybit和zbit分别表示目标孔位在车辆坐标系的x轴、y轴和z轴方向上的坐标，包括：

13、获取世界坐标系下目标孔的起始位置坐标和结束位置坐标；

14、根据目标孔的起始位置坐标和结束位置坐标构建目标孔位在世界坐标系下的姿态旋转矩阵；

15、根据目标孔的起始位置坐标和姿态旋转矩阵，构建目标孔位在世界坐标系下的齐次位姿矩阵；

16、利用车辆坐标系相对于世界坐标系的变换矩阵的逆矩阵与目标孔位在世界坐标系下的齐次位姿矩阵的乘积，将目标孔位从世界坐标系转换至车辆坐标系，得到目标孔位在车辆坐标系下的齐次位姿矩阵；

17、从目标孔位在车辆坐标系下的齐次位姿矩阵中提取位置分量，得到目标孔位相对于车辆坐标系的目标位置信息。

18、可选地，根据目标孔的起始位置坐标和结束位置坐标构建目标孔位在世界坐标系下的姿态旋转矩阵，包括：

19、以目标孔的起始位置坐标ps=(xs,ys,zs)为原点，将结束位置坐标pe=(xe,ye,ze)指向起始位置坐标的方向向量作为目标孔位z轴方向；

20、将结束位置坐标指向起始位置坐标的方向向量与预设的爆破方向向量的叉积方向作为目标孔位x轴方向；

21、以作为目标孔位y轴方向；由此构建目标孔位在世界坐标系下的3×3姿态旋转矩阵。

22、可选地，根据目标孔的起始位置坐标和姿态旋转矩阵，构建目标孔位在世界坐标系下的齐次位姿矩阵，包括：

23、构建目标孔位在世界坐标系下的4×4齐次位姿矩阵的表达式：

24、。

25、可选地，偏差最小化的优化问题的目标函数f为：

26、；

27、a1=dx+a3sinθ1sin(θ2+θ3)+(a5+a6)(cosθ1sinθ4+cosθ4sinθ1cos(θ2+θ3))-a1sinθ1；

28、a2=cosθ1cosθ4-sinθ4sinθ1cos(θ2+θ3)；

29、a3=-a2sinθ1；

30、b1=dy-a3cosθ1sin(θ2+θ3)+(a5+a6)(sinθ1sinθ4-cosθ4cosθ1cos(θ2+θ3))+a1cosθ1；

31、b2=cosθ4sinθ1+sinθ4cosθ1cos(θ2+θ3)；

32、b3=-a2cosθ1；

33、c1=dz+a3cos(θ2+θ3)；

34、c2=sin(θ2+θ3)sinθ4；

35、c3=-(a5+a6)sin(θ2+θ3)cosθ4；

36、其中，θ1为大臂左右摆动关节的旋转角度；θ2为大臂上下俯仰关节的旋转角度；θ3为推进梁上下俯仰关节的旋转角度；θ4为推进梁左右摆动关节的旋转角度；a1为第一中间系数；a2为第二中间系数；a3为第三中间系数；b1为第四中间系数；b2为第五中间系数；b3为第六中间系数；c1为第七中间系数；c2为第八中间系数；c3为第九中间系数；a4为推进梁回转中心轴线到推进梁顶端的距离；a7为推进梁的延伸长度；lbit为推进梁的顶端到底端的总长；dx为表示第二坐标系相对于第一坐标系在x轴方向上的偏移距离；a3为推进梁左右回转中心轴线到推进梁俯仰中心轴线间的距离；a5为推进梁回转中心轴线与顶尖之间的距离；a6为顶尖与钻头中心轴线间的距离；a1为大臂回转中心轴线与大臂俯仰中心轴线间的距离；dy为表示第二坐标系相对于第一坐标系在y轴方向上的偏移距离；dz为表示第二坐标系相对于第一坐标系在z轴方向上的偏移距离；第一坐标系以车辆定位天线处为原点，车辆长度方向x轴方向，车辆宽度方向为y轴方向，z轴方向垂直于车辆长度和宽度方向；第二坐标系以大臂转动处为原点，车辆长度方向x轴方向，车辆宽度方向为y轴方向，z轴方向垂直于车辆长度和宽度方向。

37、可选地，优化问题的求解满足各关节运动范围的约束条件：θi,min≤θi≤θi,max；i=1,2,3,4；θi,min为第i个关节的最小允许转动角度；θi,max为第i个关节的最大允许转动角度；采用数值迭代算法求解，迭代初始值采用各关节当前实际角度值；数值迭代算法的收敛条件为钻头实际位置与目标位置之间的偏差小于预设阈值，或迭代次数达到预设最大迭代次数。

38、第二方面，本技术提供一种露天钻机基于运动学逆解的定位目标值计算系统，包括：

39、第一构建模块，用于构建钻机大臂和推进梁各关节的运动学模型，并基于运动学模型构建钻头相对于车辆坐标系的位置和姿态关系矩阵；

40、第二构建模块，用于获取车辆的位置信息和姿态信息，以构建车辆坐标系相对于世界坐标系的变换矩阵；姿态信息包括世界坐标系下的横滚角、俯仰角和航向角；

41、确定模块，用于将位置和姿态关系矩阵与变换矩阵的乘积作为钻头相对于世界坐标系的位置和姿态关系矩阵，以表征钻头在世界坐标系下的实际位姿与车辆位姿及各关节角度之间的映射关系；

42、转换模块，用于获取目标孔位在世界坐标系下的位置信息和姿态信息，以根据目标孔位的位姿信息、变换矩阵和变换矩阵的逆矩阵将目标孔位的位姿转换至车辆坐标系下，得到目标孔位相对于车辆坐标系的目标位置信息xbit、ybit和zbit；其中xbit、ybit和zbit分别表示目标孔位在车辆坐标系的x轴、y轴和z轴方向上的坐标；

43、求解模块，用于将各关节角度作为待求解变量，根据映射关系，以目标位置信息xbit、ybit和zbit为目标，构建使钻头实际位置与目标位置之间的偏差最小化的优化问题，求解优化问题得到各关节角度目标值。

44、第三方面，本技术提供一种计算机设备，包括处理器和存储器；其中，处理器执行存储器中保存的计算机程序时实现第一方面所述的露天钻机基于运动学逆解的定位目标值计算方法的步骤。

45、第四方面，本技术提供一种计算机可读存储介质，用于存储计算机程序；计算机程序被处理器执行时实现第一方面所述的露天钻机基于运动学逆解的定位目标值计算方法的步骤。

46、有益效果：

47、本技术通过构建钻机大臂和推进梁各关节的运动学模型并据此建立钻头相对于车辆坐标系的精确位姿变换矩阵，同时在获取车辆位置及横滚角、俯仰角、航向角等姿态信息的基础上动态构建车辆坐标系相对于世界坐标系的变换矩阵，从而以两者乘积精确表征钻头在世界坐标系下的实际位姿与车辆位姿及各关节角度之间的映射关系，这一定量化映射关系的建立，使得后续任何基于该模型的目标值求解均具有统一的解析基础，规避了人工操作带来的随机误差。

48、在此基础上，本技术进一步获取目标孔位在世界坐标系下的完整位姿信息，并利用变换矩阵及其逆矩阵进行精确的坐标反算，将目标孔位准确转换至车辆坐标系下，获得目标孔位在车辆坐标系三个轴方向上的目标位置信息，该坐标系转换过程充分考虑了车辆自身因地面不平或停靠偏差导致的位姿变化，有效消除了车辆姿态对定位准确性的干扰，使转换后的目标位置真实反映钻机执行机构的工作参考系。

49、本技术还将各关节角度设为待求解变量，并以上述目标位置信息为优化目标，构建使钻头实际位置与目标位置之间偏差最小化的优化问题，进而通过数值求解获得各关节角度的最优目标值，这一优化求解策略将传统依赖人工反复试凑的定位过程转化为目标明确的数学规划问题，不仅完全摆脱了对操作人员经验与熟练程度的依赖，而且凭借其明确的收敛准则和以当前实际角度为迭代初值的快速求解机制，能够在保证定位精度收敛至预设阈值的同时满足实时控制需求，最终实现了露天钻机在复杂地形条件下自动化、高精度和高效率的孔位对准。