CN109900273B

CN109900273B - 一种室外移动机器人导引方法及导引系统

Info

Publication number: CN109900273B
Application number: CN201910153185.2A
Authority: CN
Inventors: 吴平; 刘秀娟; 刘刚军; 牛志朝; 崔孟楠; 王禺雄; 郭丰睿; 刘浩; 武欣; 窦志红; 赵炳英; 郑剑; 王浩
Original assignee: China Academy of Launch Vehicle Technology CALT; Beijing Aerospace Automatic Control Research Institute
Current assignee: China Academy of Launch Vehicle Technology CALT; Beijing Aerospace Automatic Control Research Institute
Priority date: 2019-02-28
Filing date: 2019-02-28
Publication date: 2021-02-09
Anticipated expiration: 2039-02-28
Also published as: CN109900273A

Abstract

一种室外移动机器人导引方法及导引系统，包括(1)计算机器人的当前经度、纬度、航向角；(2)判断机器人是否收到路径规划结果，即目标导引路径坐标点集；(3)更新导引路线，得到导引直线函数；(4)根据经度、纬度、航向角以及导引直线函数计算横向偏差Δd和角度偏差Δθ；(5)将Δd和Δθ作为反馈观测输入数字PID控制器，得到控制量，作为伺服电机的输入量，驱动移动机器人按导引路线运动；(6)更新计算机器人当前经度、纬度、航向角；(7)判断机器人是否移动到达中间点；(8)判断是否到达最终目标点，若到达最终目标点则结束，完成机器人导引。本发明能够修正角度误差，同时修正横向偏差，不仅适应宽大道路、场地，还能适应窄小的道路和环境。

Description

一种室外移动机器人导引方法及导引系统

技术领域

本发明涉及一种室外移动机器人导引方法，属于无人系统领域。

背景技术

随着“工业4.0”及“中国制造2025”的概念相继被提出，移动机器人技术得到了充足发展。近几年来，移动机器人被广泛应用于物流搬运、分拣、无人泊车、安防巡逻、服务等领域中。导引技术作为移动机器人控制系统最重要的技术之一，关系到移动机器人运行的安全性、可靠性和高效性。目前国内外相关学者主要通过视觉、磁感应、激光、惯性、无线定位、卫星导航、激光雷达等方式进行移动机器人导引。

俞毓锋在自动化学报发表的《基于道路结构特征的智能车单目视觉定位》中利用单目相机提取道路中的车道线线特征，计算得到智能车相对车道线的平移向量和旋转矩阵，用于反馈控制智能车运动，实现智能车自动按照车道线导引前进。刘帅在华北电力大学硕士学位论文《变电站轨道机器人的高鲁棒性定位方法》中设计的变电站巡检机器人沿着铺设的铁轨运动，同时轨道指定位置设计定位针用于定位和控制，机器人可以在铁轨铺设范围内进行巡检工作，定位针定位精度达到毫米级。牛祉霏在华北电力大学硕士学位论文《变电站设备巡检机器人系统设计方案的研究与应用》中设计的变电站巡检机器人采用磁导航的方式，要在机器人行走的路线上铺设磁条带，同时在磁条带路线上铺设RFID进行定位。这三种方式的导引控制简单、容易实现，但是都需要在工作区域内进行场地改造，施工成本高，同时线路受限，机器人不够灵活。

张智刚等人在国家发明专利《基于双天线GNSS和预瞄追踪模型的农机自动导航控制方法》中采用分段自适应的与速度相关的前视距离作为预瞄追踪模型方法中的参数对农机进行路径跟踪控制。这种方法工程容易实现，能够保证载体的前进方向始终朝向目标点，达到导引控制的目的，但是对航向角的精度过度依赖，而且只能修正角度偏差，不能修正与目标轨迹的横向偏差，造成载体运动所需的空间浪费。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供了一种室外移动机器人导引方法及系统，接收规划路径坐标点集，按顺序生成多段导引路径，同时差分卫星/惯性组合定位定姿，计算得到移动机器人相对导引路径的横向偏差和角度偏差，并反馈至运动控制消除偏差，实现移动机器人按照规划路径行进。

本发明的技术解决方案是：

一种室外移动机器人导引方法，步骤如下：

(1)通过安装在机器人上的差分卫星/惯性组合导航系统计算得到机器人的当前经度λ、纬度L、航向角ψ；

