CN116339347B - 一种无人车运行路径规划方法、装置及设备 - Google Patents

一种无人车运行路径规划方法、装置及设备 Download PDF

Info

Publication number
CN116339347B
CN116339347B CN202310456491.XA CN202310456491A CN116339347B CN 116339347 B CN116339347 B CN 116339347B CN 202310456491 A CN202310456491 A CN 202310456491A CN 116339347 B CN116339347 B CN 116339347B
Authority
CN
China
Prior art keywords
sampling point
unmanned vehicle
function
gesture
preset
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
CN202310456491.XA
Other languages
English (en)
Other versions
CN116339347A (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.)
Guangdong University of Technology
Original Assignee
Guangdong University of 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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202310456491.XA priority Critical patent/CN116339347B/zh
Publication of CN116339347A publication Critical patent/CN116339347A/zh
Application granted granted Critical
Publication of CN116339347B publication Critical patent/CN116339347B/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/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • 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/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
    • 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/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
    • 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)
  • 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

本申请公开了一种无人车运行路径规划方法、装置及设备，方法包括：基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵；根据姿态变换矩阵将环境点云数据映射至全局点云地图后，基于预设离散动力学模型计算下一时刻的姿态，得到姿态采样点集合；通过预设代价函数在姿态采样点集合中选取最优采样点，预设代价函数包括势力场函数和启发函数；以最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆；根据拓展圆和最优采样点生成无人车在运行环境中的运行路径。本申请能解决现有技术点云地图生成过程计算量太大，且不适用于未知环境，导致实际的无人车集群路径规划效率很差的技术问题。

Description

