CN114217622A - 一种基于bim的机器人自主导航方法 - Google Patents

一种基于bim的机器人自主导航方法 Download PDF

Info

Publication number
CN114217622A
CN114217622A CN202111540986.8A CN202111540986A CN114217622A CN 114217622 A CN114217622 A CN 114217622A CN 202111540986 A CN202111540986 A CN 202111540986A CN 114217622 A CN114217622 A CN 114217622A
Authority
CN
China
Prior art keywords
point
node
robot
algorithm
backtracking
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.)
Granted
Application number
CN202111540986.8A
Other languages
English (en)
Other versions
CN114217622B (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.)
Nanjing University of Science and Technology
Original Assignee
Nanjing University of Science and Technology
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 Nanjing University of Science and Technology filed Critical Nanjing University of Science and Technology
Priority to CN202111540986.8A priority Critical patent/CN114217622B/zh
Publication of CN114217622A publication Critical patent/CN114217622A/zh
Application granted granted Critical
Publication of CN114217622B publication Critical patent/CN114217622B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明的目的在于提供一种基于BIM的机器人自主导航方法，包括：构建二维栅格地图：提取待施工建筑的BIM信息中的几何实体信息，按照楼层对几何实体信息进行分类，并根据几何实体信息构建二维栅格地图；机器人定位：分别获取机器人的RTK‑GNSS和INS导航数据，基于松耦合组合导航模式选择对INS的误差状态进行状态估计得到融合算法模型，采用卡尔曼滤波算法用RTK‑GNSS导航数据修正INS导航数据的累积误差；规划机器人路径：根据任务需求特点，将任务划分为单点任务和区域任务两种，采用全局路径规划算法和全覆盖路径规划算法分别满足为单点任务和区域任务下机器人的路径规划。

Description

