买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

摘要：一种适用于船舱无人推耙机路径规划方法，包括：第一步区域推耙：应对舱内存在大量煤堆的工况，采用目标点控制区域遍历算法，遍历整个推耙区域，将煤堆聚拢到堆煤处。第二步区域内点对点：应对舱内残余些许煤堆的工况，采用改进dubins规划算法，将煤堆聚拢到堆煤处。此规划方案使推耙机的路径规划具有良好的鲁棒性且有效的提高了推耙机作业的灵活性，提高了推耙机完成作业的效率。

主权项：1.一种适用于船舱无人推耙机路径规划方法，包括如下步骤：步骤1，区域推耙：针对舱内存在大量煤堆的工况，采用目标点控制区域遍历算法，遍历整个推耙区域，将煤堆聚拢到堆煤处，具体包括：步骤1-1：将旋转中心作为推耙机的位姿点；使用一个3*1的列向量表示目标点的位置，在不同的坐标系下，表示位置的列向量不同；通过下述公式将目标点变换到车辆坐标系下； Pc，Pw分别代表目标点在车辆坐标系和世界坐标系下的位姿表示，均为3*1的列向量；wcR是一个3*3的旋转矩阵代表世界坐标系到车辆坐标系的旋转变换，Pcw是一个3*1的列向量表示世界坐标系的原点在车辆坐标系的位置；步骤1-2：控制器接受区域角点构建一个矩形推耙区域并计算区域的大小是否在设定的阈值范围内；步骤1-3：选取矩形推耙区域的下边界线作为区域推耙的堆煤线，提取离堆煤线最近的煤堆区域作为推耙起始线；根据设定的车辆宽度对推耙起始线与堆煤线进行分割，得到推耙目标点和堆煤点的位置集合；步骤1-4：按照先推耙目标点后堆煤点的顺序遍历上述两个集合中的位置点；对于一个位置点，运用步骤1-1中的变换公式将位置点变换到车辆坐标系下；根据变换后位置点在车辆坐标系下的x,y坐标来控制车辆左右旋转，调整车辆的姿态，使位置点在车辆运动的正前方；步骤1-5：计算位置点与车辆中心之间的欧式距离，当距离小于设定值时则认为车辆已到达位置点，再进行车辆姿态的修正；矫正车辆姿态时不再考虑车辆位置的变化，只判断车辆姿态值；步骤1-5将矫正车辆的位置和矫正车辆的姿态解耦；改装后无人推耙机只配备了单个电机，旋转时只能设定一个履带运动，另一个履带保持静止；这导致车辆旋转时的旋转中心不在车辆的正中心，即无法原地旋转，此时旋转会导致车辆的位置产生不可忽略的变换；如果同时考虑车辆的位置和车辆的姿态，会使车辆陷入长时间的矫正，工作效率降低；步骤1-6：获取车辆姿态与位置点位姿的欧拉角表示并进行比对，利用两者的差值控制车辆的旋转矫正；当两者的差值小于设定的阈值则判定位姿矫正成功，并执行相应的推耙动作；步骤1-7：重复执行步骤1-4到步骤1-6，直到遍历完推耙目标点和堆煤点的位置集合；步骤1-8：提取离堆煤线最近的煤堆区域作为新的推耙起始线并判断新的推耙起始线是否已经超出矩形推耙区域的范围，如是，则发送区域推耙完成的信息并进入区域内点对点推耙模式，即转到步骤2，如否，则转到步骤1-3计算得到新的推耙目标点和堆煤点的位置集合，继续执行步骤1-4到步骤1-7；步骤2，区域内点对点：针对舱内残余些许煤堆的工况，采用改进dubins规划算法，将煤堆聚拢到堆煤处，具体包括：步骤2-1：获取车辆姿态与目标点位姿的欧拉角表示，计算两者偏航角的差值，利用此差值控制推耙机的旋转使得偏航角的差值在设定的阈值内；步骤2-2：采用threepathsegment方法，将规划的路径分为三段，分别为起始点Ti到切线点1的圆弧段，切线点1到切线点2的圆弧段或直线段，切线点2到终止点Tg的圆弧段；依据每段路径的方向，将可行的路径类型整合成dubins集合D＝{LSL，RSR，RSL，LSR，RLR，LRL}；其中L表示从沿逆时针方向的圆弧运动，R表示沿顺时针方向圆弧运动，S表示沿直线运动；步骤2-3：进行坐标系的变换，将原始的世界坐标系转换为以起点Tixi,yi,αi指向终点Tgxg,yg,βg的直线作为x轴，垂直于该方向向上为y轴；经过坐标系转换后，起始点的坐标为0,0,αi-θ,终点为d,0,βg-θ；其中θ为从起点到终点的航向角度差；θ＝arctanyg-yixg-xi2D＝sqrtxg-xi2+yg-yi23d＝DR4上述公式中R为车辆的转弯半径，D为车辆离目标点的欧式距离，将D除上R是为了正则化R，使车辆转弯半径变为1，这样弧长即等于角度的弧度便于后面的计算，因此在后续看到的cosα前面省略了半径1；LSL情况下，计算dubins规划的路径长度；根据坐标系变换后的几何关系，可以得到下列约束方程：α＝αi-θ5β＝βg-θ6pcosα+t-sinα+sinβ＝d7psinα+t+cosα-cosβ＝08α+t+g＝β{mod2π}9上述公式中p为两个切线点之间的距离，t为起始段转过的弧度数即起始点Ti到切线点1的圆弧段，g为终止段转过的弧度数即切线点2到终止点Tg的圆弧段；求解上述方程组，可解得： 可计算出总长度为：L＝tlsl+plsl+glsl＝-α+β+plsl13步骤2-4：以步骤2-3所述公式计算步骤2-2中可行路径的长度，选取最短的dubins路径作为规划路径；步骤2-5：分3段执行规划路径，分别是起始点Ti到切线点1的圆弧段，从切线点1到切线点2的圆弧段或直线段，从切线点2到终止点Tg的圆弧段；到达目标点之后，再进行车辆姿态的修正并执行要求的推耙动作；步骤2-6：以堆煤点作为目标点重复步骤2-1到步骤2-5，将煤堆聚拢到堆煤区域；步骤2-7：重复步骤2-1到2-6直至推耙区域内无残余煤堆，至此完成无人推耙机的路径规划，终止推耙并等待接受下一个指令开始新的规划。

全文数据：

权利要求：

百度查询： 浙江工业大学 杭州登元科技有限公司 一种适用于船舱无人推耙机路径规划方法