CN110398963A - 无轨导航纠偏控制方法、装置、存储介质及控制器 - Google Patents
无轨导航纠偏控制方法、装置、存储介质及控制器 Download PDFInfo
- Publication number
- CN110398963A CN110398963A CN201910632253.3A CN201910632253A CN110398963A CN 110398963 A CN110398963 A CN 110398963A CN 201910632253 A CN201910632253 A CN 201910632253A CN 110398963 A CN110398963 A CN 110398963A
- Authority
- CN
- China
- Prior art keywords
- deviation
- course
- moving target
- degree
- membership
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
Abstract
本发明提出一种无轨导航纠偏控制方法、装置、存储介质及控制器，该方法包括：根据运动目标的当前坐标和当前航向获取运动目标相对于预设导航路径的距离偏差和航向偏差；对距离偏差和航向偏差进行模糊化，得到距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度；根据预设模糊推理规则推理确定在距离偏差和航向偏差隶属于不同的预设模糊等级时对应的纠偏量的模糊等级，并根据距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定纠偏量为相应模糊等级的隶属度；根据纠偏量隶属于不同的预设模糊等级的隶属度的平均值对运动目标进行纠偏控制。本发明根据距离偏差和航向偏差得到精确的控制量，并及时纠偏控制，准确快速实现导航路径跟踪。
Description
技术领域
本发明涉及导航控制技术领域，尤其涉及一种无轨导航纠偏控制方法、装置、存储介质及控制器。
背景技术
对于激光导航、惯性导航等无轨导航AGV(Automated Guided Vehicle，自动导引运输车)，导航路径是虚拟存在的，并不会对AGV产生物理上的约束。而受定位系统误差、执行机构机械误差、地面打滑等因素影响，AGV无法完全按照预定导航路径行驶。因此，在行进过程中，AGV自身需要不断计算当前位置与导航路径之间的偏差即路径偏差，然后根据偏差的大小通过纠偏控制算法计算出纠偏量，最终由执行机构响应纠偏量，保证AGV沿着导航路径行进，即实现路径跟踪。路径偏差的计算是实现路径跟踪的重要一环，是实现路径跟踪的前提。而纠偏控制则是实现路径跟踪的关键，决定了AGV能否快速准确实现导航路径跟踪。
可见，如何实现精确的纠偏控制对快速准确实现导航路径跟踪具有重要意义。
发明内容
本发明实施例提出了一种无轨导航纠偏控制方法、装置、存储介质及控制器，以实现精准的纠偏控制，准确快速实现导航路径跟踪。
本发明实施例提供了一种无轨导航纠偏控制方法，所述方法包括：
根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差；
根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化，得到所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度；
根据预设模糊推理规则推理确定在所述距离偏差和航向偏差隶属于不同的预设模糊等级时对应的纠偏量的模糊等级，并根据所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定所述纠偏量为相应模糊等级的隶属度；
获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制。
可选地，当所述预设导航路径为直线时，所述根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差，包括：
计算所述当前坐标相对于所述预设导航路径的垂直距离和方向，根据所述垂直距离和方向确定所述运动目标相对于预设导航路径的距离偏差；
计算所述预设导航路径的第一水平倾角，根据所述运动目标的当前航向与所述第一水平倾角的角度差确定所述运动目标相对于预设导航路径的航向偏差。
可选地，当所述预设导航路径为弧线时，所述根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差，包括：
计算所述当前坐标与所述预设导航路径的中心坐标之间的距离，根据所述距离与所述预设导航路径的圆弧半径之间的差值以及所述当前坐标相对于所述预设导航路径的方向确定所述运动目标相对于预设导航路径的距离偏差；
计算所述中心坐标与所述当前坐标所在直线的第二水平倾角；
根据所述第二水平倾角确定所述运动目标在当前坐标对应的目标航向，根据所述目标航向与所述当前航向的角度差确定所述运动目标相对于预设导航路径的航向偏差。
可选地，在所述根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化之前，所述方法还包括：
根据距离偏差和航向偏差对应的量化因子分别对所述距离偏差和航向偏差进行量化。
可选地，所述根据所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定所述纠偏量为相应模糊等级的隶属度，包括：
采用最小值法将所述距离偏差和航向偏差隶属于对应预设模糊等级的隶属度的最小值作为对应纠偏量为相应模糊等级的隶属度。
可选地，所述获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制，包括：
根据不同的预设模糊等级对应的权重系数采用加权平均法计算所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值；
根据纠偏量对应的量化因子对所述平均值进行量化，以将所述纠偏量转化为实际纠偏量，并根据所述实际纠偏量进行纠偏控制。
本发明另一实施例提供了一种无轨导航纠偏控制装置，所述装置包括：
获取模块，用于根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差；
模糊化模块，用于根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化，得到所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度；
推理模块，用于根据预设模糊推理规则推理确定在所述距离偏差和航向偏差隶属于不同的预设模糊等级时对应的纠偏量的模糊等级，并根据所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定所述纠偏量为相应模糊等级的隶属度；
控制模块，用于获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制。
可选地，所述获取模块，具体用于计算所述当前坐标相对于所述预设导航路径的垂直距离和方向，根据所述垂直距离和方向确定所述运动目标相对于预设导航路径的距离偏差；计算所述预设导航路径的第一水平倾角，根据所述运动目标的当前航向与所述第一水平倾角的角度差确定所述运动目标相对于预设导航路径的航向偏差。
可选地，所述获取模块，具体用于计算所述当前坐标与所述预设导航路径的中心坐标之间的距离，根据所述距离与所述预设导航路径的圆弧半径之间的差值以及所述当前坐标相对于所述预设导航路径的方向确定所述运动目标相对于预设导航路径的距离偏差；计算所述中心坐标与所述当前坐标所在直线的第二水平倾角；根据所述第二水平倾角确定所述运动目标在当前坐标对应的目标航向，并根据所述目标航向与所述当前航向的角度差确定所述运动目标相对于预设导航路径的航向偏差。
可选地，所述装置还包括：
第一量化模块，用于在所述模糊化模块根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化之前，根据距离偏差和航向偏差对应的量化因子分别对所述距离偏差和航向偏差进行量化。
可选地，所述控制模块，包括：
计算单元，用于根据不同的预设模糊等级对应的权重系数采用加权平均法计算所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值；
控制单元，用于根据纠偏量对应的量化因子对所述平均值进行量化，以将所述纠偏量转化为实际纠偏量，并根据所述实际纠偏量进行纠偏控制。
此外，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上所述方法的步骤。
此外，本发明还提供了一种控制器，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上所述方法的步骤。
本发明实施例提供的无轨导航纠偏控制方法、装置、存储介质及控制器，通过从距离偏差和航向偏差两方面考虑来计算运动目标的当前坐标和当前航向相对于预设导航路径的路径偏差，采用模糊控制算法根据距离偏差和航向偏差得到精确的控制量，并及时纠偏控制，准确快速实现无轨导航车辆的导航路径跟踪。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
附图说明
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：
图1为本发明实施例提供的一种无轨导航纠偏控制方法的流程示意图；
图2为本发明实施例提供的无轨导航纠偏控制方法中直线段路径偏差模型图；
图3为本发明实施例提供的无轨导航纠偏控制方法中圆弧段路径偏差模型图；
图4为本发明实施例提供的无轨导航纠偏控制方法中路径纠偏模糊控制方法实现原理图；
图5为本发明实施例提供的无轨导航纠偏控制方法中输入及输出变量隶属度函数示意图；
图6为本发明实施例提供的一种无轨导航纠偏控制装置的结构示意图。
具体实施方式
下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。
本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非被特定定义，否则不会用理想化或过于正式的含义来解释。
图1示意性示出了本发明一个实施例的无轨导航纠偏控制方法的流程图。参照图1，本发明实施例提出的无轨导航纠偏控制方法具体包括步骤S11～S14，如下所示：
S11、根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差。
本实施例中的运动目标为激光导航、惯性导航等无轨导航自动导引运输车AGV。
S12、根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化，得到所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度。
本实施例中，在所述根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化之前，所述方法还包括：根据距离偏差和航向偏差对应的量化因子分别对所述距离偏差和航向偏差进行量化。然后，根据事先定好的模糊变量等级以及隶属度函数，对变量(即距离偏差和航向偏差)进行模糊化。
S13、根据预设模糊推理规则推理确定在所述距离偏差和航向偏差隶属于不同的预设模糊等级时对应的纠偏量的模糊等级，并根据所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定所述纠偏量为相应模糊等级的隶属度。
本实施例中，根据事先预设的模糊推理规则进行模糊推理，得出不同距离偏差Ep和航向偏差Ea等级下所对应的纠偏量C的模糊等级及其隶属度。
S14、获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制。
本实施例中，在得出不同距离偏差Ep和航向偏差Ea等级下所对应的纠偏量C的模糊等级及其隶属度之后，通过去模糊化得到输出变量纠偏量C的精确值，乘以量化因子后输出至执行机构，实现纠偏控制。
本发明实施例提供的无轨导航纠偏控制方法，通过从距离偏差和航向偏差两方面考虑来计算运动目标的当前坐标和当前航向相对于预设导航路径的路径偏差，采用模糊控制算法根据距离偏差和航向偏差得到精确的控制量，并及时纠偏控制，准确快速实现无轨导航车辆的导航路径跟踪。
在本发明一个实施例中，当所述预设导航路径为直线时，所述根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差，具体包括以下步骤：
计算所述当前坐标相对于所述预设导航路径的垂直距离和方向，根据所述垂直距离和方向确定所述运动目标相对于预设导航路径的距离偏差；
计算所述预设导航路径的第一水平倾角，根据所述运动目标的当前航向与所述第一水平倾角的角度差确定所述运动目标相对于预设导航路径的航向偏差。
在进行无轨导航AGV轨迹偏差计算时，首先要建立AGV运行的环境的全局坐标系xoy，同时AGV本身需具备定位装置如激光导航传感器、惯性导航传感器等，能够实时反馈AGV在全局坐标系中的当前位姿(xa,ya,θ)，包括当前坐标(xa,ya)和当前航向θ。AGV在执行任务前，可由上位机调度系统规划导航路径或者通过AGV人机交互界面预先设定导航路径，导航路径可分为直线段路径和圆弧段路径。
本实施例中，对于直线段路径，首先根据起点和终点确定直线方程后，求解AGV当前坐标(xa,ya)与直线之间的距离，即得到距离偏差；然后求解AGV当前航向θ与直线倾角的差，即得到航向偏差。
具体的，对于直线段路径，可由起点P1(x1,y1)和终点P2(x2,y2)确立，如图1所示。通过P1、P2点的直线方程为：
转化成Ax+By+C＝0的形式可得：
(y2-y1)*x+(x1-x2)*y+x2*y1-x1*y2＝0
由此可得A、B、C三个参数作为直线方程的已知项：
假设以直线前进方向为准，AGV位于直线段的左侧，距离偏差定义为正；AGV位于直线段的右侧，距离偏差定义为负，则距离偏差的计算如下：
1、x1≠x2且y1≠y2时：
2、x1＝x2时：
3、y1＝y2时：
同样，假设以直线前进方向为准，AGV航向倾向于直线左侧，定义航向偏差为正；AGV航向倾向于直线右侧，定义航向偏差为负，则航向偏差的计算如下：
1、x1≠x2且y1≠y2时：
首先求出直线的水平倾斜角度：
所以航向偏差为：
2、x1＝x2时：
3、y1＝y2时：
在本发明另一实施例中，当所述预设导航路径为弧线时，所述根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差，具体包括以下步骤：
计算所述当前坐标与所述预设导航路径的中心坐标之间的距离，根据所述距离与所述预设导航路径的圆弧半径之间的差值以及所述当前坐标相对于所述预设导航路径的方向确定所述运动目标相对于预设导航路径的距离偏差；
计算所述中心坐标与所述当前坐标所在直线的第二水平倾角；
根据所述第二水平倾角确定所述运动目标在当前坐标对应的目标航向，根据所述目标航向与所述当前航向的角度差确定所述运动目标相对于预设导航路径的航向偏差。
本实施例中，对于圆弧段路径，已知的圆弧圆心坐标O(xo,yo)以及半径R，距离偏差即为AGV当前坐标(xa,ya)到圆心的距离与圆弧半径的差值；而航向偏差则首先计算AGV当前坐标(xa,ya)与圆心连线所在直线的倾角，然后求解AGV当前航向θ与直线倾角互余角的差所得。
对于圆弧段路径，可由圆心O(xo,yo)、半径R以及前进方向(顺时针或逆时针)确定，如图2所示。
与直线段路径偏差计算一样，假设以圆弧段前进方向为准，AGV位于圆弧段的左侧，距离偏差定义为正；AGV位于圆弧段的右侧，距离偏差定义为负，则距离偏差的计算如下：
首先计算圆心O(xo,yo)与AGV当前位置A(xa,ya)之间的距离：
由此，距离偏差为：
圆弧段路径航向偏差的计算，由于目标航向角时刻在变化，因此首先要确定当前时刻的目标航向角。在本方法中，把当前时刻的目标航向角定义为圆心O(xo,yo)与AGV当前位置A(xa,ya)所在直线与圆弧相交点的切线角度，其计算步骤如下：
1、计算圆心O(xo,yo)与AGV当前位置A(xa,ya)所在直线的倾角：
2、AGV在当前位置的目标航向角即为：
β＝90°-γ
由此，航向偏差为：
计算出直线段和圆弧段的路径偏差后，采用模糊控制算法进行纠偏控制，实现路径跟踪，其原理图如图4所示。基本流程为：AGV每隔一定周期检测当前位姿，根据前述路径偏差计算方法与目标路径位姿对比计算出距离偏差Ep和航向偏差Ea，两个输入变量分别乘以相应的量化因子后，根据事先定好的模糊变量等级以及隶属度函数，对变量进行模糊化。然后根据事先定好的模糊推理规则进行模糊推理，得出不同距离偏差Ep和航向偏差Ea等级下所对应的纠偏量C等级及其隶属度。最后通过去模糊化得到输出变量纠偏量C的精确值，乘以量化因子后输出至执行机构，实现纠偏控制。进一步地，在纠偏控制后，反馈新的位姿信息到控制装置，已确认纠偏效果。
在一个具体实施例中，变量模糊化的具体过程如下：
本实施例中，将距离偏差Ep、航向偏差Ea以及纠偏量C分别分为7个模糊等级，即为{NB，NM，NS，ZE，PS，PM，PB}，中文意思是{负大，负中，负小，零，正小，正中，正大}，对应的数值为{-3，-2，-1，0，1，2，3}。随后，确定三个变量的隶属度函数。隶属度函数的作用在于确定某个精确的变量隶属于某个模糊等级的程度。一个具体实施例中，本发明可以采用三角形函数作为隶属度函数，如图5所示。例如，当距离偏差Ep为-2时，根据隶属度函数可以得出，此时距离偏差100％属于NM等级，而当距离偏差Ep为0.5时，根据隶属度函数可以得出，此时距离偏差50％属于ZE等级，50％属于PS等级，由此完成模糊化的过程。
本实施例中，模糊推理规则制定及模糊推理具体过程如下：
模糊推理规则首先依靠经验法制定，然后通过实际测试逐步调整至最优。本发明制定模糊推理规则的经验，主要参考汽车驾驶经验所得，汽车在不同偏离行车道距离和角度下，驾驶员做出不同的方向盘修正动作，所制定的模糊推理规则表1所示：
表1路径纠偏模糊推理规则表
根据表1，即可推理出纠偏量C的隶属度，推理原则为最小值法，即采用最小值法将所述距离偏差和航向偏差隶属于对应预设模糊等级的隶属度的最小值作为对应纠偏量为相应模糊等级的隶属度。例如，当距离偏差Ep为PS(隶属度为0.8)，且航向偏差Ea为NB(隶属度为0.5)，由规则表及最小值法可得纠偏量C属于PS等级的隶属度为0.5(0.5<0.8)。本发明一共49条规则，每条规则都会被执行一次，确定纠偏量C在不同等级下的隶属度。
本发明实施例中，所述获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制，具体包括以下步骤：
根据不同的预设模糊等级对应的权重系数采用加权平均法计算所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值；
根据纠偏量对应的量化因子对所述平均值进行量化，以将所述纠偏量转化为实际纠偏量，并根据所述实际纠偏量进行纠偏控制。
本实施例中，确定纠偏量C在不同等级下的隶属度后，采用加权平均法进行去模糊化，得出精确的纠偏量C，公式如下：
其中，Ci为不同规则下得出的纠偏量C模糊等级所对应的数值，Y(Ci)为纠偏量C在不同等级下的隶属度。
上述距离偏差Ep、航向偏差Ea真实的数值范围并不局限于(-3，3)，因此需要乘以各自的量化因子Kp、Ka，以转化成(-3,3)范围。而纠偏量C同样数值范围并不局限于(-3，3)，需要乘以量化因子Kc转化为实际的输出数值。
本发明实施例，基于模糊控制算法，将上述计算出的距离偏差和航向偏差作为模糊输入变量，经过模糊化、模糊规则推理、解模糊化等一系列计算，得到精确的控制量，输出到执行机构，进行纠偏动作，从而实现AGV的导航路径跟踪。
本发明提供的路径偏差计算方法不仅可以计算直线段路径下的偏差，也可以计算圆弧段路径下的偏差，所计算的偏差包含距离偏差和航向偏差两个变量。同时，本发明提供的纠偏控制算法能接收距离偏差和航向偏差两个变量，经过逻辑判断做出纠偏控制，保证了AGV准确快速实现导航路径跟踪。
对于方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明实施例并不受所描述的动作顺序的限制，因为依据本发明实施例，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作并不一定是本发明实施例所必须的。
图6示意性示出了本发明一个实施例的无轨导航纠偏控制装置的结构示意图。参照图6，本发明实施例的无轨导航纠偏控制装置具体包括获取模块201、模糊化模块202、推理模块203以及控制模块204，其中：
获取模块201，用于根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差。
模糊化模块202，用于根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化，得到所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度。
推理模块203，用于根据预设模糊推理规则推理确定在所述距离偏差和航向偏差隶属于不同的预设模糊等级时对应的纠偏量的模糊等级，并根据所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定所述纠偏量为相应模糊等级的隶属度。具体的，推理模块203，具体用于采用最小值法将所述距离偏差和航向偏差隶属于对应预设模糊等级的隶属度的最小值作为对应纠偏量为相应模糊等级的隶属度。
控制模块204，用于获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制。
本发明实施例中，所述获取模块201，具体用于计算所述当前坐标相对于所述预设导航路径的垂直距离和方向，根据所述垂直距离和方向确定所述运动目标相对于预设导航路径的距离偏差；计算所述预设导航路径的第一水平倾角，根据所述运动目标的当前航向与所述第一水平倾角的角度差确定所述运动目标相对于预设导航路径的航向偏差。
本发明实施例中，所述获取模块201，具体用于计算所述当前坐标与所述预设导航路径的中心坐标之间的距离，根据所述距离与所述预设导航路径的圆弧半径之间的差值以及所述当前坐标相对于所述预设导航路径的方向确定所述运动目标相对于预设导航路径的距离偏差；计算所述中心坐标与所述当前坐标所在直线的第二水平倾角；根据所述第二水平倾角确定所述运动目标在当前坐标对应的目标航向，并根据所述目标航向与所述当前航向的角度差确定所述运动目标相对于预设导航路径的航向偏差。
本发明实施例中，所述装置还包括附图中未示出的第一量化模块，所述第一量化模块，用于在所述模糊化模块根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化之前，根据距离偏差和航向偏差对应的量化因子分别对所述距离偏差和航向偏差进行量化。
本发明实施例中，所述控制模块204，包括计算单元和控制单元，其中：
计算单元，用于根据不同的预设模糊等级对应的权重系数采用加权平均法计算所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值；
控制单元，用于根据纠偏量对应的量化因子对所述平均值进行量化，以将所述纠偏量转化为实际纠偏量，并根据所述实际纠偏量进行纠偏控制。
对于装置实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
本发明实施例提供的无轨导航纠偏控制方法、装置，通过从距离偏差和航向偏差两方面考虑来计算运动目标的当前坐标和当前航向相对于预设导航路径的路径偏差，采用模糊控制算法根据距离偏差和航向偏差得到精确的控制量，并及时纠偏控制，准确快速实现无轨导航车辆的导航路径跟踪。
此外，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上所述方法的步骤。
本实施例中，所述无轨导航纠偏控制装置集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
本发明实施例提供的控制器，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述各个无轨导航纠偏控制方法实施例中的步骤，例如图1所示的S11～S14。或者，所述处理器执行所述计算机程序时实现上述各无轨导航纠偏控制装置实施例中各模块/单元的功能，例如图6所示的获取模块201、模糊化模块202、推理模块203以及控制模块204。
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述无轨导航纠偏控制装置中的执行过程。例如，所述计算机程序可以被分割成获取模块201、模糊化模块202、推理模块203以及控制模块204。
所述控制器可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，本实施例中的控制器可以包括更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述控制器还可以包括输入输出设备、网络接入设备、总线等。
所述处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述控制器的控制中心，利用各种接口和线路连接整个控制器的各个部分。
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述控制器的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(FlashCard)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
本领域的技术人员能够理解，尽管在此的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在下面的权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (13)
1.一种无轨导航纠偏控制方法，其特征在于，所述方法包括：
根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差；
根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化，得到所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度；
根据预设模糊推理规则推理确定在所述距离偏差和航向偏差隶属于不同的预设模糊等级时对应的纠偏量的模糊等级，并根据所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定所述纠偏量为相应模糊等级的隶属度；
获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制。
2.如权利要求1所述的无轨导航纠偏控制方法，其特征在于，当所述预设导航路径为直线时，所述根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差，包括：
计算所述当前坐标相对于所述预设导航路径的垂直距离和方向，根据所述垂直距离和方向确定所述运动目标相对于预设导航路径的距离偏差；
计算所述预设导航路径的第一水平倾角，根据所述运动目标的当前航向与所述第一水平倾角的角度差确定所述运动目标相对于预设导航路径的航向偏差。
3.如权利要求2所述的无轨导航纠偏控制方法，其特征在于，当所述预设导航路径为弧线时，所述根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差，包括：
计算所述当前坐标与所述预设导航路径的中心坐标之间的距离，根据所述距离与所述预设导航路径的圆弧半径之间的差值以及所述当前坐标相对于所述预设导航路径的方向确定所述运动目标相对于预设导航路径的距离偏差；
计算所述中心坐标与所述当前坐标所在直线的第二水平倾角；
根据所述第二水平倾角确定所述运动目标在当前坐标对应的目标航向，根据所述目标航向与所述当前航向的角度差确定所述运动目标相对于预设导航路径的航向偏差。
4.如权利要求1所述的无轨导航纠偏控制方法，其特征在于，在所述根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化之前，所述方法还包括：
根据距离偏差和航向偏差对应的量化因子分别对所述距离偏差和航向偏差进行量化。
5.如权利要求1所述的无轨导航纠偏控制方法，其特征在于，所述根据所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定所述纠偏量为相应模糊等级的隶属度，包括：
采用最小值法将所述距离偏差和航向偏差隶属于对应预设模糊等级的隶属度的最小值作为对应纠偏量为相应模糊等级的隶属度。
6.如权利要求1-4任一项所述的无轨导航纠偏控制方法，其特征在于，所述获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制，包括：
根据不同的预设模糊等级对应的权重系数采用加权平均法计算所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值；
根据纠偏量对应的量化因子对所述平均值进行量化，以将所述纠偏量转化为实际纠偏量，并根据所述实际纠偏量进行纠偏控制。
7.一种无轨导航纠偏控制装置，其特征在于，所述装置包括：
获取模块，用于根据运动目标的当前坐标和当前航向获取所述运动目标相对于预设导航路径的距离偏差和航向偏差；
模糊化模块，用于根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化，得到所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度；
推理模块，用于根据预设模糊推理规则推理确定在所述距离偏差和航向偏差隶属于不同的预设模糊等级时对应的纠偏量的模糊等级，并根据所述距离偏差和航向偏差隶属于不同的预设模糊等级的隶属度确定所述纠偏量为相应模糊等级的隶属度；
控制模块，用于获取所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值，根据所述平均值对运动目标进行纠偏控制。
8.如权利要求7所述的无轨导航纠偏控制装置，其特征在于，所述获取模块，具体用于计算所述当前坐标相对于所述预设导航路径的垂直距离和方向，根据所述垂直距离和方向确定所述运动目标相对于预设导航路径的距离偏差；计算所述预设导航路径的第一水平倾角，根据所述运动目标的当前航向与所述第一水平倾角的角度差确定所述运动目标相对于预设导航路径的航向偏差。
9.如权利要求7所述的无轨导航纠偏控制装置，其特征在于，所述获取模块，具体用于计算所述当前坐标与所述预设导航路径的中心坐标之间的距离，根据所述距离与所述预设导航路径的圆弧半径之间的差值以及所述当前坐标相对于所述预设导航路径的方向确定所述运动目标相对于预设导航路径的距离偏差；计算所述中心坐标与所述当前坐标所在直线的第二水平倾角；根据所述第二水平倾角确定所述运动目标在当前坐标对应的目标航向，并根据所述目标航向与所述当前航向的角度差确定所述运动目标相对于预设导航路径的航向偏差。
10.如权利要求7所述的无轨导航纠偏控制装置，其特征在于，所述装置还包括：
第一量化模块，用于在所述模糊化模块根据预设的隶属度函数对所述距离偏差和航向偏差进行模糊化之前，根据距离偏差和航向偏差对应的量化因子分别对所述距离偏差和航向偏差进行量化。
11.如权利要求7-10任一项所述的无轨导航纠偏控制装置，其特征在于，所述控制模块，包括：
计算单元，用于根据不同的预设模糊等级对应的权重系数采用加权平均法计算所述纠偏量隶属于不同的预设模糊等级的隶属度的平均值；
控制单元，用于根据纠偏量对应的量化因子对所述平均值进行量化，以将所述纠偏量转化为实际纠偏量，并根据所述实际纠偏量进行纠偏控制。
12.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-6任一项所述方法的步骤。
13.一种控制器，其特征在于，存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-6任一项所述方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910632253.3A CN110398963A (zh) | 2019-07-13 | 2019-07-13 | 无轨导航纠偏控制方法、装置、存储介质及控制器 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910632253.3A CN110398963A (zh) | 2019-07-13 | 2019-07-13 | 无轨导航纠偏控制方法、装置、存储介质及控制器 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110398963A true CN110398963A (zh) | 2019-11-01 |
Family
ID=68325401
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910632253.3A Pending CN110398963A (zh) | 2019-07-13 | 2019-07-13 | 无轨导航纠偏控制方法、装置、存储介质及控制器 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110398963A (zh) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111123930A (zh) * | 2019-12-23 | 2020-05-08 | 北京星航机电装备有限公司 | 一种产品转运agv视觉导航控制方法 |
| CN111338353A (zh) * | 2020-03-26 | 2020-06-26 | 吉林大学 | 一种动态驾驶环境下智能车辆变道轨迹规划方法 |
| CN111650936A (zh) * | 2020-06-03 | 2020-09-11 | 杭州迦智科技有限公司 | 一种伺服控制方法、处理器、存储介质和可移动平台 |
| CN112947487A (zh) * | 2021-03-31 | 2021-06-11 | 深圳市优必选科技股份有限公司 | 一种自动引导车辆及其曲线路径跟踪方法、控制装置 |
| CN113093722A (zh) * | 2021-02-20 | 2021-07-09 | 意欧斯物流科技(上海)有限公司 | 一种agv智能路径规划及模糊控制方法 |
| CN113252040A (zh) * | 2021-05-08 | 2021-08-13 | 云南财经大学 | 一种改进的agv小车二维码弧线导航方法 |
| CN114137988A (zh) * | 2021-12-01 | 2022-03-04 | 山东新坐标智能装备有限公司 | 机器人路径导航纠偏方法、系统、及存储介质 |
| CN114460931A (zh) * | 2021-12-23 | 2022-05-10 | 广东嘉腾机器人自动化有限公司 | 色带导航纠偏方法、存储介质和电子设备 |
| CN114721375A (zh) * | 2022-03-14 | 2022-07-08 | 常州江苏大学工程技术研究院 | 一种农机单天线导航路径跟踪方法 |
| CN117093001A (zh) * | 2023-08-25 | 2023-11-21 | 杭州士腾科技有限公司 | 自动导向车的纠偏方法及系统 |
| CN117093001B (zh) * | 2023-08-25 | 2024-05-07 | 杭州士腾科技有限公司 | 自动导向车的纠偏方法及系统 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101630162A (zh) * | 2008-07-16 | 2010-01-20 | 中国科学院自动化研究所 | 多移动机器人局部跟随控制方法 |
| CN104765368A (zh) * | 2015-04-10 | 2015-07-08 | 中国人民解放军海军工程大学 | 一种基于模糊自适应算法的无人艇航向航速协同控制方法 |
| CN105867377A (zh) * | 2016-04-13 | 2016-08-17 | 浙江理工大学 | 一种农业机械自动导航控制方法 |
| CN106708068A (zh) * | 2017-01-19 | 2017-05-24 | 中国科学院自动化研究所 | 仿生波动鳍推进水下航行器路径跟踪控制方法 |
| CN108021135A (zh) * | 2017-12-05 | 2018-05-11 | 合肥泰禾光电科技股份有限公司 | 一种自动导引车的控制方法和装置 |
| CN108548544A (zh) * | 2018-03-30 | 2018-09-18 | 济南大学 | 一种现代物流中agv小车的导航模糊控制方法 |
-
2019
- 2019-07-13 CN CN201910632253.3A patent/CN110398963A/zh active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101630162A (zh) * | 2008-07-16 | 2010-01-20 | 中国科学院自动化研究所 | 多移动机器人局部跟随控制方法 |
| CN104765368A (zh) * | 2015-04-10 | 2015-07-08 | 中国人民解放军海军工程大学 | 一种基于模糊自适应算法的无人艇航向航速协同控制方法 |
| CN105867377A (zh) * | 2016-04-13 | 2016-08-17 | 浙江理工大学 | 一种农业机械自动导航控制方法 |
| CN106708068A (zh) * | 2017-01-19 | 2017-05-24 | 中国科学院自动化研究所 | 仿生波动鳍推进水下航行器路径跟踪控制方法 |
| CN108021135A (zh) * | 2017-12-05 | 2018-05-11 | 合肥泰禾光电科技股份有限公司 | 一种自动导引车的控制方法和装置 |
| CN108548544A (zh) * | 2018-03-30 | 2018-09-18 | 济南大学 | 一种现代物流中agv小车的导航模糊控制方法 |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111123930A (zh) * | 2019-12-23 | 2020-05-08 | 北京星航机电装备有限公司 | 一种产品转运agv视觉导航控制方法 |
| CN111338353A (zh) * | 2020-03-26 | 2020-06-26 | 吉林大学 | 一种动态驾驶环境下智能车辆变道轨迹规划方法 |
| WO2021244598A1 (zh) * | 2020-06-03 | 2021-12-09 | 杭州迦智科技有限公司 | 伺服控制方法、处理器、存储介质和可移动平台 |
| CN111650936A (zh) * | 2020-06-03 | 2020-09-11 | 杭州迦智科技有限公司 | 一种伺服控制方法、处理器、存储介质和可移动平台 |
| CN113093722A (zh) * | 2021-02-20 | 2021-07-09 | 意欧斯物流科技(上海)有限公司 | 一种agv智能路径规划及模糊控制方法 |
| CN112947487A (zh) * | 2021-03-31 | 2021-06-11 | 深圳市优必选科技股份有限公司 | 一种自动引导车辆及其曲线路径跟踪方法、控制装置 |
| CN113252040A (zh) * | 2021-05-08 | 2021-08-13 | 云南财经大学 | 一种改进的agv小车二维码弧线导航方法 |
| CN114137988A (zh) * | 2021-12-01 | 2022-03-04 | 山东新坐标智能装备有限公司 | 机器人路径导航纠偏方法、系统、及存储介质 |
| CN114460931A (zh) * | 2021-12-23 | 2022-05-10 | 广东嘉腾机器人自动化有限公司 | 色带导航纠偏方法、存储介质和电子设备 |
| CN114721375A (zh) * | 2022-03-14 | 2022-07-08 | 常州江苏大学工程技术研究院 | 一种农机单天线导航路径跟踪方法 |
| CN114721375B (zh) * | 2022-03-14 | 2024-03-26 | 常州江苏大学工程技术研究院 | 一种农机单天线导航路径跟踪方法 |
| CN117093001A (zh) * | 2023-08-25 | 2023-11-21 | 杭州士腾科技有限公司 | 自动导向车的纠偏方法及系统 |
| CN117093001B (zh) * | 2023-08-25 | 2024-05-07 | 杭州士腾科技有限公司 | 自动导向车的纠偏方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110398963A (zh) | 无轨导航纠偏控制方法、装置、存储介质及控制器 | |
| Li et al. | Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles | |
| Brown et al. | Coordinating tire forces to avoid obstacles using nonlinear model predictive control | |
| CN110789528B (zh) | 一种车辆行驶轨迹预测方法、装置、设备及存储介质 | |
| Ye et al. | Linear model predictive control of automatic parking path tracking with soft constraints | |
| US10114378B2 (en) | Automatic guidance system with stability control for an agricultural vehicle | |
| WO2019184179A1 (zh) | 控制自主式移动机器移动的方法、装置、机器及存储介质 | |
| CN108791289A (zh) | 一种车辆控制方法和装置 | |
| CN103204162A (zh) | 具有有效后转向的车道跟踪系统 | |
| CN113415276A (zh) | 一种智能驾驶预瞄控制方法、装置和存储介质 | |
| CN111795699B (zh) | 无人车的路径规划方法、装置和计算机可读存储介质 | |
| CN107031741B (zh) | 车体位姿校正方法及装置 | |
| Song et al. | A vehicle trajectory tracking method with a time-varying model based on the model predictive control | |
| CN113515038A (zh) | 一种车辆换道方法、装置、设备及可读存储介质 | |
| CN110316249A (zh) | 横向控制方法及装置 | |
| Peicheng et al. | Intelligent vehicle path tracking control based on improved MPC and hybrid PID | |
| CN114637292A (zh) | 一种兼顾避障的车辆轨迹跟踪鲁棒控制方法和系统 | |
| CN113830174B (zh) | 一种转向角度修正方法、装置、设备及可读存储介质 | |
| CN115309170A (zh) | 一种考虑舒适性约束的轨迹规划方法、装置和系统 | |
| CN114012732B (zh) | 一种机器人控制方法、装置、设备和介质 | |
| CN110083158B (zh) | 一种确定局部规划路径的方法和设备 | |
| CN112925323B (zh) | 一种基于规则的移动机器人速度调节方法及系统 | |
| Kang et al. | Lateral control for autonomous lane keeping system on highways | |
| CN110968082A (zh) | 一种自动驾驶车辆路径追踪方法及装置 | |
| Björnberg | Shared control for vehicle teleoperation with a virtual environment interface |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20191101 |