一种无人车运行路径规划方法、装置及设备
技术领域
本申请涉及无人车技术领域，尤其涉及一种无人车运行路径规划方法、装置及设备。
背景技术
物流无人车在货物搬运、巡航安检等服务业邻域得到了广泛的应用，利用无人车进行救援、补给、探索等任务一直是研究的热点，基于无人车组成无人车集群，通过通信系统配合协同，可充分发挥无人车的优势以满足复杂多样的任务需求。但这也对无人车的单体控制提出了更高的要求，在多无人车集群在协同执行任务的过程中某台无人车的碰撞可能会导致无人车集群工作系统的瘫痪，所以单体无人车的安全行使是整个无人车集群完成工作的重要前提。
在其中，无人车的可行安全空间的构建是研究的重点。为了构建无人车的安全行使空间，通常做法是获取周围环境的点云分布，并在此基础上构建行驶区域周围的环境的地图，后在生成的地图中采用路径搜索算法，通过对搜索到的路径进行膨胀就可以得到安全可行的空间。但在实际的运行中，由点云信息生成地图的过程会耗费大量的算力，倘若减少迭代的时间会极大地降低地图的精度，由此算得的安全区域可靠性不高；单无人车计算资源的浪费会导致无人车集群效率的急剧下降；此外，对未知环境的安全路径生成不适用，且搜索效率较差。
发明内容
本申请提供了一种无人车运行路径规划方法、装置及设备，用于解决现有技术点云地图生成过程计算量太大，且不适用于未知环境，导致实际的无人车集群路径规划效率很差的技术问题。
有鉴于此，本申请第一方面提供了一种无人车运行路径规划方法，包括：
基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵；
根据所述姿态变换矩阵将所述环境点云数据映射至全局点云地图后，基于预设离散动力学模型计算下一时刻的姿态，得到姿态采样点集合；
通过预设代价函数在所述姿态采样点集合中选取最优采样点，所述预设代价函数包括势力场函数和启发函数；
以所述最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆；
根据所述拓展圆和所述最优采样点生成无人车在运行环境中的运行路径，所述运行路径包括运行安全区域。
优选地，所述基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵，包括：
通过预设特征提取函数在无人车运行环境的环境点云数据中提取线点和面点；
基于不同时刻的两组所述线点和面点计算激光雷达的相对姿态变换，所述相对姿态变换包括相对变换线点和相对变换面点；
根据所述相对变换线点和相对变换面点构建非线性函数，并求解出激光雷达的姿态变换矩阵。
优选地，所述基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵，之前还包括：
通过激光雷达获取无人车运行环境的点云信息，得到环境点云数据。
优选地，所述通过预设代价函数在所述姿态采样点集合中选取最优采样点，所述预设代价函数包括势力场函数和启发函数，包括：
基于所述姿态采样点集合中采样点产生的势力场构建势力场函数；
根据所述势力场函数和启发函数构建采样点的代价函数，得到预设代价函数；
以所述预设函数求解最小值为依据，在所述姿态采样点集合中选取最优采样点。
优选地，所述以所述最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆，之前还包括：
采用KD-tree算法搜索所述最优采样点的近邻点，确定最邻近采样点；
根据所述最优采样点与所述最邻近采样点计算预置半径。
本申请第二方面提供了一种无人车运行路径规划装置，包括：
姿态估计单元，用于基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵；
姿态采集单元，用于根据所述姿态变换矩阵将所述环境点云数据映射至全局点云地图后，基于预设离散动力学模型计算下一时刻的姿态，得到姿态采样点集合；
最优选取单元，用于通过预设代价函数在所述姿态采样点集合中选取最优采样点，所述预设代价函数包括势力场函数和启发函数；
空间拓展单元，用于以所述最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆；
路径规划单元，用于根据所述拓展圆和所述最优采样点生成无人车在运行环境中的运行路径，所述运行路径包括运行安全区域。
优选地，所述姿态估计单元，包括：
特征提取子单元，用于通过预设特征提取函数在无人车运行环境的环境点云数据中提取线点和面点；
姿态变换子单元，用于基于不同时刻的两组所述线点和面点计算激光雷达的相对姿态变换，所述相对姿态变换包括相对变换线点和相对变换面点；
函数计算子单元，用于根据所述相对变换线点和相对变换面点构建非线性函数，并求解出激光雷达的姿态变换矩阵。
优选地，所述最优选取单元，包括：
第一函数构建子单元，用于基于所述姿态采样点集合中采样点产生的势力场构建势力场函数；
第二函数构建子单元，用于根据所述势力场函数和启发函数构建采样点的代价函数，得到预设代价函数；
最优选取计算子单元，用于以所述预设函数求解最小值为依据，在所述姿态采样点集合中选取最优采样点。
优选地，还包括：
邻近搜索单元，用于采用KD-tree算法搜索所述最优采样点的近邻点，确定最邻近采样点；
半径计算单元，用于根据所述最优采样点与所述最邻近采样点计算预置半径。
本申请第三方面提供了一种无人车运行路径规划设备，所述设备包括处理器以及存储器；
所述存储器用于存储程序代码，并将所述程序代码传输给所述处理器；
所述处理器用于根据所述程序代码中的指令执行第一方面所述的无人车运行路径规划方法。
从以上技术方案可以看出，本申请实施例具有以下优点：
本申请中，提供了一种无人车运行路径规划方法，包括：基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵；根据姿态变换矩阵将环境点云数据映射至全局点云地图后，基于预设离散动力学模型计算下一时刻的姿态，得到姿态采样点集合；通过预设代价函数在姿态采样点集合中选取最优采样点，预设代价函数包括势力场函数和启发函数；以最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆；根据拓展圆和最优采样点生成无人车在运行环境中的运行路径，运行路径包括运行安全区域。
本申请提供的无人车运行路径规划方法，基于环境点云数据计算出激光雷达的姿态变换矩阵后，根据姿态变换矩阵将环境点云数据映射到全局点云地图，不需要基于点云数据构建环境地图，能够在一定程度上节省算力。在路径规划过程中，引入无人车离散动力学模型和预设代价函数选取最优采样点，使得基于此生成的拓展圆和运行路径更加准确，对环境的未知与否没有要求，还能保证路径可靠性。因此，本申请能够解决现有技术点云地图生成过程计算量太大，且不适用于未知环境，导致实际的无人车集群路径规划效率很差的技术问题。
附图说明
图1为本申请实施例提供的一种无人车运行路径规划方法的流程示意图；
图2为本申请实施例提供的一种无人车运行路径规划装置的结构示意图；
图3为本申请实施例提供的线点距离计算示意图；
图4为本申请实施例提供的面点距离计算示意图；
图5为本申请实施例提供的无人车离散动力学模型示意图；
图6为本申请实施例提供的点云势力场示意图；
图7为本申请实施例提供的点云势力场仿真示意图；
图8为本申请实施例提供的全局势力场仿真示意图一；
图9为本申请实施例提供的全局势力场仿真示意图二；
图10为本申请实施例提供的最大安全行驶空间圆示意图一；
图11为本申请实施例提供的最大安全行驶空间圆示意图二；
图12为本申请实施例提供的无人车运行路径示意图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
为了便于理解，请参阅图1，本申请提供的一种无人车运行路径规划方法的实施例，包括：
步骤101、基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵。
进一步地，步骤101，包括：
通过预设特征提取函数在无人车运行环境的环境点云数据中提取线点和面点；
基于不同时刻的两组线点和面点计算激光雷达的相对姿态变换，相对姿态变换包括相对变换线点和相对变换面点；
根据相对变换线点和相对变换面点构建非线性函数，并求解出激光雷达的姿态变换矩阵。
本实施例中的环境点云数据是采用激光雷达获取的，也可以是带深度信息的摄像头(RGBD)，具体的操作原理相同，具体的在此不作限定。提取线点和面点的过程实质就是基于点云信息提取特征点的过程，本实施例定义的预设特征提取函数表达为：
其中，分别为第k次扫描点i和点j的位置，ζ为比例系数，用来增加区别度，λ为同义词扫描中获取点i的连续点集，s为点i到附近连续点集j的平均距离。得到的s值较大时则判断为线点，较小则判断为面点。
相对姿态变换是对于激光雷达而言，由于无人车在执行过程中会带动激光雷达处于运动状态，这给构建准确点云带来了影响，所以需要计算出相对变换状态，确保点云的准确性。本实施例通过前后两个时刻的两组线点和面点数据进行相对姿态变换的计算，请参阅图3，对于线点：
其中，为第k次扫描所得的线点，/>分别为第k-1次扫描所得的两个点云线点，根据几何关系可以求得/>到/>所在直线的距离为dL。
对于面点，请参阅图4：
其中，为第k-1次扫描得到的三个面点，可以构成一个面，/>为第k次扫描得到的面点，该点到三个面点形成的平面的距离即为dS。
将相对变换线点dL和相对变换面点dS进行累加，可以得到激光雷达姿态相对变换与线面的距离d，基于此可以构建非线性函数：
其中，为[tk,t]区间内激光雷达的姿态变换矩阵，/>其中，Δx、Δy、Δθx、Δθy分别为激光雷达坐标系x、y、θx、θy的偏移量，η为求解算法确定的相关系数，其中J为偏导函数，表达为：
采用Levenberg-Marquardt算法求解上述非线性函数，使其最小化，从而得到姿态变换矩阵
此外，本实施例的点云相关数据均采取KD-tree数据格式存储，便于后续进行近邻查找。而且，为了证实本实施例中求得的姿态变换矩阵的准确性，还可以采用点云匹配算法ICP计算出相同场景下的激光雷达的姿态验证矩阵，基于姿态验证矩阵对姿态变换矩阵进行校验，校验通过则可以证实本实施例求得的姿态变换矩阵的可靠性。
进一步地，步骤101，之前还包括：
通过激光雷达获取无人车运行环境的点云信息，得到环境点云数据。
步骤102、根据姿态变换矩阵将环境点云数据映射至全局点云地图后，基于预设离散动力学模型计算下一时刻的姿态，得到姿态采样点集合。
需要说明的是，本实施例映射过程并非基于点云数据构建环境地图的过程，而是基于姿态变换矩阵调整点云数据表达的过程，省去了环境地图重建的过程能够最大程度的节省重建过程的算力。
预设离散动力学模型的构建过程是：
由于无人车的运动受力学限制，考虑无人车的阿克曼转向运动模型为：
其中，指的是下一个状态x、y坐标的相对变化，θ为前轮转角，即无人车正方向与x正方向的夹角，/>为下一状态转角大小。此处的输入控制量为后轮速度v和前轮转角θ，前轮与后轮的轴距为L，在全局坐标系下对应的里程计算为：
xt+1、yt+1、θt+1为在初始位置与角度分别为xt、yt、θt，输入控制量为后轮速度vt与前轮角速度ωt时在全局坐标系下的姿态表示。记t时刻无人车的姿态表示为Zt＝[xt,yt,θt]T，记t时刻下的初始系数矩阵为：
输入矩阵为Ut＝[vt,wt]T，那么里程计算过程可以表达为：
Zt+1＝Zt+MtUt
设在时间dt内，后轮速度vt与前轮角速度ωt最大变化量为Δv、Δ，为了得到符合无人车动力学的离散模型，将Δv均分为n份，将Δω均分为m份。在t时刻有vt,i∈{vt,1,vt,2,......,vt,n-1,vt,n}，ωt,j∈{ωt,1,ωt,2,......,ωt,m-1,ωt,m}，记在输入为vt,i、wt,j时无人车在t+1时刻的姿态为Zt+1(vt,i,ωt,j)。
得到的预设离散动力学模型表达为：
请参阅图5，是在t时刻在不同的控制输入量vt,i,ωt,j的情况下所得到的动力学离散姿态集合，/>U(vt,i,ωt,j)表示输入为vt,i、wt,j时的输入矩阵。经过动力学输入离散后可以得到n×m个不同在t+1时刻的姿态从而形成姿态采样点集合，集合大小是n×m。
步骤103、通过预设代价函数在姿态采样点集合中选取最优采样点，预设代价函数包括势力场函数和启发函数。
进一步地，步骤103，包括：
基于姿态采样点集合中采样点产生的势力场构建势力场函数；
根据势力场函数和启发函数构建采样点的代价函数，得到预设代价函数；
以预设函数求解最小值为依据，在姿态采样点集合中选取最优采样点。
基于以上可以得到t+1时刻n×m个不同的姿态请参阅图6，每一个姿态是由t时刻的姿态下在不同输入的控制量vt,i、wt,j下所得到采样点，本实施例通过定义预设代价函数在这些采样点中选取出最优采样点。
请参阅图6，根据姿态采样点集合中采样点产生的势力场可以初步构建势力场初始函数：
为在全局坐标系下输入vt,i、wt,j所得到的位置，/>为某点云O对距离为的位置Pi,j所产生的斥力，Re为当前距离范围，/>为障碍物点云作用的有效范围，ε为斥增益常量当点云距离位置Pi,j的距离/>超出此有效范围不产生斥力。
若记姿态采样点集合为S，点O为其中一个点，有效距离内的点云对点的斥力之和表达为：
即为可以用来构建代价函数的势力场函数。请参阅图7，是采用仿真软件仿真得到的点云势力场仿真图，图8为存在目的地的目标势力场仿真图，图9为全局势力场仿真图。
为了能更快有效地生成到达目标点的安全路径，添加当前位置(x,y)距离目标点(xgoal,ygoal)曼哈顿距离作为启发式函数h(Pi,j)，可以通过此构建引力场：
h(Pi,j)＝|xgoal-x|+|ygoal-y|
结合势力场函数和启发函数h(Pi,j)，就可以得到预设代价函数：
以上预设代价函数的物理意义是，在平面上的所有点都受到了范围内每个选取点与目标点的影响。对于采样点Pi,j，点云势力场函数的值越小则表示采样点Pi,j离选取点的距离越远，启发函数h(Pi,j)的值越小则表示采样点Pi,j离终点距离越近。f(Pi,j)的值越小则代表采样点Pi,j离选取点越远与终点越近。所以我们可以计算图6中采样点集中每一个离散点Pi,j的代价函数f(Pi,j)，取f(Pi,j)值最小的点作为t+1时刻的最优采样点，每次的下一个时刻都可以选取出这样的最优采样点。
步骤104、以最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆。
进一步地，步骤104，之前还包括：
采用KD-tree算法搜索最优采样点的近邻点，确定最邻近采样点；
根据最优采样点与最邻近采样点计算预置半径。
以最优采样点为圆心，以最优采样点与最邻近采样点之间的距离为预置半径，可以作圆，这个圆内区域即为拓展出的安全空间，即拓展圆。最邻近采样点是距离最优采样点最近的点，是根据KD-tree算法进行近邻搜索得到的；预置半径是基于两个点的点云信息计算得到的距离值。
步骤105、根据拓展圆和最优采样点生成无人车在运行环境中的运行路径，运行路径包括运行安全区域。
请参阅图10、图11和图12，可以依据本实施例中的算法获取每个下一时刻t+1的最优采样点，并基于最优采样点找到拓展圆，最后可以生成多个交叠的圆，形成最大的安全行使空间，这个空间就是可以规划无人车运行路径的区域，不论是在未知环境还是在已知环境，这一方法均可性，适用性更强，路径规划应用效果更好；每个拓展圆的中心点，即最优采样点连接起来就可以生成安全环境中的运行路径。
为了便于理解，本实施例基于MATLAB进行逐步仿真，采用随机数生成的方式生成随机点，将随机点模拟传感器得到的点云信息，考虑到模型误差，当算法搜索到距离目标点一定范围时，就判定到达目的地，已经得到规划的路径。
本申请提供的无人车运行路径规划方法，基于环境点云数据计算出激光雷达的姿态变换矩阵后，根据姿态变换矩阵将环境点云数据映射到全局点云地图，不需要基于点云数据构建环境地图，能够在一定程度上节省算力。在路径规划过程中，引入无人车离散动力学模型和预设代价函数选取最优采样点，使得基于此生成的拓展圆和运行路径更加准确，对环境的未知与否没有要求，还能保证路径可靠性。因此，本申请能够解决现有技术点云地图生成过程计算量太大，且不适用于未知环境，导致实际的无人车集群路径规划效率很差的技术问题。
为了便于理解，请参阅图2，本申请提供了一种无人车运行路径规划装置的实施例，包括：
姿态估计单元201，用于基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵；
姿态采集单元202，用于根据姿态变换矩阵将环境点云数据映射至全局点云地图后，基于预设离散动力学模型计算下一时刻的姿态，得到姿态采样点集合；
最优选取单元203，用于通过预设代价函数在姿态采样点集合中选取最优采样点，预设代价函数包括势力场函数和启发函数；
空间拓展单元204，用于以最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆；
路径规划单元205，用于根据拓展圆和最优采样点生成无人车在运行环境中的运行路径，运行路径包括运行安全区域。
进一步地，姿态估计单元201，包括：
特征提取子单元2011，用于通过预设特征提取函数在无人车运行环境的环境点云数据中提取线点和面点；
姿态变换子单元2012，用于基于不同时刻的两组线点和面点计算激光雷达的相对姿态变换，相对姿态变换包括相对变换线点和相对变换面点；
函数计算子单元2013，用于根据相对变换线点和相对变换面点构建非线性函数，并求解出激光雷达的姿态变换矩阵。
进一步地，最优选取单元203，包括：
第一函数构建子单元2031，用于基于姿态采样点集合中采样点产生的势力场构建势力场函数；
第二函数构建子单元2032，用于根据势力场函数和启发函数构建采样点的代价函数，得到预设代价函数；
最优选取计算子单元2033，用于以预设函数求解最小值为依据，在姿态采样点集合中选取最优采样点。
进一步地，还包括：
邻近搜索单元206，用于采用KD-tree算法搜索最优采样点的近邻点，确定最邻近采样点；
半径计算单元207，用于根据最优采样点与最邻近采样点计算预置半径。
本申请还提供了一种无人车运行路径规划设备，设备包括处理器以及存储器；
存储器用于存储程序代码，并将程序代码传输给处理器；
处理器用于根据程序代码中的指令执行上述方法实施例中的无人车运行路径规划方法。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以通过一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(英文全称：Read-OnlyMemory，英文缩写：ROM)、随机存取存储器(英文全称：Random Access Memory，英文缩写：RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (8)

1.一种无人车运行路径规划方法，其特征在于，包括：
基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵；
根据所述姿态变换矩阵将所述环境点云数据映射至全局点云地图后，基于预设离散动力学模型计算下一时刻的姿态，得到姿态采样点集合，所述预设离散动力学模型表达为：
其中，是在t时刻在不同的控制输入量vt,i,ωt,j的情况下所得到的动力学离散姿态集合，/>U(vt,i,ωt,j)表示输入为vt,i、wt,j时的输入矩阵，Mt为初始系数矩阵；经过动力学输入离散后得到n×m个不同在t+1时刻的姿态/>从而形成姿态采样点集合，集合大小是n×m；
通过预设代价函数在所述姿态采样点集合中选取最优采样点，所述预设代价函数包括势力场函数和启发函数，选取过程具体为：
基于所述姿态采样点集合中采样点产生的势力场构建势力场函数；
根据所述势力场函数和启发函数构建采样点的代价函数，得到预设代价函数；
以所述预设代价函数求解最小值为依据，在所述姿态采样点集合中选取最优采样点；
所述预设代价函数表达为：
其中，为基于点云矩阵位置Pi,j构建的所述势力场函数，h(Pi,j)为所述启发函数，/>表达为：
h(Pi,j)表达为：
h(Pi,j)＝|xgoal-x|+|ygoal-y|
其中，为某点云O对距离为/>的位置Pi,j所产生的斥力，S为点云集，(x,y)为当前位置坐标，(xgoal,ygoal)为目标点位置坐标；
以所述最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆；
根据所述拓展圆和所述最优采样点生成无人车在运行环境中的运行路径，所述运行路径包括运行安全区域。
2.根据权利要求1所述的无人车运行路径规划方法，其特征在于，所述基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵，包括：
通过预设特征提取函数在无人车运行环境的环境点云数据中提取线点和面点；
基于不同时刻的两组所述线点和面点计算激光雷达的相对姿态变换，所述相对姿态变换包括相对变换线点和相对变换面点；
根据所述相对变换线点和相对变换面点构建非线性函数，并求解出激光雷达的姿态变换矩阵。
3.根据权利要求1所述的无人车运行路径规划方法，其特征在于，所述基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵，之前还包括：
通过激光雷达获取无人车运行环境的点云信息，得到环境点云数据。
4.根据权利要求1所述的无人车运行路径规划方法，其特征在于，所述以所述最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆，之前还包括：
采用KD-tree算法搜索所述最优采样点的近邻点，确定最邻近采样点；
根据所述最优采样点与所述最邻近采样点计算预置半径。
5.一种无人车运行路径规划装置，其特征在于，包括：
姿态估计单元，用于基于无人车运行环境的环境点云数据从线点和面点的角度估计激光雷达的姿态变换，得到姿态变换矩阵；
姿态采集单元，用于根据所述姿态变换矩阵将所述环境点云数据映射至全局点云地图后，基于预设离散动力学模型计算下一时刻的姿态，得到姿态采样点集合，所述预设离散动力学模型表达为：
其中，是在t时刻在不同的控制输入量vt,i,ωt,j的情况下所得到的动力学离散姿态集合，/>U(vt,i,ωt,j)表示输入为vt,i、wt,j时的输入矩阵，Mt为初始系数矩阵；经过动力学输入离散后得到n×m个不同在t+1时刻的姿态/>从而形成姿态采样点集合，集合大小是n×m；
最优选取单元，用于通过预设代价函数在所述姿态采样点集合中选取最优采样点，所述预设代价函数包括势力场函数和启发函数，所述最优选取单元，包括：
第一函数构建子单元，用于基于所述姿态采样点集合中采样点产生的势力场构建势力场函数；
第二函数构建子单元，用于根据所述势力场函数和启发函数构建采样点的代价函数，得到预设代价函数；
最优选取计算子单元，用于以所述预设代价函数求解最小值为依据，在所述姿态采样点集合中选取最优采样点；
所述预设代价函数表达为：
其中，为基于点云矩阵位置Pi,j构建的所述势力场函数，h(Pi,j)为所述启发函数，/>表达为：
h(Pi,j)表达为：
h(Pi,j)＝|xgoal-x|+|ygoal-y|
其中，为某点云O对距离为/>的位置Pi,j所产生的斥力，S为点云集，(x,y)为当前位置坐标，(xgoal,ygoal)为目标点位置坐标；
空间拓展单元，用于以所述最优采样点为圆心，根据预置半径进行圆安全空间拓展操作，得到拓展圆；
路径规划单元，用于根据所述拓展圆和所述最优采样点生成无人车在运行环境中的运行路径，所述运行路径包括运行安全区域。
6.根据权利要求5所述的无人车运行路径规划装置，其特征在于，所述姿态估计单元，包括：
特征提取子单元，用于通过预设特征提取函数在无人车运行环境的环境点云数据中提取线点和面点；
姿态变换子单元，用于基于不同时刻的两组所述线点和面点计算激光雷达的相对姿态变换，所述相对姿态变换包括相对变换线点和相对变换面点；
函数计算子单元，用于根据所述相对变换线点和相对变换面点构建非线性函数，并求解出激光雷达的姿态变换矩阵。
7.根据权利要求5所述的无人车运行路径规划装置，其特征在于，还包括：
邻近搜索单元，用于采用KD-tree算法搜索所述最优采样点的近邻点，确定最邻近采样点；
半径计算单元，用于根据所述最优采样点与所述最邻近采样点计算预置半径。
8.一种无人车运行路径规划设备，其特征在于，所述设备包括处理器以及存储器；
所述存储器用于存储程序代码，并将所述程序代码传输给所述处理器；
所述处理器用于根据所述程序代码中的指令执行权利要求1-4任一项所述的无人车运行路径规划方法。
CN202310456491.XA 2023-04-24 2023-04-24 一种无人车运行路径规划方法、装置及设备 Active CN116339347B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310456491.XA CN116339347B (zh) 2023-04-24 2023-04-24 一种无人车运行路径规划方法、装置及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310456491.XA CN116339347B (zh) 2023-04-24 2023-04-24 一种无人车运行路径规划方法、装置及设备

Publications (2)

Publication Number Publication Date
CN116339347A CN116339347A (zh) 2023-06-27
CN116339347B true CN116339347B (zh) 2023-10-31

Family

ID=86891394

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310456491.XA Active CN116339347B (zh) 2023-04-24 2023-04-24 一种无人车运行路径规划方法、装置及设备

Country Status (1)

Country Link
CN (1) CN116339347B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117346791B (zh) * 2023-12-01 2024-03-22 北京科技大学 基于视觉图像的智能轮椅路径规划方法及系统

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102591332A (zh) * 2011-01-13 2012-07-18 同济大学 用于无人驾驶汽车局部路径规划的装置及方法
CN110361026A (zh) * 2019-06-05 2019-10-22 华南理工大学 一种基于3d点云的仿人机器人路径规划方法
CN111230888A (zh) * 2020-03-23 2020-06-05 江苏科技大学 一种基于rgbd相机的上肢外骨骼机器人避障方法
CN111721297A (zh) * 2020-06-19 2020-09-29 重庆大学 一种智能车库多agv的路径规划方法
CN112577491A (zh) * 2020-12-14 2021-03-30 上海应用技术大学 一种基于改进人工势场法的机器人路径规划方法
CN113776519A (zh) * 2021-09-14 2021-12-10 西南科技大学 一种无光动态开放环境下agv车辆建图与自主导航避障方法
CN113791619A (zh) * 2021-09-14 2021-12-14 北京航空航天大学 一种机场自动驾驶牵引车调度导航系统及方法
KR20220021158A (ko) * 2020-08-13 2022-02-22 한국전력공사 무인항공기의 비행경로 생성장치 및 그 방법
CN114442637A (zh) * 2022-02-10 2022-05-06 北京理工大学 一种无人车局部动态避障路径规划方法
CN115540850A (zh) * 2022-09-05 2022-12-30 广东工业大学 一种激光雷达与加速度传感器结合的无人车建图方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102591332A (zh) * 2011-01-13 2012-07-18 同济大学 用于无人驾驶汽车局部路径规划的装置及方法
CN110361026A (zh) * 2019-06-05 2019-10-22 华南理工大学 一种基于3d点云的仿人机器人路径规划方法
CN111230888A (zh) * 2020-03-23 2020-06-05 江苏科技大学 一种基于rgbd相机的上肢外骨骼机器人避障方法
CN111721297A (zh) * 2020-06-19 2020-09-29 重庆大学 一种智能车库多agv的路径规划方法
KR20220021158A (ko) * 2020-08-13 2022-02-22 한국전력공사 무인항공기의 비행경로 생성장치 및 그 방법
CN112577491A (zh) * 2020-12-14 2021-03-30 上海应用技术大学 一种基于改进人工势场法的机器人路径规划方法
CN113776519A (zh) * 2021-09-14 2021-12-10 西南科技大学 一种无光动态开放环境下agv车辆建图与自主导航避障方法
CN113791619A (zh) * 2021-09-14 2021-12-14 北京航空航天大学 一种机场自动驾驶牵引车调度导航系统及方法
CN114442637A (zh) * 2022-02-10 2022-05-06 北京理工大学 一种无人车局部动态避障路径规划方法
CN115540850A (zh) * 2022-09-05 2022-12-30 广东工业大学 一种激光雷达与加速度传感器结合的无人车建图方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于改进人工势场法的飞行器轨迹规划;范世鹏;吴广;王亮;刘运鹏;李伶;祁琪;;航天控制(01);第51-55页 *
无人驾驶路径规划算法综述;海振洋, 王健, 牟思凯等.;《农业装备与 车辆工程》;第60卷(第11期);第142-146页 *

Also Published As

Publication number Publication date
CN116339347A (zh) 2023-06-27

Similar Documents

Publication Publication Date Title
CN111971574B (zh) 用于自动驾驶车辆的lidar定位的基于深度学习的特征提取
EP3714290B1 (en) Lidar localization using 3d cnn network for solution inference in autonomous driving vehicles
EP3714285B1 (en) Lidar localization using rnn and lstm for temporal smoothness in autonomous driving vehicles
He et al. Non-cooperative spacecraft pose tracking based on point cloud feature
Li et al. Autonomous underwater vehicle optimal path planning method for seabed terrain matching navigation
Babinec et al. VFH* TDT (VFH* with Time Dependent Tree): A new laser rangefinder based obstacle avoidance method designed for environment with non-static obstacles
CN108230361A (zh) 用无人机探测器和追踪器融合来增强目标追踪方法及系统
Ayawli et al. Path planning for mobile robots using Morphological Dilation Voronoi Diagram Roadmap algorithm
Zhang et al. Monocular vision based obstacle avoidance trajectory planning for Unmanned Aerial Vehicle
CN116339347B (zh) 一种无人车运行路径规划方法、装置及设备
Wang et al. End-to-end interactive prediction and planning with optical flow distillation for autonomous driving
CN112166458A (zh) 目标检测与跟踪方法、系统、设备及存储介质
US20240210958A1 (en) Reactive Collision Avoidance For Autonomous Vehicles Considering Physical Constraints
Naujoks et al. Fast 3D extended target tracking using NURBS surfaces
CN115166716A (zh) 一种基于多元数据融合的地面无人平台跟踪方法
CN105844672A (zh) 一种多关节模型的快速连续碰撞检测的方法
Geng et al. Robo-Centric ESDF: A fast and accurate whole-body collision evaluation tool for any-shape robotic planning
Wang et al. A vector polar histogram method based obstacle avoidance planning for AUV
Xiao et al. Reinforcement learning-driven dynamic obstacle avoidance for mobile robot trajectory tracking
Padial et al. Tumbling target reconstruction and pose estimation through fusion of monocular vision and sparse-pattern range data
Cho et al. Real-time 3D reconstruction method using massive multi-sensor data analysis and fusion
CN115981313A (zh) 机器人路径规划方法、系统、电子设备及存储介质
Cocaud et al. SLAM-based navigation scheme for pinpoint landing on small celestial body
CN112747752A (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质
Törő et al. Cooperative object detection in road traffic

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
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Chen Longsheng

Inventor after: Meng Wei

Inventor after: Guo Chuangpeng

Inventor before: Meng Wei

Inventor before: Chen Longsheng

Inventor before: Guo Chuangpeng