CN112925327B - 一种二维码惯导导航控制算法 - Google Patents

一种二维码惯导导航控制算法 Download PDF

Info

Publication number
CN112925327B
CN112925327B CN202110133259.3A CN202110133259A CN112925327B CN 112925327 B CN112925327 B CN 112925327B CN 202110133259 A CN202110133259 A CN 202110133259A CN 112925327 B CN112925327 B CN 112925327B
Authority
CN
China
Prior art keywords
agv
angle
dimensional code
inertial navigation
pid
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.)
Active
Application number
CN202110133259.3A
Other languages
English (en)
Other versions
CN112925327A (zh
Inventor
陈检
李蜜
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Gaobuda Intelligent Technology Co ltd
Original Assignee
Shenzhen Gaobuda Intelligent Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen Gaobuda Intelligent Technology Co ltd filed Critical Shenzhen Gaobuda Intelligent Technology Co ltd
Priority to CN202110133259.3A priority Critical patent/CN112925327B/zh
Publication of CN112925327A publication Critical patent/CN112925327A/zh
Application granted granted Critical
Publication of CN112925327B publication Critical patent/CN112925327B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
    • G05D1/0236—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Optics & Photonics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种二维码惯导导航控制算法，包括第一角度环控制器、第二角度环控制器以及IMU惯导模块；第一角度环控制器用于调整AGV行驶的大方向，即调整AGV的行驶方向与AGV的目标角度和二维码的相对角度一致；第二角度环控制器用于调整AGV行驶的小方向，即调整AGV在行驶过程中距离二维码的中心位置为0；IMU惯导模块用于反馈AGV的当前航向角；本发明在双PID算法的基础上增加了IMU惯导模块用于反馈当前航向角，通过计算出目标航向角与IMU惯导模块反馈的实际航向角进行精准对比，控制执行机构以满足二者航向角一致，从而实现闭环控制，保证了AGV精准无误到达指定位置，解决了现有的二维码导航算法容易导致系统不稳定且控制精度误差大的问题。

Description

一种二维码惯导导航控制算法
技术领域
本发明涉及二维码导航AGV技术领域，具体涉及一种二维码惯导导航控制算法。
背景技术
二维码导航AGV的出现大大减少了人工劳动力；间歇性贴上二维码，可以在一定程度上避免AGV轮子碾压二维码导致二维码损坏，造成失控,因此得到了广大客户的一致认可。在应用现场环境中，能够清晰地看到二维码导航AGV沿着二维码路径实现前进、后退、原地旋转、加速、减速、遇到障碍物自主减速停车等动作。
二维码导航AG中，需要控制好航向角，以使AGV精准的停在指定的目标二维码上面。如图1所示，两轮差速车体的物理数学方程如下：
由公式3可得：
代入公式1、2、3可得：
其中，υleft为两轮差速车体的左轮线速度，υright为两轮差速车体的右轮线速度，ω为两轮差速车体的角速度，R为两轮差速车体的运动半径，b为两轮差速车体的左轮和右轮的间距，根据公式5可知，若想改变AGV车体的航向角，只需要调节两个轮子的速度差，就能够实现AGV车体的航向角的改变，从而达到控制AGV行驶方向的目的。
发明内容
针对目前的二维码导航算法，容易导致控制系统不稳定且控制精度误差大的问题，本发明的目的是提供一种二维码惯导导航控制算法，在双PID算法的基础上增加了IMU惯导模块用于反馈当前航向角，通过计算出目标航向角与IMU惯导模块反馈的实际航向角进行精准对比，控制执行机构以满足二者航向角一致，从而实现闭环控制，保证了AGV精准无误到达指定位置，解决了现有的二维码导航算法容易导致系统不稳定且控制精度误差大的问题。
为了达到上述目的，本发明所采用的技术方案是：提供一种二维码惯导导航控制算法，用于AGV沿着预规划的二维码的路径行驶，所述AGV的车体类型为双轮差速车，包括左轮和右轮，所述AGV的执行机构为差速电机；
所述AGV的控制点定义为SO，所述SO为所述左轮和所述右轮的连线的中点，所述SO的垂直线指向所述AGV的车头方向，所述AGV的车头方向为所述SO的角度；所述SO在全局规划地图中，多个所述二维码以一定的方向铺开，每个所述二维码之间的间距可以不同，所述AGV经过所述二维码产生的相机拍摄的偏差信息定义为：x、y、θ，所述x为所述相机拍摄的所述AGV偏离所述二维码中心点x轴的偏差值，所述y为所述相机拍摄的所述AGV偏离所述二维码中心点y轴的偏差值，所述θ为所述相机拍摄的所述AGV与所述二维码的相对角度；所述左轮的线速度定义为υleft，所述右轮的线速度定义为υright，所述AGV的目标航向角定义为α，所述AGV的理论航向角定义为β；
所述二维码惯导导航控制算法包括第一角度环控制器、第二角度环控制器以及IMU惯导模块；所述第一角度环控制器用于调整所述AGV行驶的大方向，即调整所述AGV的行驶方向与所述AGV的目标角度和所述二维码的相对角度一致；所述第二角度环控制器用于调整所述AGV行驶的小方向，即调整所述AGV的当前航向角与所述二维码的中心位置偏差为0；所述IMU惯导模块用于反馈所述AGV的当前航向角；所述二维码惯导导航控制算法通过计算所述目标航向角与所述IMU惯导模块反馈的当前航向角进行精确对比，控制所述执行机构通过调整所述左轮的线速度和所述右轮的线速度以使所述当前航向角与所述目标航向角一致，从而通过实现闭环控制保证所述AGV到达指定位置；
所述第一角度环控制器的PID目标值定位为SVH0，所述第一角度环控制器的PID输入值定位为PVH0，所述第一角度环控制器的PID输出值定义为MVH0，所述第一角度环控制器的PID比例参数定义为PH0，所述第一角度环控制器的PID积分参数定义为IH0，所述第一角度环控制器的PID微分参数为DH0；所述第二角度环控制器的PID目标值定位为SVH1，所述第二角度环控制器的PID输入值定位为PVH1，所述第二角度环控制器的PID输出值定义为MVH1，所述第二角度环控制器的PID比例参数定义为PH1，所述第二角度环控制器的PID积分参数定义为IH1，所述第二角度环控制器的PID微分参数为DH1；
所述SVH0和所述PVH0的偏差作为所述第一角度环控制器的输入值，经过所述PH0、所述IH0以及所述DH0的PID计算得出所述MVH0，所述MVH0与所述β之和作为所述SVH1的值，所述SVH1和所述PVH1的偏差作为所述第二角度环控制器的输入值，经过所述PH1、所述IH1以及所述DH1的PID计算得出所述MVH1，则所述MVH1与所述υleft之和为控制所述左轮的线速度，所述MVH1与所述υright之和为控制所述右轮的线速度。
进一步地，所述IMU惯导模块采用3轴MEMS陀螺仪芯片、3轴MEMS加速度计芯片、3轴MEMS电子罗盘芯片以及MEMS气压高度计芯片组成，所述IMU惯导模块反馈的当前航向角定位为IMUθ。
进一步地，所述AGV的车体角度与规划好的所述二维码的方向呈固定角度，所述固定角度的范围为0°～360°；所述SO的距离与所述二维码的中心距离为零。
进一步地，所述二维码惯导导航控制算法包括以下步骤：
S1：设所述SVH0＝α，所述PVH0＝θ；
S2：所述SVH0与所述PVH0的偏差经过所述PH0、所述IH0和所述DH0进行调整计算，得到所述MVH0；
S3:令所述SVH1＝β+MVH0，PVH1＝IMUθ；
S4：所述SVH1与所述PVH1的偏差经过所述PH1、所述IH1和所述DH1进行调整计算，得到MVH1；
S5：则得到控制所述左轮的线速度为υleft+MVH1，控制所述右轮的线速度为υright+MVH1。
进一步地，所述AGV经过所述二维码产生的实际偏差信息为：x＇、y＇、θ＇，所述x＇为所述AGV偏离所述二维码中心点x轴的实际偏差值，所述y＇为所述AGV偏离所述二维码中心点y轴的实际偏差值，所述θ＇为所述AGV与所述二维码的实际相对角度，则：
进一步地，所述AGV将要经过的两个所述二维码之间的距离为d，则：
β＝tan-1(y′/d+x′)。
进一步地，所述AGV每经过一个所述二维码，将会对所述IMU惯导模块的当前惯导数据进行清零，以简化所述二维码惯导导航控制算法。
进一步地，其特征在于，
SVH1＝β+MVH0＝tan-1(y′/d+x′)+MVH0
＝tan-1((x×cosα-y×sinα)/(d+(x×sinα+y×cosα)))+MVH0。
进一步地，在所述AGV行驶的全程中，所述AGV每经过一个所述二维码，就通过所述二维码惯导导航控制算法调整一次所述左轮的线速度和所述右轮的线速度，这样通过多次有效调节，所述AGV最终满足指定的停止条件并且实现精确停止在指定位置上。
进一步地，所述二维码惯导导航控制算法通过控制所述左轮的线速度和所述右轮的线速度，以实现所述AGV的车体的控制航向角的改变，当所述IMU惯导模块反馈的航向角与所述控制航向角一致时，所述左轮和所述右轮保持当前速度驶往下一个二维码，从而在所述AGV经过所述下一个二维码时，控制所述AGV的车体尽可能的靠近所述下一个二维码的中心位置，并且所述AGV的车头角度与所述AGV的目标角度一致。
与现有技术相比，本发明提供的一种二维码惯导导航控制算法，包括第一角度环控制器、第二角度环控制器以及IMU惯导模块；第一角度环控制器用于调整AGV行驶的大方向，即调整AGV的行驶方向与AGV的目标角度和二维码的相对角度一致；第二角度环控制器用于调整AGV行驶的小方向，即调整AGV在行驶过程中距离二维码的中心位置为0；IMU惯导模块用于反馈AGV的当前航向角；本发明在双PID算法的基础上增加了IMU惯导模块用于反馈当前航向角，通过计算出目标航向角与IMU惯导模块反馈的实际航向角进行精准对比，控制执行机构以满足二者航向角一致，从而实现闭环控制，保证了AGV精准无误到达指定位置，解决了现有的二维码导航算法容易导致系统不稳定且控制精度误差大的问题。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是两轮差速车体的物理数学方程示意图。
图2是本发明实施例提供的一种二维码惯导导航控制算法的算法原理图。
图3是本发明实施例提供的一种二维码惯导导航控制算法的算法步骤流程图。
图4是本发明实施例提供的一种二维码惯导导航控制算法的AGV经过二维码时的信息解析图。
图5是本发明实施例提供的一种二维码惯导导航控制算法的AGV航向角理论图。
图6是本发明实施例提供的一种二维码惯导导航控制算法的控制流程图。
图7是本发明实施例提供的一种二维码惯导导航控制算法的全局地图示意图。
上述附图中的标记为：1、AGV；11、第一角度环控制器；12、第二角度环控制器；101、二维码。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要说明的是，当元件被称为“固定于”另一个元件，它可以直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件需要理解的是，若有术语“上”、“下”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
以下结合附图与具体实施例，对本发明的技术方案做详细的说明。
参照图2至图7，为本发明的较佳实施例。
参照图2和图5，本发明提供的一种二维码惯导导航控制算法，用于AGV1沿着预规划的二维码101的路径行驶，AGV1的车体类型为双轮差速车，包括左轮和右轮，AGV1的执行机构为差速电机；
AGV1的控制点定义为SO，SO为左轮和右轮的连线的中点，SO的垂直线指向AGV1的车头方向，AGV1的车头方向为SO的角度；SO在全局规划地图中，多个二维码101以一定的方向铺开，每个二维码101之间的间距可以不同，AGV1经过二维码101产生的相机拍摄的偏差信息定义为：x、y、θ，x为相机拍摄的AGV1偏离二维码101中心点x轴的偏差值，y为相机拍摄的AGV1偏离二维码101中心点y轴的偏差值，θ为相机拍摄的AGV1与二维码101的相对角度；左轮的线速度定义为υleft，右轮的线速度定义为υright，AGV1的目标航向角定义为α，AGV1的理论航向角定义为β；
二维码惯导导航控制算法包括第一角度环控制器11、第二角度环控制器12以及IMU惯导模块；第一角度环控制器11用于调整AGV1行驶的大方向，即调整AGV1的行驶方向与AGV1的目标角度和二维码101的相对角度一致；第二角度环控制器12用于调整AGV1行驶的小方向，即调整AGV1在行驶过程中距离二维码101的中心位置为0；IMU惯导模块用于反馈AGV1的当前航向角；二维码惯导导航控制算法通过计算目标航向角与IMU惯导模块反馈的当前航向角进行精确对比，控制执行机构通过调整左轮的线速度和右轮的线速度以使当前航向角与目标航向角一致，从而通过实现闭环控制保证AGV1到达指定位置；
第一角度环控制器11的PID目标值定位为SVH0，第一角度环控制器11的PID输入值定位为PVH0，第一角度环控制器11的PID输出值定义为MVH0，第一角度环控制器11的PID比例参数定义为PH0，第一角度环控制器11的PID积分参数定义为IH0，第一角度环控制器11的PID微分参数为DH0；第二角度环控制器12的PID目标值定位为SVH1，第二角度环控制器12的PID输入值定位为PVH1，第二角度环控制器12的PID输出值定义为MVH1，第二角度环控制器12的PID比例参数定义为PH1，第二角度环控制器12的PID积分参数定义为IH1，第二角度环控制器12的PID微分参数为DH1；
SVH0和PVH0的偏差作为第一角度环控制器11的输入值，经过PH0、IH0以及DH0的PID计算得出MVH0，MVH0与β之和作为SVH1的值，SVH1和PVH1的偏差作为第二角度环控制器12的输入值，经过PH1、IH1以及DH1的PID计算得出MVH1，则MVH1与υleft之和为控制左轮的线速度，MVH1与υright之和为控制右轮的线速度。
上述提供的技术方案，包括第一角度环控制器11、第二角度环控制器12以及IMU惯导模块；第一角度环控制器11用于调整AGV1行驶的大方向，即调整AGV1的行驶方向与AGV1的目标角度和二维码101的相对角度一致；第二角度环控制器12用于调整AGV1行驶的小方向，即调整AGV1在行驶过程中距离二维码101的中心位置为0；IMU惯导模块用于反馈AGV1的当前航向角；本发明在双PID算法的基础上增加了IMU惯导模块用于反馈当前航向角，通过计算出目标航向角与IMU惯导模块反馈的实际航向角进行精准对比，控制执行机构以满足二者航向角一致，从而实现闭环控制，保证了AGV1精准无误到达指定位置，解决了现有的二维码101导航算法容易导致系统不稳定且控制精度误差大的问题。
具体地，IMU惯导模块采用3轴MEMS陀螺仪芯片、3轴MEMS加速度计芯片、3轴MEMS电子罗盘芯片以及MEMS气压高度计芯片组成，IMU惯导模块反馈的当前航向角定位为IMUθ。
具体地，AGV1的车体角度与规划好的二维码101的方向呈固定角度，固定角度的范围为0°～360°；SO的距离与二维码101的中心距离为零。
作为本发明的一种实施方式，参照图3，该跟随方法包括以下步骤：
二维码惯导导航控制算法包括以下步骤：
S1：设SVH0＝α，PVH0＝θ；
S2：SVH0与PVH0的偏差经过PH0、IH0和DH0进行调整计算，得到MVH0；
S3：令SVH1＝β+MVH0，PVH1＝IMUθ；
S4：SVH1与PVH1的偏差经过PH1、IH1和DH1进行调整计算，得到MVH1；
S5：则得到控制左轮的线速度为υleft+MVH1，控制右轮的线速度为υright+MVH1。
作为本发明的一种实施方式，参照图4，AGV1经过二维码101产生的实际偏差信息为：x＇、y＇、θ＇，x＇为AGV1偏离二维码101中心点x轴的实际偏差值，y＇为AGV1偏离二维码101中心点y轴的实际偏差值，θ＇为AGV1与二维码101的实际相对角度，则：
作为本发明的一种实施方式，参照图5，AGV1将要经过的两个二维码101之间的距离为d，QR1为AGV1将要经过的第一个二维码101，QR2为AGV1将要经过的第二个二维码101，则：
β＝tan-1(y′/d+x′)。
优选地，AGV1每经过一个二维码101，将会对IMU惯导模块的当前惯导数据进行清零，以简化二维码惯导导航控制算法。
具体地，
SVH1＝β+MVH0＝tan-1(y′/d+x′)+MVH0
＝tan-1(x×cosα-y×sinα)/(d+(x×sinα+y×cosα))+MVH0。
优选地，在AGV1行驶的全程中，AGV1每经过一个二维码101，就通过二维码惯导导航控制算法调整一次左轮的线速度和右轮的线速度，这样通过多次有效调节，AGV1最终满足指定的停止条件并且实现精确停止在指定位置上。
作为本发明的一种实施方式，二维码惯导导航控制算法通过控制左轮的线速度和右轮的线速度，以实现AGV1的车体的控制航向角的改变，当IMU惯导模块反馈的航向角与控制航向角一致时，左轮和右轮保持当前速度驶往下一个二维码101，从而在AGV1经过下一个二维码101时，控制AGV1的车体尽可能的靠近下一个二维码101的中心位置，并且AGV1的车头角度与AGV1的目标角度一致。
参照图6，本二维码惯导导航控制算法的控制流程如下：
首先，判断AGV1运行时的目标航向角与二维码101的相对角度是否一致，判断AGV1运行时的当前航向角与二维码101的中心偏差是否为0；
如果AGV1运行时的目标航向角与二维码101的相对角度一致，且AGV1运行时的当前航向角与二维码101的中心偏差是为0，则保持第一角度环控制器11的输出值；
如果AGV1运行时的目标航向角与二维码101的相对角度不一致，且AGV1运行时的当前航向角与二维码101的中心偏差不为0，则改变第二角度环控制器12的输出值。
本二维码惯导导航控制算法的全局地图示意图参照图7，右上角坐标轴代表全局地图的角度，地图中各个二维码101为正交关系，相邻二维码101之间的距离可以不一致，虚线表示可执行路径，路径行驶方向包括前进+后退，本二维码惯导导航控制算法可在二维码101上实现AGV1原地旋转，以达到改变AGV1的车体在全局地图中的角度值。
优选地，本发明涉及的PID算法均采用经典PID控制算法公式，本领域技术人员可以轻易通过现有的软件程序按照本发明的算法原理图、算法步骤流程图以及控制流程图实现本发明。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种二维码惯导导航控制算法，其特征在于，用于AGV沿着预规划的二维码的路径行驶，所述AGV的车体类型为双轮差速车，包括左轮和右轮，所述AGV的执行机构为差速电机；
所述AGV的控制点定义为SO，所述SO为所述左轮和所述右轮的连线的中点，所述SO的垂直线指向所述AGV的车头方向，所述AGV的车头方向为所述SO的角度；所述SO在全局规划地图中，多个所述二维码以一定的方向铺开，每个所述二维码之间的间距可以不同，所述AGV经过所述二维码产生的相机拍摄的偏差信息定义为：x、y、θ，所述x为所述相机拍摄的所述AGV偏离所述二维码中心点x轴的偏差值，所述y为所述相机拍摄的所述AGV偏离所述二维码中心点y轴的偏差值，所述θ为所述相机拍摄的所述AGV与所述二维码的相对角度；所述左轮的线速度定义为υleft，所述右轮的线速度定义为υright，所述AGV的目标航向角定义为α，所述AGV的理论航向角定义为β；
所述二维码惯导导航控制算法包括第一角度环控制器、第二角度环控制器以及IMU惯导模块；所述第一角度环控制器用于调整所述AGV行驶的大方向，即调整所述AGV的行驶方向与所述AGV的目标角度和所述二维码的相对角度一致；所述第二角度环控制器用于调整所述AGV行驶的小方向，即调整所述AGV的当前航向角与所述二维码的中心位置偏差为0；所述IMU惯导模块用于反馈所述AGV的当前航向角；所述二维码惯导导航控制算法通过计算所述目标航向角与所述IMU惯导模块反馈的当前航向角进行精确对比，控制所述执行机构通过调整所述左轮的线速度和所述右轮的线速度以使所述当前航向角与所述目标航向角一致，从而通过实现闭环控制保证所述AGV到达指定位置；
所述第一角度环控制器的PID目标值定位为SVH0，所述第一角度环控制器的PID输入值定位为PVH0，所述第一角度环控制器的PID输出值定义为MVH0，所述第一角度环控制器的PID比例参数定义为PH0，所述第一角度环控制器的PID积分参数定义为IH0，所述第一角度环控制器的PID微分参数为DH0；所述第二角度环控制器的PID目标值定位为SVH1，所述第二角度环控制器的PID输入值定位为PVH1，所述第二角度环控制器的PID输出值定义为MVH1，所述第二角度环控制器的PID比例参数定义为PH1，所述第二角度环控制器的PID积分参数定义为IH1，所述第二角度环控制器的PID微分参数为DH1；
所述SVH0和所述PVH0的偏差作为所述第一角度环控制器的输入值，经过所述PH0、所述IH0以及所述DH0的PID计算得出所述MVH0，所述MVH0与所述β之和作为所述SVH1的值，所述SVH1和所述PVH1的偏差作为所述第二角度环控制器的输入值，经过所述PH1、所述IH1以及所述DH1的PID计算得出所述MVH1，则所述MVH1与所述υleft之和为控制所述左轮的线速度，所述MVH1与所述υright之和为控制所述右轮的线速度；
所述IMU惯导模块采用3轴MEMS陀螺仪芯片、3轴MEMS加速度计芯片、3轴MEMS电子罗盘芯片以及MEMS气压高度计芯片组成，所述IMU惯导模块反馈的当前航向角定位为IMUθ；
所述AGV的车体角度与规划好的所述二维码的方向呈固定角度，所述固定角度的范围为0°～360°；所述SO的距离与所述二维码的中心距离为零；
所述二维码惯导导航控制算法包括以下步骤：
S1：设所述SVH0＝α，所述PVH0＝θ；
S2：所述SVH0与所述PVH0的偏差经过所述PH0、所述IH0和所述DH0进行调整计算，得到所述MVH0；
S3:令所述SVH1＝β+MVH0，PVH1＝IMUθ；
S4：所述SVH1与所述PVH1的偏差经过所述PH1、所述IH1和所述DH1进行调整计算，得到MVH1；
S5：则得到控制所述左轮的线速度为υleft+MVH1，控制所述右轮的线速度为υright+MVH1；
所述AGV经过所述二维码产生的实际偏差信息为：x＇、y＇、θ＇，所述x＇为所述AGV偏离所述二维码中心点x轴的实际偏差值，所述y＇为所述AGV偏离所述二维码中心点y轴的实际偏差值，所述θ＇为所述AGV与所述二维码的实际相对角度，则：
所述AGV将要经过的两个所述二维码之间的距离为d，则：
β＝tan-1(y′/d+x′)；
所述AGV每经过一个所述二维码，将会对所述IMU惯导模块的当前惯导数据进行清零，以简化所述二维码惯导导航控制算法；
SVH1＝β+MVH0＝tan-1(y′/d+x′)+MVH0
＝tan-1((x×cosα-y×sinα)/(d+(x×sinα+y×cosα)))+MVH0。
2.根据权利要求1所述的一种二维码惯导导航控制算法，其特征在于，在所述AGV行驶的全程中，所述AGV每经过一个所述二维码，就通过所述二维码惯导导航控制算法调整一次所述左轮的线速度和所述右轮的线速度，这样通过多次有效调节，所述AGV最终满足指定的停止条件并且实现精确停止在指定位置上；
所述二维码惯导导航控制算法通过控制所述左轮的线速度和所述右轮的线速度，以实现所述AGV的车体的控制航向角的改变，当所述IMU惯导模块反馈的航向角与所述控制航向角一致时，所述左轮和所述右轮保持当前速度驶往下一个二维码，从而在所述AGV经过所述下一个二维码时，控制所述AGV的车体尽可能的靠近所述下一个二维码的中心位置，并且所述AGV的车头角度与所述AGV的目标角度一致。
CN202110133259.3A 2021-01-29 2021-01-29 一种二维码惯导导航控制算法 Active CN112925327B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110133259.3A CN112925327B (zh) 2021-01-29 2021-01-29 一种二维码惯导导航控制算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110133259.3A CN112925327B (zh) 2021-01-29 2021-01-29 一种二维码惯导导航控制算法

Publications (2)

Publication Number Publication Date
CN112925327A CN112925327A (zh) 2021-06-08
CN112925327B true CN112925327B (zh) 2024-04-02

Family

ID=76169040

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110133259.3A Active CN112925327B (zh) 2021-01-29 2021-01-29 一种二维码惯导导航控制算法

Country Status (1)

Country Link
CN (1) CN112925327B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117093001B (zh) * 2023-08-25 2024-05-07 杭州士腾科技有限公司 自动导向车的纠偏方法及系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018072712A1 (zh) * 2016-10-21 2018-04-26 北京京东尚科信息技术有限公司 Agv运输车及其控制方法
CN108088439A (zh) * 2018-01-19 2018-05-29 浙江科钛机器人股份有限公司 一种融合电子地图、二维码和色带的agv复合导航系统及方法
CN108592906A (zh) * 2018-03-30 2018-09-28 合肥工业大学 基于二维码和惯性传感器的agv复合导航方法
WO2019154443A2 (zh) * 2019-04-04 2019-08-15 上海快仓智能科技有限公司 导航控制方法、智能仓储系统和自动引导车
CN110427033A (zh) * 2019-08-12 2019-11-08 苏州寻迹智行机器人技术有限公司 一种基于二维码的激光导航agv高精度定位方法
CN110440806A (zh) * 2019-08-12 2019-11-12 苏州寻迹智行机器人技术有限公司 一种激光与二维码融合的agv精确定位方法
CN110794838A (zh) * 2019-11-01 2020-02-14 牧星机器人(江苏)有限公司 一种agv导航角度补偿方法、系统、agv及存储介质
CN111830980A (zh) * 2020-07-14 2020-10-27 深圳市高步达智能科技有限公司 一种激光导航路径跟随方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018072712A1 (zh) * 2016-10-21 2018-04-26 北京京东尚科信息技术有限公司 Agv运输车及其控制方法
CN108088439A (zh) * 2018-01-19 2018-05-29 浙江科钛机器人股份有限公司 一种融合电子地图、二维码和色带的agv复合导航系统及方法
CN108592906A (zh) * 2018-03-30 2018-09-28 合肥工业大学 基于二维码和惯性传感器的agv复合导航方法
WO2019154443A2 (zh) * 2019-04-04 2019-08-15 上海快仓智能科技有限公司 导航控制方法、智能仓储系统和自动引导车
CN110427033A (zh) * 2019-08-12 2019-11-08 苏州寻迹智行机器人技术有限公司 一种基于二维码的激光导航agv高精度定位方法
CN110440806A (zh) * 2019-08-12 2019-11-12 苏州寻迹智行机器人技术有限公司 一种激光与二维码融合的agv精确定位方法
CN110794838A (zh) * 2019-11-01 2020-02-14 牧星机器人(江苏)有限公司 一种agv导航角度补偿方法、系统、agv及存储介质
CN111830980A (zh) * 2020-07-14 2020-10-27 深圳市高步达智能科技有限公司 一种激光导航路径跟随方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于双摄像头扫码的AGV运动轨迹修正方法;芦姝冰;于宝成+;王春梅;杨世团;熊永燚;计算机工程与设计;20201231(第012期);全文 *

Also Published As

Publication number Publication date
CN112925327A (zh) 2021-06-08

Similar Documents

Publication Publication Date Title
CN107390691B (zh) 一种agv路径跟踪方法
JP3002206B2 (ja) 移動ロボットの走行制御方法
CN108673496B (zh) 一种基于纯追踪模型的工业机器人路径跟踪方法
CN111580520B (zh) 基于二维码导航的四轮转向agv轨迹纠偏方法
CN109144078A (zh) 一种基于二维码导航共线双舵轮agv路线纠偏方法及系统
CN112462760B (zh) 一种双舵轮agv路径跟踪方法
CN112896147B (zh) 用于矿用车辆的双向定位泊车控制方法及装置
CN109813305A (zh) 基于激光slam的无人叉车
CN112925327B (zh) 一种二维码惯导导航控制算法
Wang et al. Attitude and Altitude Controller Design for Quad‐Rotor Type MAVs
CN112596521A (zh) 一种双舵轮磁导航agv纠偏方法及装置
JP2021071734A (ja) 移動体の制御装置及び移動体の制御方法
Rezende et al. Autonomous system for a racing quadcopter
JP2000298518A (ja) 無人搬送車
CN111830980B (zh) 一种激光导航路径跟随方法
CN112947487B (zh) 一种自动引导车辆及其曲线路径跟踪方法、控制装置
TWI770966B (zh) 無人自走車之導引控制方法
Stephan et al. Spline trajectory planning and guidance for fixed-wing drones
CN114089730B (zh) 机器人的运动规划方法和自动引导车
Park Vision-based guidance for loitering over a target
Savnani et al. Modelling, Design and Control of a Four wheel Holonomic Drive
CN117093001B (zh) 自动导向车的纠偏方法及系统
CN112363515B (zh) 一种基于视觉定位的麦克纳姆轮式agv停位方法
CN113467460B (zh) 一种基于双圆形前视距离的农业机械路径跟踪方法及系统
CN115431969A (zh) 无人自走车的导引控制方法

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
GR01 Patent grant
GR01 Patent grant