一种基于BIM的机器人自主导航方法
技术领域
本发明涉及一种建筑机器人导航技术，特别是一种基于BIM的机器人自主导航方法。
背景技术
当下，越来越多的建筑公司、科研机构开始投入大量人力物力致力于建筑施工机器人的研发，针对某种特定施工工艺流程通过机器人实现自动化，例如墙体砌筑机器人、地面施工机器人等等，这些机器人大多数还只能在特定的结构化实验场地进行演示，并不能投入到真实的施工线程使用，造成这种现象一大原因在于建筑施工现场的环境复杂多变，区别于室内或者其他稳定的作业场景，这就要求机器人对环境感知能力要高，尤其是针对移动作业机器人来说，高效地构建用于导航的地图是十分重要的，如果采用传统的机器人建图方案，势必会带来很大的麻烦，无法实际投入使用。
发明内容
本发明的目的在于提供一种基于BIM的机器人自主导航方法，包括
构建二维栅格地图：提取待施工建筑的BIM信息中的几何实体信息，按照楼层对几何实体信息进行分类，并根据几何实体信息构建二维栅格地图；
机器人定位：分别获取机器人的RTK-GNSS和INS导航数据，基于松耦合组合导航模式选择对INS的误差状态进行状态估计得到融合算法模型，采用卡尔曼滤波算法用RTK-GNSS导航数据修正INS导航数据的累积误差；
规划机器人路径：根据任务需求特点，将任务划分为单点任务和区域任务两种，采用全局路径规划算法和全覆盖路径规划算法分别满足为单点任务和区域任务下机器人的路径规划。
进一步地，融合算法模型包括姿态误差、速度误差、位置误差模型和惯性传感器误差模型；其中
(1)姿态误差模型
Figure BDA0003414141560000011
Figure BDA0003414141560000012
Figure BDA0003414141560000021
Figure BDA0003414141560000022
其中，L和h分别为机器人当前位置的维度和海拔高度，
Figure BDA0003414141560000023
分别为导航坐标系下机器人的速度值，RN、RE分别为地球南北方向和东西方向的曲率半径，wie为地球自转角速度在导航坐标系下的投影，
Figure BDA0003414141560000024
为IMU测量误差；
(2)速度误差模型
Figure BDA0003414141560000025
Figure BDA0003414141560000026
Figure BDA0003414141560000027
Figure BDA0003414141560000028
其中，
Figure BDA0003414141560000029
为IMU获取比力值在导航坐标系下的投影，ba,x、ba,y分别为加速度计在x轴和y轴的零偏误差，ψ＝[ψxψyψz]T为平台失准角；
(3)位置误差模型
Figure BDA00034141415600000210
Figure BDA0003414141560000031
其中L和λ分别代表机器人当前位置对应的纬度和经度。
(4)惯性传感器误差模型
Figure BDA0003414141560000032
Figure BDA0003414141560000033
其中，ba、bg分别为加速度计和陀螺仪的零偏误差，τa、Wba分别为加速度计零偏的相关时间和噪声项，τg、Wbg分别为陀螺仪计零偏的相关时间和噪声项。
进一步地，采用卡尔曼滤波算法用RTK-GNSS导航数据修正INS导航数据的累积误差的具体过程包括
步骤S331，设机器人状态模型变量为：
Figure BDA0003414141560000034
将式(1)～(10)代入式(11)得到状态方程和状态矩阵(12)
Figure BDA0003414141560000035
Figure BDA0003414141560000036
其中A为系统状态矩阵，W代表系统噪声，在实际计算中可以将过程协方差矩阵中的噪声项简化为高斯白噪声，
Figure BDA0003414141560000041
Figure BDA0003414141560000042
Figure BDA0003414141560000043
Figure BDA0003414141560000044
Figure BDA0003414141560000045
Figure BDA0003414141560000046
Figure BDA0003414141560000047
Figure BDA0003414141560000051
Figure BDA0003414141560000052
步骤S332，对上述连续时间的状态方程进行离散化，得到式(13)
xk＝φk-1xk-1+Qk-1 (13)
其中，φk-1＝eAδt,Hk＝H，δt为采样周期，Qk-1为过程噪声协方差矩阵；
步骤S333，针对RTK-GNSS定位数据的特点，选择RTK-GNSS与INS的速度差值、位置差值作为测量模型中的观测量，构建测量模型；其中
速度差值与状态变量满足以下关系：
Figure BDA0003414141560000053
其中，
Figure BDA0003414141560000054
和
Figure BDA0003414141560000055
分别代表INS输出沿x方向的速度值和沿y方向的速度值，
Figure BDA0003414141560000056
和
Figure BDA0003414141560000057
分别代表RTK-GNSS输出沿x方向的速度值和沿y方向的速度值，ξx和ξy分别表示RTK-GNSS接收机在x方向和y方向上的测量误差；
位置差值与状态变量之间满足以下关系：
Figure BDA0003414141560000058
其中，LI和λI分别代表INS输出纬度值和经度值，LG和λG分别代表RTK-GNSS输出纬度值和经度值，Nx和Ny分别表示RTK-GNSS接收机在x方向和y方向上的测量误差。
步骤S334，定义观测矩阵为：
Figure BDA0003414141560000059
根据式(14)(15)得到测量模型中测量矩阵为
Figure BDA0003414141560000061
进一步地，全局路径规划算法采用改进theta*算法实现单点任务下机器人路径规划，其中open表中堆顶元素始终是代价值最小的节点，其中代价值为当前节点与起始节点的欧拉距离加上当前节点与目标节点的欧拉距离之和；改进theta*算法具体包括：
步骤S410，添加机器人的起始节点和结束节点；
步骤S411，将初始状态加入open表中，转步骤S412；
步骤S412，获取open表中堆顶节点作为当前节点；若当前节点为目标节点，输出节点信息；若不为目标节点，转步骤S413；
步骤S413，判断当前节点父节点与当前节点相邻节点的可见性；若相邻节点可见，转步骤S414；否则转步骤S415；
步骤S414，将当前节点的相邻节点作为拓展节点，并将拓展节点的父节点调整为当前节点的父节点，同时从open堆中移除当前节点和新拓展节点，转步骤S416；
步骤S415，拓展新状态，将不重复的新状态添加到open堆中，从open堆中移除当前节点，转步骤S416；
步骤S416，输出结果；其中
步骤S413当前节点父节点与相邻节点的可见性判断标准为：
在当前节点中维护角度范围变量θ，若当前节点的父节点与当前节点的相邻节点形成直线与参考方向所成角度落在θ内，则该相邻节点相对于当前节点的父节点是可见的。
进一步地，全覆盖路径规划算法实现区域任务下机器人路径规划的方法包括：
步骤S401，初始化起始位置等相关参数，如果当前位置为起始位置，转步骤S402；否则通过改进theta*算法进行路径规划抵达起始位置后转步骤S402；
步骤S402，通过改进BCD算法进行局部区域覆盖；
步骤S403，将符合要求的栅格位置记录在回溯表中，当机器人移动到死点位置时，根据回溯点筛选规则对回溯表进行筛选，若筛选后回溯表不为空集，转步骤S404；若回溯表为空集，转步骤S406；
步骤S404，筛选后的回溯表作为区域衔接目标点的候选集合，分别计算回溯点与当前位置点的欧拉距离，并选取欧拉距离最小的回溯点作为目标点；
步骤S405，在选定区域衔接目标点之后，通过改进theta*算法进行路径规划抵达目标点，将目标点作为当前位置，转步骤S402；
步骤S406，结束本次全覆盖过程。
进一步地，步骤S402中的改进BCD算法中机器人包括8个移动方向，分别为正北、正南、正西、正东、东北、西北、西南、东南，且规定移动方向的优先级，优先级包括：
优先级1：水平和垂直方向的优先级高于倾斜方向；
优先级2：在优先级1相同的情况下，靠近起始点的方向优先级高于远离起始点的方向；
优先级3：在优先级2相同的情况下，垂直方向的优先级高于水平方向，且正北高于正南，正东高于正西。
进一步地，改进BCD算法具体过程包括：
步骤S420，根据二维栅格地图，确定待工作区域的栅格地图并确定出发点；
步骤S421，通过全局路径规划算法规划出当前位置到出发点所在位置的路径，并移动到出发位置；
步骤S422，获取当前位置所有可移动方向，如果存在可移动方向则通过改进BCD算法优先级规则确定当前移动方向，若可移动，转步骤S423；否则，输出局部覆盖路径。
步骤S423，沿着选定方向移动一个栅格并更新当前位置，转步骤S422。
进一步地，步骤S403中的回溯表用于存储两个子区域间衔接时选取的候选目标点，所述候选目标点是机器人所在位置相邻八个方向上非障碍物且机器人尚未抵达过的栅格位置，其中判断候选目标的过程包括：
步骤S4311，假设待筛选回溯点位于A点，A点可移动的八个方向上的点分别为p1～p8，在这些点中能够与A点形成直线的一共有四对，分别是p1和p5、p2和p6、p3和p7、p4和p8；
步骤S4312，定义状态描述函数
Figure BDA0003414141560000081
d(pi,pj)用于反映与A点在同一直线上的相邻两点是否均为自由未覆盖栅格；若d(pi,pj)＝1则说明与A点在同一直线上的相邻两点pi、pj均为自由未覆盖栅格，则可判定A点必然不是所在子区域的边缘点，进而可以将A点从回溯表中移除，转步骤S4313；
步骤S4313，定义回溯点判断函数如下：
D(A)＝d(p1,p5)+d(p2,p6)+d(p3,p7)+d(p4,p8) (17)
若D(A)≥1，则表示在A点与相邻位置形成的四条直线中，存在直线上A点相邻的两个位置点均为自由非覆盖栅格，那么A点必然不是边缘点，从回溯表中删除；若D(A)＝0，则说明A点是边缘点，需要保留。在图8所示环境中，根据上述判定规则，自由黑色实心点标记的栅格位置才具有作为回溯点的条件，其他栅格位置均不需要维护在回溯表中。
本发明与现有技术相比，优点在于：(1)本发明的机器人自主导航系统通过从BIM中提取的信息构建出用于自主导航的栅格地图，避免了在施工现场建图的不便；(2)本发明的机器人自主导航系统使用RTK-GNSS/INS组合导航，基于RTK-GNSS高精度定位数据修正INS累积误差，从而提高了导航数据的更新速率和可靠性；(3)本发明将任务划分为单点振捣和区域振捣，能够在更大程度上满足对机器人需求；(4)本发明的机器人自主导航系统采用改进theta*算法完成全局路径规划，提高了全局路径规划的效率；(5)本发明的机器人自主导航系统通过多级优先级定向优化BCD算法，同时建立回溯机制，提高了全覆盖路径规划的效率。
下面结合说明书附图对本发明作进一步描述。
附图说明
图1为本发明方法流程示意图。
图2为传统Theta*算法流程示意图。
图3为本发明步骤S400中改进Theta*算法流程示意图。
图4为本发明步骤S400中改进Theta*算法中角度范围定义图。
图5为本发明步骤S400中改进的BCD算法流程示意图。
图6为本发明步骤S400中改进的BCD算法处理齿形障碍物示意图。
图7为本发明步骤S400中回溯点判断栅格标记示意图。
图8为本发明步骤S400中回溯点筛选示意图。
图9为本发明步骤S400的具体流程示意图。
具体实施方式
结合图1，一种基于BIM的机器人自主导航方法，包括以下步骤：
步骤S100，对待施工建筑的BIM信息按照几何信息类型提取出几何实体信息，并按照楼层将几何实体信息进行分类；
步骤S200，根据几何实体信息构建二维栅格地图；
步骤S300，分别获取RTK-GNSS和INS导航数据，基于松耦合组合导航模式选择对INS的误差状态进行状态估计，采用卡尔曼滤波算法对RTK-GNSS导航数据和INS导航数据进行融合，通过RTK-GNSS定位数据修正INS的累积误差；
步骤S400，根据任务需求特点，将任务划分为单点任务和区域任务两种，分别设计全局路径规划算法和全覆盖路径规划算法满足不同任务对路径规划的需求。
步骤S100中涉及的几何信息类型和建筑结构对应，主要包含墙、柱、门、横梁、窗、楼梯等。本实施例以振捣机器人为例，在振捣机器人行进的自主导航过程中，可能会遇到墙、柱、横梁等障碍物，因此需要提取墙、柱、横梁以及楼梯等类型建筑结构对应的几何实体信息。
步骤S100的具体过程包括以下步骤：
步骤S110，获取几何信息类型，并判断是否属于几何实体信息，过滤掉非几何实体信息；
步骤S120，保留所需几何实体信息；
步骤S130对所需的几何实体信息进行分类，获得个楼层的几何实体信息。
本实施例中，步骤S100可以利用IFC文件实现几何实体信息的获取，具体为：
步骤S110，将BIM模型导出为IFC(Industry Foundation Classes)文件类型保存，然后读取IFC文件数据，获取文件中定义的IfcBuildingElement要素信息，IfcBuildingElement要素信息包含IfcWall(墙面)、IfcColum(柱)、IfcBeam(横梁)、IfcStair(楼梯)的等类型要素信息；
步骤S120，遍历所有的IfcBuildingElement要素信息，通过不同描述语句的属性判断是否属于几何实体信息，过滤掉非几何实体要素；建立目标建筑结构类型过滤池，对几何实体信息进行过滤，按照保留需要的几何实体类型进行过滤，保留下来的类型有IfcWall(墙面)、IfcColum(柱)、IfcBeam(横梁)、IfcStair(楼梯)，其他的均过滤掉，得到所需几何实体信息；
步骤S130，将所需几何实体信息，根据每个几何实体父节点所关联的IfcAbstractEntity实体类型是否为IfcBuildingStorey类型来对几何实体信息进行分类，最终得到各楼层的几何实体信息。每个节点之间有继承关系，比如A节点继承自B节点，则B节点就叫A节点的父节点。
步骤S200的具体过程包括以下步骤：
步骤S210，遍历并获取每个几何实体信息；
步骤S220，根据几何实体信息构建二维栅格地图。
本实施例中，步骤S202中根据提取的BIM信息生成ROS可读取的PGM类型的地图文件。以振捣机器人为例，步骤S202具体包括：
步骤S221，首先需要根据障碍物的尺寸信息确定栅格尺寸；
步骤S222，根据栅格尺寸大小计算得到每层建筑平面需要占据的栅格数量；
步骤S223，根据总栅格数量，选定PGM图片的像素点数，其中每个像素代表一个栅格；选定建筑平面的左上角位置为参考点，并映射到像素平面的栅格中；
步骤S224，依次将建筑结构信息映射到栅格地图中，生成一张PGM格式的导航地图，其中对于柱、楼梯等建筑结构为振捣作业空间中的障碍物，在构建栅格地图时，将此类建筑结构状态设置为障碍物状态。
步骤S300中的RTK-GNSS和INS导航数据分别为全球导航卫星数据和惯性导航数据。步骤S300的具体过程为：
步骤S310，获取RTK-GNSS和INS定位数据；
步骤S320，基于松耦合组合导航模式，选择对INS的误差状态进行状态估计，并推导出融合算法模型，模型包括姿态误差、速度误差、位置误差模型和惯性传感器误差模型；
步骤S330，采用卡尔曼滤波算法对GNSS导航数据和INS导航数据进行融合。
步骤S310中RTK-GNSS定位数据通过双天线GNSS接收机接收的卫星信号，结合基站发送过来的差分修正值得到精准的定位定向数据；INS定位数据通过IMU测量的加速度与角速度值进行积分推算得到。
步骤S320所述的姿态误差、速度误差和位置误差模型分别为：
(1)姿态误差模型
Figure BDA0003414141560000111
Figure BDA0003414141560000112
Figure BDA0003414141560000113
Figure BDA0003414141560000114
其中，L和h分别为机器人当前位置的维度和海拔高度，
Figure BDA0003414141560000115
分别为导航坐标系下机器人的速度值，RN、RE分别为地球南北方向和东西方向的曲率半径，wie为地球自转角速度在导航坐标系下的投影，
Figure BDA0003414141560000116
为IMU测量误差；
(2)速度误差模型
Figure BDA0003414141560000117
Figure BDA0003414141560000121
Figure BDA0003414141560000122
Figure BDA0003414141560000123
其中，
Figure BDA0003414141560000124
为IMU获取比力值在导航坐标系下的投影，ba,x、ba,y分别为加速度计在x轴和y轴的零偏误差，ψ＝[ψxψyψz]T为平台失准角；
(3)位置误差模型
Figure BDA0003414141560000125
Figure BDA0003414141560000126
其中L和λ分别代表机器人当前位置对应的纬度和经度。
(4)惯性传感器误差模型
Figure BDA0003414141560000127
Figure BDA0003414141560000128
其中，ba、bg分别为加速度计和陀螺仪的零偏误差，τa、Wba分别为加速度计零偏的相关时间和噪声项，τg、Wbg分别为陀螺仪计零偏的相关时间和噪声项。
步骤S330的具体过程包括
步骤S331，设机器人状态模型变量为：
Figure BDA0003414141560000131
将式(1)～(10)代入式(11)得到状态方程和状态矩阵(12)
Figure BDA0003414141560000132
Figure BDA0003414141560000133
其中A为系统状态矩阵，W代表系统噪声，在实际计算中可以将过程协方差矩阵中的噪声项简化为高斯白噪声，
Figure BDA0003414141560000134
Figure BDA0003414141560000135
Figure BDA0003414141560000136
Figure BDA0003414141560000137
Figure BDA0003414141560000141
Figure BDA0003414141560000142
Figure BDA0003414141560000143
Figure BDA0003414141560000144
Figure BDA0003414141560000145
步骤S332，传感器通常采用的是离散采样，因此对上述连续时间的状态方程进行离散化，得到式(13)
xk＝φk-1xk-1+Qk-1 (13)
其中，φk-1＝eAδt,Hk＝H，δt为采样周期，Qk-1为过程噪声协方差矩阵；
步骤S333，针对RTK-GNSS定位数据的特点，选择RTK-GNSS与INS的速度差值、位置差值作为测量模型中的观测量，构建测量模型；
速度差值与状态变量满足以下关系：
Figure BDA0003414141560000146
其中，
Figure BDA0003414141560000151
和
Figure BDA0003414141560000152
分别代表INS输出沿x方向的速度值和沿y方向的速度值，
Figure BDA0003414141560000153
和
Figure BDA0003414141560000154
分别代表RTK-GNSS输出沿x方向的速度值和沿y方向的速度值，ξx和ξy分别表示RTK-GNSS接收机在x方向和y方向上的测量误差；
位置差值与状态变量之间满足以下关系：
Figure BDA0003414141560000155
其中，LI和λI分别代表INS输出纬度值和经度值，LG和λG分别代表RTK-GNSS输出纬度值和经度值，Nx和Ny分别表示RTK-GNSS接收机在x方向和y方向上的测量误差。
定义观测矩阵为：
Figure BDA0003414141560000156
根据式(14)(15)得到测量模型中测量矩阵为
Figure BDA0003414141560000157
实现卡尔曼滤波实现RTK-GNSS数据与INS数据的融合，从而得到最优的状态估计，实现RTK-GNSS定位数据修正INS的累积误差。
步骤S400中，单点任务指施工是在不连续区域内的多个独立位置点进行的任务，区域任务指在一片连续的区域内进行覆盖性施工的任务。针对单点任务，本实施例采用改进theta*算法对一些指定的工作位置可以规划出全局最优路径，抵达工作位置完成任务；针对区域任务，机器人需要以指定速度在一子区域进行全覆盖移动，因此采用BCD局部区域覆盖算法实现子区域的全覆盖。
结合图2，传统theta*算法采用的是数组结构，在获取目标节点时需要进行遍历，时间复杂度为O(n)。而本实施例中步骤S400中通过BIM信息构建先验地图的基础上，采用改进theta*算法完成全局路径规划，以代价距离的大小为依据维护未搜索节点的堆数据结构，以O(1)的时间复杂度获取目标节点从而降低获取目标节点的时间复杂度。具体的结合图3，改进theta*算法具体对实现中使用的数据结构以及两节点之间的直线路径上是否存在障碍节点的判断方法进行了改进：
(1)数据结构方面主要是针对open表(OPEN表保存所有已生成而未考察的节点)的数据结构进行改进，传统的theta*算法在保存未被搜索到的节点时采用的是数组或者链表的数据结构进行存储，由于在theta*算法执行中需要从open表中找到代价最小的节点作为下一个搜索节点，如果采用数组或者链表的数据结构，则需要对所有节点进行遍历才能得到目标节点，程序运行时间复杂度为O(n)。因此，本实施例采用最小堆数据结构来维护open表，根据各个节点的代价值大小维护最小堆，基于最小堆数据结构的特点，堆顶元素始终是代价值最小的节点。所以程序运行时可以通过O(1)的时间复杂度获取目标节点。通过改进open表的数据结构，可以在一定程度上提升theta*算法的执行效率。所述代价值为当前节点与起始节点的欧拉距离加上当前节点与目标节点的欧拉距离之和。
(2)在判断两节点之间的直线路径上是否有障碍物节点方面，传统的theta*算法采用的是视线算法，实现原理就是通过判断该直线路径所穿过的所有栅格节点中是否存在障碍物节点，如果存在障碍物节点则判断给定两节点之间不存在无障碍的直线路径，这种判断方法不仅需要判断两节点之间的连线穿过哪些节点，还需要进一步判断这些节点是否是障碍物节点，因此算法的运行效率较低。本实施例通过在节点中记录/添加(即维护)一项角度范围变量θ，该角度范围变量是当前节点的父节点与当前节点相邻的障碍物节点所成角度范围。由于维护了该角度范围值，在判断待拓展节点与当前节点的父节点之间是否存在直线路径时，可以直接通过判断当前节点的父节点与待拓展节点形成直线与参考方向所成角度是否落在当前节点所维护的角度范围变量值之间，如果是说明不存在直线路径。如图4所示，当前节点为s，parent(s)为当前节点的父节点，待拓展节点为s′，目标节点为Goal，由图可以看出parent(s)与s′所成直线不在θ范围内，因此待拓展节点s′与parent(s)之间不存在无障碍直线路径。
对于这种改进方法，关键在于维护节点的角度范围变量，所有角度必须具有唯一的参考方向，本实施例所有角度均是沿着x轴正方向逆时针旋转所成角度，角度范围为[0°,360°]，对于起始节点的角度范围直接初始化为[0°,360°]。使用角度范围进行改进的theta*算法避免了对节点的遍历判断操作，提升了算法的效率。
结合图3，本实施例的改进theta*算法具体包括以下步骤：
步骤S410，添加机器人的起始节点和结束节点；
步骤S411，将初始状态加入open表中，转步骤S412；
步骤S412，获取open表中堆顶节点作为当前节点，即栅格；若当前节点为目标节点，输出节点信息；若不为目标节点，转步骤S413；
步骤S413，通过维护的角度范围信息，判断当前节点父节点与相邻节点的可见性；若相邻节点可见，转步骤S414；否则转步骤S415；
步骤S414，将当前节点的相邻节点作为拓展节点，并将拓展节点的父节点调整为当前节点的父节点，同时从open堆中移除当前节点和新拓展节点，转步骤S416；
步骤S415，拓展新状态，将不重复的新状态添加到open堆中，从open堆中移除当前节点，转步骤S416；
步骤S416，输出结果。
在步骤S412中，由于是最小堆数据结构维护的Open表，插入节点时会自动按照代价值进行排序，堆顶节点始终是最小代价值的节点。
本实施例提出的改进BCD算法将BCD算法中规定的四个方向增加到八个方向，机器人除了可以向North、South、West以及East方向移动之外，还可以向NorthWest、SouthWest、NorthEast以及SouthEast四个方向移动。另外该算法针对移动方向的选择制定了三条优先级规则：
以上三条优先级规则在判断时同样具有优先级，优先级顺序依次降低，即只有在具备较高优先级规则判定情况相同时才能使用较低优先级判定规则。结合图5，改进BCD算法具体过程包括：
步骤S420，根据二维栅格地图，确定待工作区域的栅格地图并确定出发点；
步骤S421，通过全局路径规划算法规划出当前位置到出发点所在位置的路径，并移动到出发位置；
步骤S422，获取当前位置所有可移动方向，如果存在可移动方向则通过改进BCD算法优先级规则确定当前移动方向，若可移动，转步骤S423；否则，输出局部覆盖路径。
步骤S423，沿着选定方向移动一个栅格并更新当前位置，转步骤S422。
通过上述算法，机器人完成了某一子区域的覆盖任务，在机器人完成当前子区域的覆盖任务后，需要前往下一个子区域的起点，但是此时可能存在多个未工作的局部子区域，如何选取局部子区域是机器人全覆盖算法需要解决的重要问题，即选取两个子区域衔接路径的目标点进一步地。本实施例设计回溯点机制解决子区域选择问题，包括回溯点的维护、筛选和选取，通过维护一张尚未被覆盖的坐标点信息，并通过是否存在一条直线，使得当前位置点在该直线上的两个相邻位置点均为自由未覆盖栅格的规则判定当前点是否可以筛选掉，依据这一判定规则对回溯点进行筛选，从而减小了回溯点的数量，通过欧拉距离最短的原则从回溯表中选取回溯点完成子区域的衔接。具体过程包括：
步骤S430，建立回溯表，用于存储两个子区域间衔接时选取的候选目标点，所述候选目标点是机器人所在位置相邻八个方向上的自由(非障碍物)且未覆盖的栅格(机器人尚未抵达过的栅格位置)；
步骤S431，判断回溯表中的候选目标点是否处于未覆盖子区域的边缘位置，结合图7、图8，判断的具体过程为：
步骤S4311，假设待筛选回溯点位于A点，A点可移动的八个方向上的点分别为p1～p8，在这些点中能够与A点形成直线的一共有四对，分别是p1和p5、p2和p6、p3和p7、p4和p8；
步骤S4312，定义状态描述函数
Figure BDA0003414141560000181
d(pi,pj)用于反映与A点在同一直线上的相邻两点是否均为自由未覆盖栅格；若d(pi,pj)＝1则说明与A点在同一直线上的相邻两点pi、pj均为自由未覆盖栅格，则可判定A点必然不是所在子区域的边缘点，进而可以将A点从回溯表中移除，转步骤S4313；
步骤S4313，定义回溯点判断函数如下：
D(A)＝d(p1,p5)+d(p2,p6)+d(p3,p7)+d(p4,p8) (17)
若D(A)≥1，则表示在A点与相邻位置形成的四条直线中，存在直线上A点相邻的两个位置点均为自由非覆盖栅格，那么A点必然不是边缘点，从回溯表中删除；若D(A)＝0，则说明A点是边缘点，需要保留。在图8所示环境中，根据上述判定规则，自由黑色实心点标记的栅格位置才具有作为回溯点的条件，其他栅格位置均不需要维护在回溯表中。
步骤S432，当机器人移动到死点(即在该位置处，机器人无法继续移动)位置时，需要在回溯表中选取一个回溯点(保存在回溯表中的候选目标点)作为子区域衔接的目标点或者下一个子区域的起始点，选取的依据为：分别计算回溯表中回溯点与死点位置之间的欧拉距离，选取欧拉距离最小的回溯点作为此次区域衔接路径规划的目标点。机器人从死点移动至目标点面在另一子区域进行工作。
综上，结合图9，基于上述对全局路径规划算法、局部区域覆盖算法以及回溯方案，步骤S400的具体过程为：
步骤S401，初始化起始位置等相关参数，如果当前位置就是起始位置，转步骤S402；否则通过改进theta*算法进行路径规划抵达起始位置后转步骤S402；
步骤S402，通过改进BCD算法进行局部区域覆盖；
步骤S403，需要将符合要求的栅格位置记录在回溯表中，当机器人移动到死点位置时，根据回溯点筛选规则对回溯表进行筛选，若筛选后回溯表存在候选目标点，转步骤S404；若回溯表为空集，转步骤S406；
步骤S404，筛选后的回溯表作为区域衔接目标点的候选集合，分别计算回溯点与当前位置点的欧拉距离，并选取欧拉距离最小的回溯点作为目标点；
步骤S405，在选定区域衔接目标点之后，通过改进theta*算法进行路径规划抵达目标点，将目标点作为当前位置，转步骤S402；
步骤S406，结束本次全覆盖过程。

Claims (8)

1.一种基于BIM的机器人自主导航方法，其特征在于，包括：
构建二维栅格地图：提取待施工建筑的BIM信息中的几何实体信息，按照楼层对几何实体信息进行分类，并根据几何实体信息构建二维栅格地图；
机器人定位：分别获取机器人的RTK-GNSS和INS导航数据，基于松耦合组合导航模式选择对INS的误差状态进行状态估计得到融合算法模型，采用卡尔曼滤波算法用RTK-GNSS导航数据修正INS导航数据的累积误差；
规划机器人路径：根据任务需求特点，将任务划分为单点任务和区域任务两种，采用全局路径规划算法和全覆盖路径规划算法分别满足为单点任务和区域任务下机器人的路径规划。
2.根据权利要求1所述的方法，其特征在于，融合算法模型包括姿态误差、速度误差、位置误差模型和惯性传感器误差模型；其中
(1)姿态误差模型
Figure FDA0003414141550000011
Figure FDA0003414141550000012
Figure FDA0003414141550000013
Figure FDA0003414141550000014
其中，L和h分别为机器人当前位置的维度和海拔高度，
Figure FDA0003414141550000015
分别为导航坐标系下机器人的速度值，RN、RE分别为地球南北方向和东西方向的曲率半径，wie为地球自转角速度在导航坐标系下的投影，
Figure FDA0003414141550000016
为IMU测量误差；
(2)速度误差模型
Figure FDA0003414141550000017
Figure FDA0003414141550000021
Figure FDA0003414141550000022
Figure FDA0003414141550000023
其中，
Figure FDA0003414141550000024
为IMU获取比力值在导航坐标系下的投影，ba,x、ba,y分别为加速度计在x轴和y轴的零偏误差，ψ＝[ψx ψy ψz]T为平台失准角；
(3)位置误差模型
Figure FDA0003414141550000025
Figure FDA0003414141550000026
其中L和λ分别代表机器人当前位置对应的纬度和经度；
(4)惯性传感器误差模型
Figure FDA0003414141550000027
Figure FDA0003414141550000028
其中，ba、bg分别为加速度计和陀螺仪的零偏误差，τa、Wba分别为加速度计零偏的相关时间和噪声项，τg、Wbg分别为陀螺仪计零偏的相关时间和噪声项。
3.根据权利要求2所述的方法，其特征在于，采用卡尔曼滤波算法用RTK-GNSS导航数据修正INS导航数据的累积误差的具体过程包括
步骤S331，设机器人状态模型变量为：
Figure FDA0003414141550000031
将式(1)～(10)代入式(11)得到状态方程和状态矩阵(12)
Figure FDA0003414141550000032
Figure FDA0003414141550000033
其中A为系统状态矩阵，W代表系统噪声，在实际计算中可以将过程协方差矩阵中的噪声项简化为高斯白噪声，
Figure FDA0003414141550000034
Figure FDA0003414141550000035
Figure FDA0003414141550000036
Figure FDA0003414141550000037
Figure FDA0003414141550000041
Figure FDA0003414141550000042
Figure FDA0003414141550000043
Figure FDA0003414141550000044
Figure FDA0003414141550000045
步骤S332，对上述连续时间的状态方程进行离散化，得到式(13)
xk＝φk-1xk-1+Qk-1 (13)
其中，φk-1＝eAδt,Hk＝H，δt为采样周期，Qk-1为过程噪声协方差矩阵；
步骤S333，针对RTK-GNSS定位数据的特点，选择RTK-GNSS与INS的速度差值、位置差值作为测量模型中的观测量，构建测量模型；其中
速度差值与状态变量满足以下关系：
Figure FDA0003414141550000046
其中，
Figure FDA0003414141550000051
和
Figure FDA0003414141550000052
分别代表INS输出沿x方向的速度值和沿y方向的速度值，
Figure FDA0003414141550000053
和
Figure FDA0003414141550000054
分别代表RTK-GNSS输出沿x方向的速度值和沿y方向的速度值，ξx和ξy分别表示RTK-GNSS接收机在x方向和y方向上的测量误差；
位置差值与状态变量之间满足以下关系：
Figure FDA0003414141550000055
其中，LI和λI分别代表INS输出纬度值和经度值，LG和λG分别代表RTK-GNSS输出纬度值和经度值，Nx和Ny分别表示RTK-GNSS接收机在x方向和y方向上的测量误差，
步骤S334，定义观测矩阵为：
Figure FDA0003414141550000056
根据式(14)(15)得到测量模型中测量矩阵为
Figure FDA0003414141550000057
4.根据权利要求1所述的方法，其特征在于，全局路径规划算法采用改进theta*算法实现单点任务下机器人路径规划，其中open表中堆顶元素始终是代价值最小的节点，其中代价值为当前节点与起始节点的欧拉距离加上当前节点与目标节点的欧拉距离之和；改进theta*算法具体包括：
步骤S410，添加机器人的起始节点和结束节点；
步骤S411，将初始状态加入open表中，转步骤S412；
步骤S412，获取open表中堆顶节点作为当前节点；若当前节点为目标节点，输出节点信息；若不为目标节点，转步骤S413；
步骤S413，判断当前节点父节点与当前节点相邻节点的可见性；若相邻节点可见，转步骤S414；否则转步骤S415；
步骤S414，将当前节点的相邻节点作为拓展节点，并将拓展节点的父节点调整为当前节点的父节点，同时从open堆中移除当前节点和新拓展节点，转步骤S416；
步骤S415，拓展新状态，将不重复的新状态添加到open堆中，从open堆中移除当前节点，转步骤S416；
步骤S416，输出结果；其中
步骤S413当前节点父节点与相邻节点的可见性判断标准为：
在当前节点中维护角度范围变量θ，若当前节点的父节点与当前节点的相邻节点形成直线与参考方向所成角度落在θ内，则该相邻节点相对于当前节点的父节点是可见的。
5.根据权利要求4所述的方法，其特征在于，全覆盖路径规划算法实现区域任务下机器人路径规划的方法包括：
步骤S401，初始化起始位置等相关参数，如果当前位置为起始位置，转步骤S402；否则通过改进theta*算法进行路径规划抵达起始位置后转步骤S402；
步骤S402，通过改进BCD算法进行局部区域覆盖；
步骤S403，将符合要求的栅格位置记录在回溯表中，当机器人移动到死点位置时，根据回溯点筛选规则对回溯表进行筛选，若筛选后回溯表不为空集，转步骤S404；若回溯表为空集，转步骤S406；
步骤S404，筛选后的回溯表作为区域衔接目标点的候选集合，分别计算回溯点与当前位置点的欧拉距离，并选取欧拉距离最小的回溯点作为目标点；
步骤S405，在选定区域衔接目标点之后，通过改进theta*算法进行路径规划抵达目标点，将目标点作为当前位置，转步骤S402；
步骤S406，结束本次全覆盖过程。
6.根据权利要求5所述的方法，其特征在于，步骤S402中的改进BCD算法中机器人包括8个移动方向，分别为正北、正南、正西、正东、东北、西北、西南、东南，且规定移动方向的优先级，优先级包括：
优先级1：水平和垂直方向的优先级高于倾斜方向；
优先级2：在优先级1相同的情况下，靠近起始点的方向优先级高于远离起始点的方向；
优先级3：在优先级2相同的情况下，垂直方向的优先级高于水平方向，且正北高于正南，正东高于正西。
7.根据权利要求6所述的方法，其特征在于，改进BCD算法具体过程包括：
步骤S420，根据二维栅格地图，确定待工作区域的栅格地图并确定出发点；
步骤S421，通过全局路径规划算法规划出当前位置到出发点所在位置的路径，并移动到出发位置；
步骤S422，获取当前位置所有可移动方向，如果存在可移动方向则通过改进BCD算法优先级规则确定当前移动方向，若可移动，转步骤S423；否则，输出局部覆盖路径；
步骤S423，沿着选定方向移动一个栅格并更新当前位置，转步骤S422。
8.根据权利要求8所述的方法，其特征在于，步骤S403中的回溯表用于存储两个子区域间衔接时选取的候选目标点，所述候选目标点是机器人所在位置相邻八个方向上非障碍物且机器人尚未抵达过的栅格位置，其中判断候选目标的过程包括：
步骤S4311，假设待筛选回溯点位于A点，A点可移动的八个方向上的点分别为p1～p8，在这些点中能够与A点形成直线的一共有四对，分别是p1和p5、p2和p6、p3和p7、p4和p8；
步骤S4312，定义状态描述函数
Figure FDA0003414141550000071
d(pi,pj)用于反映与A点在同一直线上的相邻两点是否均为自由未覆盖栅格；若d(pi,pj)＝1则说明与A点在同一直线上的相邻两点pi、pj均为自由未覆盖栅格，则可判定A点必然不是所在子区域的边缘点，进而可以将A点从回溯表中移除，转步骤S4313；
步骤S4313，定义回溯点判断函数如下：
D(A)＝d(p1,p5)+d(p2,p6)+d(p3,p7)+d(p4,p8) (17)
若D(A)≥1，则表示在A点与相邻位置形成的四条直线中，存在直线上A点相邻的两个位置点均为自由非覆盖栅格，那么A点必然不是边缘点，从回溯表中删除；若D(A)＝0，则说明A点是边缘点，需要保留，在图8所示环境中，根据上述判定规则，自由黑色实心点标记的栅格位置才具有作为回溯点的条件，其他栅格位置均不需要维护在回溯表中。
CN202111540986.8A 2021-12-16 2021-12-16 一种基于bim的机器人自主导航方法 Active CN114217622B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111540986.8A CN114217622B (zh) 2021-12-16 2021-12-16 一种基于bim的机器人自主导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111540986.8A CN114217622B (zh) 2021-12-16 2021-12-16 一种基于bim的机器人自主导航方法

Publications (2)

Publication Number Publication Date
CN114217622A true CN114217622A (zh) 2022-03-22
CN114217622B CN114217622B (zh) 2023-09-01

Family

ID=80702881

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111540986.8A Active CN114217622B (zh) 2021-12-16 2021-12-16 一种基于bim的机器人自主导航方法

Country Status (1)

Country Link
CN (1) CN114217622B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114608549A (zh) * 2022-05-10 2022-06-10 武汉智会创新科技有限公司 一种基于智能机器人的建筑测量方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108519615A (zh) * 2018-04-19 2018-09-11 河南科技学院 基于组合导航和特征点匹配的移动机器人自主导航方法
CN109801358A (zh) * 2018-12-06 2019-05-24 宁波市电力设计院有限公司 一种基于slam扫描和点云影像融合的变电站三维勘测方法
CN109916393A (zh) * 2019-03-29 2019-06-21 电子科技大学 一种基于机器人位姿的多重栅格值导航方法及其应用
CN109959377A (zh) * 2017-12-25 2019-07-02 北京东方兴华科技发展有限责任公司 一种机器人导航定位系统及方法
CN111487960A (zh) * 2020-03-26 2020-08-04 北京控制工程研究所 一种基于定位能力估计的移动机器人路径规划方法
CN111721290A (zh) * 2020-07-13 2020-09-29 南京理工大学 一种多源传感器信息融合定位切换方法
CN112304318A (zh) * 2020-11-10 2021-02-02 河北工业大学 一种虚实耦合约束环境下的机器人自主导航方法
JP2021064241A (ja) * 2019-10-16 2021-04-22 トヨタ自動車株式会社 物品搬送システム

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109959377A (zh) * 2017-12-25 2019-07-02 北京东方兴华科技发展有限责任公司 一种机器人导航定位系统及方法
CN108519615A (zh) * 2018-04-19 2018-09-11 河南科技学院 基于组合导航和特征点匹配的移动机器人自主导航方法
CN109801358A (zh) * 2018-12-06 2019-05-24 宁波市电力设计院有限公司 一种基于slam扫描和点云影像融合的变电站三维勘测方法
CN109916393A (zh) * 2019-03-29 2019-06-21 电子科技大学 一种基于机器人位姿的多重栅格值导航方法及其应用
JP2021064241A (ja) * 2019-10-16 2021-04-22 トヨタ自動車株式会社 物品搬送システム
CN111487960A (zh) * 2020-03-26 2020-08-04 北京控制工程研究所 一种基于定位能力估计的移动机器人路径规划方法
CN111721290A (zh) * 2020-07-13 2020-09-29 南京理工大学 一种多源传感器信息融合定位切换方法
CN112304318A (zh) * 2020-11-10 2021-02-02 河北工业大学 一种虚实耦合约束环境下的机器人自主导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
包加桐: "基于云平台地图的侦察机器人室外自主导航" *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114608549A (zh) * 2022-05-10 2022-06-10 武汉智会创新科技有限公司 一种基于智能机器人的建筑测量方法