(2)判断机器人是否收到路径规划结果，即目标导引路径坐标点集{(λ₁,L₁)，…，(λ_n,L_n)}_n≥2，若收到则将导引路线计数index初始化为2并进入步骤(3)，否则返回步骤(1)；

(3)更新导引路线，得到导引直线函数；

(4)根据机器人上差分卫星/惯性组合导航系统输出的经度λ、纬度L、航向角ψ以及导引直线函数计算横向偏差Δd和角度偏差Δθ；

(5)将Δd和Δθ作为反馈观测输入数字PID控制器，得到控制量u(t)，作为伺服电机的输入量，驱动移动机器人按导引路线运动：

(6)令所述差分卫星/惯性组合导航系统更新计算机器人当前经度λ、纬度L、航向角ψ；

(7)判断机器人是否移动到达中间点(λ_index,L_index)，若到达则进入步骤(8)，否则返回步骤(4)；

(8)判断是否到达最终目标点(λ_n,L_n)，若到达最终目标点则结束，完成机器人导引，否则令index加1并返回步骤(3)更新导引路线。

所述步骤(3)更新导引路线，得到导引直线函数，具体为：

(3.1)导引路线的起始点为(λ_index-1,L_index-1)，终点为(λ_index,L_index)；

(3.2)根据起始点和终点计算导引直线函数a·λ+b·L+1＝0：

所述步骤(4)计算横向偏差Δd和角度偏差Δθ，具体为：

步骤(5)中数字PID控制器表示为

其中，K_p、K_i、K_d为PID控制参数，通过试验调试获得；e(t)为时刻t的偏差量。

所述步骤(7)判断到达的条件如下：

机器人与目标点(λ_index,L_index)之间的距离不大于预设值M，即：

所述预设值M的取值范围为0.2m～1m。

所述步骤(8)判断是否到达最终目标点(λ_n,L_n)即判断index是否等于n，若index＝n则到达最终目标点。

一种基于所述的室外移动机器人导引方法实现的导引系统，包括：

组合导航解算模块：用于通过安装在机器人上的差分卫星/惯性组合导航系统计算得到机器人的当前经度λ、纬度L、航向角ψ；

路径规划判别模块：用于判断机器人是否收到路径规划结果，即目标导引路径坐标点集{(λ₁,L₁)，…，(λ_n,L_n)}_n≥2，若收到则将导引路线计数index初始化为2；

导引路线更新模块：用于更新导引路线，得到导引直线函数；

偏差计算模块：用于根据机器人上差分卫星/惯性组合导航系统输出的经度λ、纬度L、航向角ψ以及导引直线函数计算横向偏差Δd和角度偏差Δθ；

PID控制模块：用于将Δd和Δθ作为反馈观测输入数字PID控制器，得到控制量u(t)，作为伺服电机的输入量，驱动移动机器人按导引路线运动；

路径中点判别模块：用于判断机器人是否移动到达中间点(λ_index,L_index)；

路径终点判别模块：用于判断是否到达最终目标点(λ_n,L_n)，若到达最终目标点则结束机器人导引。

所述导引路线更新模块更新导引路线，得到导引直线函数，具体为：

(3.2)根据起始点和终点计算导引直线函数a·λ+b·L+1＝0：

偏差计算模块计算横向偏差Δd和角度偏差Δθ，具体为：

数字PID控制器表示为

路径中点判别模块判断到达的条件如下：

路径终点判别模块判断机器人是否到达最终目标点(λ_n,L_n)，具体为判断index是否等于n，若index＝n则到达最终目标点。

本发明与现有技术相比的有益效果是：

(1)传统依靠视觉导引跟踪车道线、铺设铁轨、铺设磁带等方式，活动范围受限于车道线、铁轨、磁带的布置范围，同时施工成本高也不够灵活，本发明能够覆盖测绘过的地图范围，相比添置车道线更容易、更灵活；

(2)现有卫星/惯性组合的导引方法采用寻的导引的方式，通过修正角度误差来保证机器人始终朝向目标点前进，因此活动的道路需要足够宽或者坐标点集要足够密集，本发明能够修正角度误差，同时修正横向偏差，不仅适应宽大道路、场地，还能适应窄小的道路和环境。

附图说明

图1为机器人移动系统框图；

图2为本发明导引参数示意图；

