申请/专利权人：深圳市高步达智能科技有限公司

申请日：2021-01-29

公开（公告）日：2024-04-02

公开（公告）号：CN112925327B

主分类号：G05D1/43

分类号：G05D1/43;G05D1/243;G05D1/246;G05D1/65;G05D1/644;G05D1/247;G05D109/10

优先权：

专利状态码：有效-授权

法律状态：2024.04.02#授权;2021.06.25#实质审查的生效;2021.06.08#公开

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

主权项：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-1y′d+x′；所述AGV每经过一个所述二维码，将会对所述IMU惯导模块的当前惯导数据进行清零，以简化所述二维码惯导导航控制算法；SVH1＝β+MVH0＝tan-1y′d+x′+MVH0＝tan-1x×cosα-y×sinαd+x×sinα+y×cosα+MVH0。

全文数据：

权利要求：

百度查询： 深圳市高步达智能科技有限公司 一种二维码惯导导航控制算法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD