技术领域
[0001] 本
发明涉及一种自动导航小车技术领域，尤其涉及一种自动导航小车自动纠偏方法。
背景技术
[0002] 自动导航小车主要用来在工业、物流、仓储等领域代替人类进行一些重复性强或劳动量大的工作。像工业生产中的上下料、重物搬运，以及物流行业的智能仓储、自动转运等工作，都在广泛应用自动导航小车。虽然应用场景不同，但它们对于自动导航小车运动的精确性、快速性和连续性等性能有比较高的需求。在比较传统的应用中，往往采用磁轨式导航方案，因为这种导航方式稳定可靠、便于纠偏，但这种导航方式固定死板、易于磨损，对于仓库的利用率也不高，因此在更灵活多样、空间有限的环境中会采用更新颖的点阵式导航方案。
[0003] 点阵式导航的自动导航小车通常会采用二维码导航的方案，这种方案会采用码枪作为
传感器，在二维码标识点上码枪可以识别出自动导航小车的
位置和
角度。此外在点与点之间，点阵式自动导航小车需要采用
陀螺仪作为过程中角度导航的传感器，以保证过程中行进方向的正确。
[0004] 但由于机械结构上的误差、
电机加减速过程的不同、左右
车轮地面情况的不一致，都会使得自动导航小车在运动过程中偏离自己原有的轨道，如果不能及时将其纠正，则会使自动导航小车位置丢失、自动运行中断，甚至是与其他物体碰撞。因此对于点阵式自动导航小车偏差纠正是保证其连续自动运行的重要条件。
[0005] 传统的自动导航小车纠偏方法会采用PID控制，但这一方法调参时间长、抗干扰能
力差，且在运动纠偏时不能确定调整距离的长短，可能会使自动导航小车错过标识点，从而导致行驶逻辑错误。此外，由于没有利用自动导航小车自身的运动学特性，因此大多数PID调整方法都会存在振荡和超调，这会使得行驶
稳定性下降且影响运行效率。
发明内容
[0006] 为了解决以上技术问题，本发明提供了一种自动导航小车自动纠偏方法。
[0007] 本发明所解决的技术问题可以采用以下技术方案来实现：
[0008] 一种自动导航车自动纠偏方法，其特征在于，包括：
[0009] 步骤S1：所述自动导航车在运动过程中通过传感器检测当前位置相较于标识点的角度偏差和距离偏差；
[0010] 步骤S2：获取所述自动导航车的运动参数，根据所述运动参数和所述步骤S1读取到的所述角度偏差和所述距离偏差，采用双变量同时调整方法进行计算各个运动阶段的运动时间，当所述运动时间存在不合理项，执行步骤S4，否则执行步骤S3；
[0011] 步骤S3：计算出所述自动导航车的各个运动阶段的运动关键参数，得到规划的路径轨迹，执行步骤S6；
[0012] 步骤S4：采用单变量分段调整方法计算，根据偏差角度、距离及运动速度计算出分段系数，并根据所述分段系数将预计运动距离划分为一角度调整段与一距离调整段；
[0013] 步骤S5：根据所述步骤S4的所述角度调整段和所述距离调整段，求出所述自动导航车的运动关键参数，得到规划的路径轨迹，执行步骤S6；
[0014] 步骤S6：根据所述运动关键参数，逐周期下发所述自动导航车的左轮速及右轮速的控制命令，控制所述自动导航车沿规划的路径轨迹运动，并调整偏差。
[0015] 优选的，所述步骤S1中，所述传感器为陀螺仪或码枪或具有二维码识别功能的
照相机。
[0016] 优选的，所述步骤S3中，采用的所述双变量同时调整方法，包括第一子模式、第二子模式以及第三子模式，判断依据为系数β，β计算方法如下：
[0017]
[0018] 其中，θ为所述自动导航车的偏移角度，单位为弧度，v为所述自动导航车的运动速度，单位为m/s，d为所述自动导航车的偏移目标轨迹距离，单位为m；
[0019] 所述第三子模式的偏差角度小于所述第一子模式的偏差角度大于所述第一子模式的偏差角度；所述第三子模式的偏差距离大于第一子模式的偏差距离小于第二子模式的偏差距离
[0020] 优选的，所述步骤S4中，采用的所述单变量分段调整方法，包含有分段系数α，所述分段系数α计算方法如下：
[0021]
[0022]
[0023]
[0024] 其中，m,K均为计算过程中的参数；θ为所述自动导航车偏移角度，单位为弧度；v为所述自动导航车运动速度，单位为m/s；d为所述自动导航车偏移目标轨迹距离，单位为m；s为设定的所述自动导航车调整距离，单位为m；α为最终求出的分段系数。
[0025] 优选的，所述第一子模式、所述第二子模式以及所述第三子模式均包含
角速度匀
加速、角速度匀减速、匀速、反向角速度匀加速及反向角速度匀减速五个阶段。
[0026] 一种自动导航车自动纠偏系统，其特征在于，包括：
[0027] 自动导航车
控制器，包括运动规划模
块、偏差
采样模块以及运动控
制模块；
[0028] 位置和角度传感器，与所述自动导航车控制器通过通信总线连接；
[0029] 电机，连接所述自动导航车控制器。
[0030] 优选的，所述偏差采样模块通过与所述码枪或所述陀螺仪通信获取在二维码标识点上的位置和角度，经过平滑滤波后与标准值比较获得所述自动导航车当前的角度和距离偏差。
[0031] 优选的，所述运动规划模块将偏差值、当前速度值、设定的所述自动导航车调整距离作为输入，对纠偏进行运动规划。
[0032] 优选的，所述运动
控制模块根据所述运动规划模块输入的运动参数开始运动，将所述自动导航车在各个时刻的位置、速度、角速度、
角加速度以输出所述电机转速命令的方式输出。
[0033] 优选的，当设定调整距离运动完成后，所述运动控制模块恢复为所述陀螺仪角度PID控制模式，当再次通过识别二维码得到偏差角度和距离时，会再次调所述用运动规划模块对所述自动导航车进行新的一次运动调整；当有暂停、停止命令输入，会终止当前运动，并保存当前运动状态参数，等待新的运动命令输入。
[0034] 其有益效果在于：本发明提供的控制方法不仅可以综合调整距离和方向偏差，并且可以保证在
指定距离内完成调整，还可大大节省PID调整方法调整参数的时间，为自动导航小车的偏差调整提供了一种准确、高效、可靠的控制方法。
附图说明
[0035] 图1为本发明提供的一种自动导航小车自动纠偏方法步骤图；
[0036] 图2为本发明
实施例中采用单变量分段调整方法得到的运动轨迹图；
[0037] 图3为本发明实施例中采用单变量分段调整方法获取的角速度变化情况图；
[0038] 图4为本发明实施例中采用双变量同时调整方法得到的运动轨迹图；
[0039] 图5为本发明实施例中采用双变量同时调整方法获取的角速度变化情况图；
[0040] 图6位本发明提供的一种自动导航车自动纠偏系统。
具体实施方式
[0041] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0042] 需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
[0043] 下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。
[0044] 参照图1为本发明提供的一种自动导航小车自动纠偏方法步骤图，包括：
[0045] 步骤S1：所述自动导航车在运动过程中通过传感器检测当前位置相较于标识点的角度偏差和距离偏差；
[0046] 步骤S2：获取所述自动导航车的运动参数，根据所述运动参数和所述步骤S1读取到的所述角度偏差和所述距离偏差，采用双变量同时调整方法进行计算各个运动阶段的运动时间，当所述运动时间存在不合理项，执行步骤S4，否则执行步骤S3；
[0047] 步骤S3：计算出所述自动导航车的各个运动阶段的运动关键参数，得到规划的路径轨迹，执行步骤S6；
[0048] 步骤S4：采用单变量分段调整方法计算，根据偏差角度、距离及运动速度计算出分段系数，并根据所述分段系数将预计运动距离划分为一角度调整段与一距离调整段；
[0049] 步骤S5：根据所述步骤S4的所述角度调整段和所述距离调整段，求出所述自动导航车的运动关键参数，得到规划的路径轨迹，执行步骤S6；
[0050] 步骤S6：根据所述运动关键参数，逐周期下发所述自动导航车的左轮速及右轮速的控制命令，控制所述自动导航车沿规划的路径轨迹运动，并调整偏差。
[0051] 进一步地，步骤S1中，传感器为陀螺仪或码枪或具有二维码识别功能的照相机。
[0052] 具体的，传感器为包括但不限于陀螺仪或码枪或具有二维码识别功能的照相机等。
[0053] 进一步地，步骤S3中，采用的双变量同时调整方法，包括第一子模式、第二子模式以及第三子模式，判断依据为系数β，β计算方法如下：
[0054]
[0055] 其中，θ为自动导航车的偏移角度，单位为弧度，v为自动导航车的运动速度，单位为m/s，d为自动导航车的偏移目标轨迹距离，单位为m；
[0056] 具体的，双变量同时调整方法根据偏差的情况不同会被分为三种情况：第一子模式、第二子模式以及第三子模式，判定条件为系数β，采用公式(1)计算得到。这三
种子状态对应不同的运动学公式，但都包含以下五个阶段：角速度匀加速、角速度匀减速、匀速、反向角速度匀加速及反向角速度匀减速，第二子模式角度小于第一模式角度大于第三子模式角度；第二子模式距离大于第一模式距离大于第三子模式距离。不同子状态对应的各运动阶段的时间比例不同，但均保证在设定的小车调整距离到达时偏差角度和偏差距离同时为零。
[0057] 进一步地，所述步骤S4中，采用的所述单变量分段调整方法，包含有分段系数α，所述分段系数α计算方法如下：
[0058]
[0059]
[0060]
[0061] 其中，m,K均为计算过程中的参数；θ为所述自动导航小车偏移角度，单位为弧度；v为所述自动导航小车运动速度，单位为m/s；d为所述自动导航小车偏移目标轨迹距离，单位为m；s为设定的所述自动导航小车调整距离，单位为m；α为最终求出的分段系数。
[0062] 具体的，单变量分段调整方法将先进行角度方向调整，之后进行距离方向调整，相较于双变量同时调整方法，单变量分段调整方法的连贯性和运动平稳性会略微受影响，但计算更为简单和稳定，可保证对合理的偏差调整情况稳定有解。通过式(4)可以计算分段系数，根据这一系数可将设定的运动距离进行合理划分，尽量保证两段运动的加速度相同，从而更好地保证了两段运动的均匀和平稳。
[0063] 进一步地，第一子模式、第二子模式以及第三子模式均包含角速度匀加速、角速度匀减速、匀速、反向角速度匀加速及反向角速度匀减速五个阶段。
[0064] 在本发明较佳的实施例中，由自动导航小车控制器从通信总线获取码枪传感器和陀螺仪数据，经过均值滤波后得到当前位置相较于标识点的偏差角度和偏差距离，将以上数据及车辆运行速度、设定调整距离作为输入值，输入给控制器中的运动规划模块，求解关键运动参数。
[0065] 通过式(1)计算双变量同时调整方法的判定系数β，根据系数β所处的限定区间，判定现有情况处于的子模式状态，再依据子模式状态求出角速度匀加速、角速度匀减速、匀速、反向角速度匀加速及反向角速度匀减速等五个运动阶段的运行时间，当求解时间中有不合理项，则表明双变量同时调整规划不成功，需要采用单变量分段调整方法规划。当时间解均正确，则进一步计算各个运动阶段的角加速度、角速度变化情况，最终求出规划的路程和轨迹。
[0066] 当采用单变量分段调整方法规划，则需要通过式(4)先计算分段系数α，之后将设定调整距离分为两段，第一段进行角度调整，第二段进行距离调整。在每一段运动中，也都包含角速度匀加速、角速度匀减速、匀速、反向角速度匀加速及反向角速度匀减速等五个阶段，分别计算他们占用的时间和角加速度、角速度变化情况，之后保存关键运动参数。
[0067] 根据运动参数，自动导航小车控制器每周期计算当周期运动所对应的角速度，并根据自动导航小车结构参数将其转化为左右轮速差，将其补偿在现有车速
基础上，即可实现符合设定加减速变化规律的运动，并能在运动过程中保持连贯性和快速性。
[0068] 当运动过程中有新的偏移距离和偏移角度数据传入控制器时，系统将重新规划运动。如果在调整阶段运动完成后，仍无新偏移数据输入，则结束当前控制方法，转用偏角PID控制保持自动导航小车的运动方向，直到重新扫码获取偏移距离和角度，再开始进行新一次调整。
[0069] 参照图2为本发明一实施例，图示为采用单变量分段调整方法得到的运动轨迹；图中的点表示每个离散周期小车所在的位置，横坐标表示小车前进方向，纵坐标表示偏移方向。
[0070] 参照图3为本发明一实施例，图示采用单变量分段调整方法获取的角速度变化情况图；其中横坐标为时间，纵坐标为小车角速度。
[0071] 图4为本发明一实施例，图示采用双变量同时调整方法得到的运动轨迹图；图中的点表示每个离散周期小车所在的位置，横坐标表示小车前进方向，纵坐标表示偏移方向。
[0072] 图5为本发明一实施例，图示采用双变量同时调整方法获取的角速度变化情况图；其中横坐标为时间，纵坐标为小车角速度。
[0073] 参照图6位本发明提供的一种自动导航车自动纠偏系统，包括：
[0074] 自动导航车控制器1，包括运动规划模块4、偏差采样模块5以及运动控制模块6；
[0075] 位置和角度传感器2，与自动导航车控制器1通过通信总线连接；
[0076] 电机3，连接自动导航车控制器。
[0077] 进一步地，偏差采样模块5通过与码枪或陀螺仪通信获取在二维码标识点上的位置和角度，经过平滑滤波后与标准值比较获得自动导航车当前的角度和距离偏差。
[0078] 进一步地，运动规划模块4将偏差值、当前速度值、设定的自动导航车调整距离作为输入，对纠偏进行运动规划。
[0079] 进一步地，运动控制模块6根据运动规划模块输入的运动参数开始运动，将自动导航车在各个时刻的位置、速度、角速度、角加速度以输出电机3转速命令的方式输出。
[0080] 进一步地，当设定调整距离运动完成后，运动控制模块6恢复为陀螺仪角度PID控制模式，当再次通过识别二维码得到偏差角度和距离时，会再次调用运动规划模块4对自动导航车进行新的一次运动调整；当有暂停、停止命令输入，会终止当前运动，并保存当前运动状态参数，等待新的运动命令输入。
[0081] 以上所述仅为本发明较佳的实施例，并非因此限制本发明的实施方式及保护范围，对于本领域技术人员而言，应当能够意识到凡运用本发明
说明书及图示内容所作出的等同替换和显而易见的变化所得到的方案，均应当包含在本发明的保护范围内。