Also Published As

Publication number Publication date
CN114217622B (zh) 2023-09-01

Similar Documents

Publication Publication Date Title
CN111402339B (zh) 一种实时定位方法、装置、系统及存储介质
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
Georgiev et al. Localization methods for a mobile robot in urban environments
CN112268559B (zh) 复杂环境下融合slam技术的移动测量方法
CN113126618B (zh) 一种越野环境下的无人驾驶全局路径规划及重规划方法
ES2514441T3 (es) Procedimiento de planificación de una trayectoria
Rekleitis Cooperative localization and multi-robot exploration
JP5018458B2 (ja) 座標補正方法、座標補正プログラム、及び自律移動ロボット
Toledo-Moreo et al. Fusing GNSS, dead-reckoning, and enhanced maps for road vehicle lane-level navigation
JPH10501908A (ja) 自律可動式装置のオリエンテーション、ルート選定及び制御のための方法
CN113654555A (zh) 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
JPH01205205A (ja) 軌跡プランニング方法および装置
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN113674412B (zh) 基于位姿融合优化的室内地图构建方法、系统及存储介质
Delobel et al. A real-time map refinement method using a multi-sensor localization framework
CN110542916A (zh) 卫星与视觉紧耦合定位方法、系统及介质
CN114217622B (zh) 一种基于bim的机器人自主导航方法
JP6801269B2 (ja) 自律移動装置
CN112506200A (zh) 机器人定位方法、装置、机器人及存储介质
Li et al. Hybrid filtering framework based robust localization for industrial vehicles
KR100446195B1 (ko) 3차원 위치 측정 장치 및 그 방법
CN115639823A (zh) 崎岖起伏地形下机器人地形感知与移动控制方法及系统
CN116047565A (zh) 一种多传感器数据融合定位系统
CN113639722B (zh) 连续激光扫描配准辅助惯性定位定姿方法
Hussnain et al. Enhanced trajectory estimation of mobile laser scanners using aerial images

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