图3为本发明方法流程图；

图4为移动效果示意图。

具体实施方式

下面结合附图对本发明的具体实施方式进行进一步的详细描述。

本发明的导引系统由电子地图、路径规划系统、偏差计算单元、差分卫星/惯性组合导航系统组成，如图1所示。电子地图包括测绘得到移动范围内的若干经纬度坐标集及坐标之间的连通性。路径规划系统根据电子地图、导引系统计算得到的经纬度以及目标点经纬度生成规划路径。路径由一序列的坐标点组成，并将坐标点序列按顺序发送给偏差计算单元。偏差计算单元根据规划坐标点集生成多段导引直线，根据差分卫星/惯性组合的机器人经纬度和航向角计算相对导引直线的横向偏差、角度偏差，如图2所示，分段进行导引，同时根据横向偏差、角度偏差计算控制量，闭环消除偏差，实现移动机器人按照规划路线导引运动。使用该方法的移动机器人运动灵活，且无需对使用环境进行改造，降低成本；该方法便于工程应用，不仅可以修正距离目标点的角度偏差，还能修正距离导引路径的横向距离偏差，空间占用少。

偏差计算单元接收路径规划系统发送的规划路径坐标点集、差分卫星接收机的经纬度以及惯性测量组合的加速度和角速度，计算机器人相对规划路径的横向偏差和角度偏差，驱动电机消除偏差，实现机器人沿着规划路径行驶，详细工作流程如图3所示：

(1)通过安装在机器人上的差分卫星/惯性组合导航系统计算得到机器人的当前经度λ、纬度L、航向角ψ；组合导航系统内部通过惯性导航周期解算经度、纬度、航向角，并结合差分卫星导航观测的经度、纬度，通过卡尔曼滤波器进行组合解算，得到高精度的经度、纬度、航向角。

(2)判断机器人是否收到路径规划结果，即目标导引路径坐标点集{(λ₁,L₁)，…，(λ_n,L_n)}_n≥2，若收到则将导引路线计数index初始化为2并进入步骤(3)，否则返回步骤(1)；结合目标导引路径坐标点集由路径规划系统提供，如附图1所示。路径规划系统根据带有坐标点及连通性的电子地图，采用路径规划算法，例如A*算法、Dijkstra算法等，计算得到点到点的最优路径。

(3)更新导引路线，得到导引直线函数：导引直线函数根据两点确定直线的原理得到。

(3.2)根据起始点和终点计算导引直线函数a·λ+b·L+1＝0：

(4)根据机器人上差分卫星/惯性组合导航系统输出的经度λ、纬度L、航向角ψ以及导引直线函数计算横向偏差Δd和角度偏差Δθ：偏差量根据点与直线之间的几何关系得到。

(5)将Δd和Δθ作为反馈观测输入数字PID控制器，得到控制量u(t)，作为伺服电机的输入量，驱动移动机器人按导引路线运动：数字PID控制器根据偏差量的当前值、累积值、差分值以及对应的PID参数计算得到控制量。控制量u(t)根据移动机器人的运动控制模型确定，例如c可为移动机器人的前向速度v_f、右向速度v_r、航向角速度ω_yaw组成，即u(t)＝[v_f v_r ω_yaw]^T。

其中K_p、K_i、K_d为PID控制参数，通过试验调试获得；e(t)为时刻t的偏差量。

(7)判断机器人是否移动到达中间点(λ_index,L_index)，若到达则进入步骤(8)，否则返回步骤(4)：

预设值M的取值范围为0.2m～1m。

(8)判断是否到达最终目标点(λ_n,L_n)，即判断index是否等于n，若index＝n则到达最终目标点。若到达最终目标点则结束，完成机器人导引，否则令index加1并返回步骤(3)更新导引路线。

进一步的，本发明还提出一种室外移动机器人导引系统，包括：

本发明相对传统靠视觉导引的方式，能够覆盖测绘过的地图范围，相比添置车道线更容易、更灵活；本发明能够修正角度误差，同时修正横向偏差，不仅适应宽大道路、场地，还能适应窄小的道路和环境。

某次室外机器人导引运动试验采用上述方法和系统，目的是让移动机器人按照规划的路径进行导引移动，移动效果如图4所示，可见机器人的移动轨迹基本为两个地图坐标间的直线，与导引路径基本重合，达到导引控制的目的。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。