CN107144292A - The odometer method and mileage counter device of a kind of sports equipment - Google Patents
The odometer method and mileage counter device of a kind of sports equipment Download PDFInfo
- Publication number
- CN107144292A CN107144292A CN201710426765.5A CN201710426765A CN107144292A CN 107144292 A CN107144292 A CN 107144292A CN 201710426765 A CN201710426765 A CN 201710426765A CN 107144292 A CN107144292 A CN 107144292A
- Authority
- CN
- China
- Prior art keywords
- pose
- laser
- current moment
- map
- frame
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 230000033001 locomotion Effects 0.000 claims abstract description 82
- 238000004364 calculation method Methods 0.000 claims description 25
- 230000009466 transformation Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 9
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000001186 cumulative effect Effects 0.000 abstract description 3
- 230000008859 change Effects 0.000 description 10
- 238000005516 engineering process Methods 0.000 description 10
- 125000000205 L-threonino group Chemical group [H]OC(=O)[C@@]([H])(N([H])[*])[C@](C([H])([H])[H])([H])O[H] 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000008030 elimination Effects 0.000 description 3
- 238000003379 elimination reaction Methods 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开了一种运动设备的里程计方法和里程计装置,包括获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图;根据当前时刻的位姿,以及第二激光帧与局部点云地图,计算得到当前时刻的最优位姿等步骤,本发明提供的里程计方法,通过码盘转数获得初始相对运动量及位姿,通过迭代近邻算法等进行激光帧间匹配,以此获得较准确的相对运动量和位姿。以上一步获得的位姿为初值,对激光帧与局部点云地图进行匹配,获得更准确的位姿及运动量,并对外输出。本发明提供的里程计装置,解决了现有的现有里程计不精确以及累计误差的不足,可用于众多通用或专用的计算系统环境或配置中。
The invention discloses an odometer method and an odometer device for sports equipment, which include obtaining the pose of the sports equipment at the current moment, a second laser frame at the current moment, and a local point cloud map; according to the pose at the current moment, and the second The laser frame and the local point cloud map are calculated to obtain the optimal pose at the current moment. The odometer method provided by the present invention obtains the initial relative motion and pose through the number of code wheel revolutions, and performs laser frame interpolation through iterative neighbor algorithms. Matching, in order to obtain more accurate relative motion and pose. The pose obtained in the previous step is the initial value, and the laser frame is matched with the local point cloud map to obtain a more accurate pose and motion, and output it externally. The odometer device provided by the invention solves the shortcomings of inaccuracy and cumulative error of existing odometers, and can be used in many general or special computing system environments or configurations.
Description
技术领域technical field
本发明属于运动设备的运动技术领域,特别是涉及运动设备的里程计方法和里程计装置。The invention belongs to the technical field of sports equipment, in particular to an odometer method and an odometer device for the sports equipment.
背景技术Background technique
移动机器人在室内环境中移动时,里程计信息是机器人运动状态的最基本信息。机器人的里程计是一种利用致动器的移动数据来估算机器人位姿随时间改变量的方法。这个位姿变化可由编码器和惯性导航传感器来得到。传统的编码器估计历程方法仅仅利用内部传感器信息的累计得到里程信息,例如一个机器人在其轮子或腿关节处配备有旋转编码器等设备,当它向前移动一段时间后,想要知道大致的移动距离,借助旋转编码器,可以测量出轮子旋转的圈数,如果知道了轮子的周长,便可以计算出机器人移动的距离。When a mobile robot moves in an indoor environment, the odometer information is the most basic information of the robot's motion state. A robot's odometry is a method that uses actuator movement data to estimate how much a robot's pose has changed over time. This pose change can be obtained by encoders and inertial navigation sensors. The traditional encoder estimating method only uses the accumulation of internal sensor information to obtain mileage information. For example, a robot is equipped with rotary encoders at its wheels or leg joints. When it moves forward for a period of time, it wants to know the approximate distance. Moving distance, with the help of a rotary encoder, the number of turns the wheel rotates can be measured, and if the circumference of the wheel is known, the distance the robot moves can be calculated.
但这类里程计总是会遇到精度问题。由于误差是由多种因素混合产生,并且误差随时间的累积导致了里程计的读数随着时间的增加而变得越来越不可靠。But such odometers always suffer from accuracy problems. Since the error is caused by a mixture of many factors, and the accumulation of error over time leads to the odometer reading becoming less and less reliable as time goes by.
发明内容Contents of the invention
为了解决现有技术中里程计精度低误差高的缺陷,本发明提供了一种运动设备的里程计方法以及里程计装置。In order to solve the defect of low precision and high error of the odometer in the prior art, the present invention provides an odometer method and an odometer device for sports equipment.
本发明是通过以下技术方案来实现的:获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图;The present invention is achieved through the following technical solutions: acquiring the pose of the motion equipment at the current moment, the second laser frame at the current moment, and the local point cloud map;
根据当前时刻的位姿,以及第二激光帧与局部点云地图,计算得到当前时刻的最优位姿。According to the pose at the current moment, as well as the second laser frame and the local point cloud map, the optimal pose at the current moment is calculated.
进一步的,以运动设备当前时刻的位姿为初值,对第二激光帧与局部点云地图进行点云匹配得到最优位姿,即所述最优位姿为根据以下公式获得的最优化解,所述公式为:Further, with the current pose of the sports equipment as the initial value, point cloud matching is performed on the second laser frame and the local point cloud map to obtain the optimal pose, that is, the optimal pose is the optimal pose obtained according to the following formula Solution, the formula is:
其中:为局部点云地图构成的曲面;pi为第二激光帧内的激光点;符号代表旋转-平移变换;符号代表点到局部点云地图构成的曲面的欧几里得投影;P为计算过程中的位姿,以当前时刻的位姿为初值。in: is the surface formed by the local point cloud map; p i is the laser point in the second laser frame; the symbol Represents a rotation-translation transformation; the symbol Surfaces representing points to local point cloud maps The Euclidean projection of ; P is the pose during the calculation process, with the pose at the current moment as the initial value.
进一步的,所述第二激光帧为剔除动态激光点后的第二激光帧。Further, the second laser frame is the second laser frame after excluding dynamic laser points.
进一步的,剔除第二激光帧中的动态激光点的步骤包括:根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标;根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标;根据栅格坐标所在栅格的概率值确定该激光点是否为动态激光点,如果是则剔除动态激光点。Further, the step of eliminating the dynamic laser points in the second laser frame includes: obtaining the position coordinates of the laser points in the second laser frame in the local probability grid map according to the pose at the current moment; determining the second laser point according to the position coordinates The grid coordinates of the laser point in the frame on the local probability grid map; determine whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located, and if so, remove the dynamic laser point.
进一步的,所述当前时刻的位姿为较优位姿,具体的,所述较优位姿的获取包括步骤:获取运动设备第一时刻到当前时刻的初始相对运动量;获取第一时刻的第一激光帧以及当前时刻的第二激光帧;根据初始相对运动量,以及第一激光帧与第二激光帧,得到帧间相对运动量;根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿。Further, the pose at the current moment is a better pose. Specifically, the acquisition of the better pose includes the steps of: acquiring the initial relative motion of the sports equipment from the first moment to the current moment; A laser frame and the second laser frame at the current moment; according to the initial relative motion amount, and the first laser frame and the second laser frame, the relative motion amount between frames is obtained; calculated according to the pose of the moving device at the first moment and the relative motion amount between frames The best pose at the current moment.
进一步的,以相对运动量为初值,对第一激光帧与第二激光帧进行帧间匹配得到帧间运动量。Further, with the relative motion amount as an initial value, inter-frame matching is performed on the first laser frame and the second laser frame to obtain an inter-frame motion amount.
进一步地,所述帧间运动量具体通过求解以下公式的最优化解获得,公式为:Further, the inter-frame motion amount is specifically obtained by solving the optimal solution of the following formula, which is:
其中:为第一激光帧构成的曲面;pi为第二激光帧的激光点;q为计算过程中运动设备的相对运动量,以相对运动量为初值;符号代表旋转-平移变换;符号代表点到第一激光帧构成的曲面的欧几里得投影。in: is the curved surface formed by the first laser frame; p i is the laser point of the second laser frame; q is the relative motion of the moving device during the calculation process, with the relative motion as the initial value; the symbol Represents a rotation-translation transformation; the symbol represents the surface formed from the point to the first laser frame Euclidean projection.
进一步的,根据最优位姿与第二激光帧更新地图,包括更新局部点云地图和/或更新局部概率栅格地图。Further, updating the map according to the optimal pose and the second laser frame includes updating the local point cloud map and/or updating the local probability grid map.
进一步的,当相对运动量的位移达到设定阈值时,重置栅格地图,所述重置栅格地图包括重置栅格地图的概率值,重置地图原点、更新局部概率栅格地图。Further, when the displacement of the relative movement amount reaches a set threshold, the grid map is reset, and the grid map reset includes resetting the probability value of the grid map, resetting the origin of the map, and updating the local probability grid map.
本发明还提供一种里程计装置,其技术方案是所述里程计装置包括The present invention also provides an odometer device, the technical solution of which is that the odometer device includes
第一获取模块,获取运动设备当前时刻的位姿;The first acquisition module acquires the pose of the sports equipment at the current moment;
第二获取模块,获取运动设备当前时刻的第二激光帧;The second acquisition module acquires the second laser frame at the current moment of the sports equipment;
第三获取模块,获取局部点云地图;The third acquisition module acquires the local point cloud map;
第一运算模块,根据当前时刻的位姿,以及第二激光帧与局部点云地图,计算得到当前时刻的最优位姿;The first calculation module calculates the optimal pose at the current moment according to the pose at the current moment, as well as the second laser frame and the local point cloud map;
进一步的,所述装置还包括剔除模块,用于剔除第二激光帧内的动态激光点;所述第二获取模块获取剔除动态激光点后的第二激光帧。Further, the device further includes a removing module, configured to remove dynamic laser points in the second laser frame; the second acquisition module obtains the second laser frame after removing the dynamic laser points.
进一步的,所述剔除模块包括Further, the elimination module includes
第一坐标单元,根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标;The first coordinate unit obtains the position coordinates of the laser point in the local probability grid map in the second laser frame according to the pose at the current moment;
第二坐标单元,根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标;The second coordinate unit determines the grid coordinates of the laser point in the second laser frame on the local probability grid map according to the position coordinates;
判断单元,根据栅格坐标所在栅格的概率值判断该激光点是否为动态激光点;The judging unit judges whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located;
剔除单元,判断单元的判断结果剔除动态激光点,即判断结果为动态激光点则剔除。The rejecting unit, the judgment result of the judging unit rejects the dynamic laser spot, that is, the judgment result is a dynamic laser spot, and then the dynamic laser spot is rejected.
进一步的,所述装置还包括地图更新模块,用于根据最优位姿与第二激光帧更新局部点云地图和/或更新局部概率栅格地图。Further, the device further includes a map update module, configured to update the local point cloud map and/or update the local probability grid map according to the optimal pose and the second laser frame.
进一步的,还包括地图重置模块,用于根据运动设备的移动距离重置局部概率栅格地图。Further, a map reset module is also included, which is used to reset the local probability grid map according to the moving distance of the sports equipment.
进一步的,所述第一获取模块具体用于获取当前时刻的较优位姿,包括Further, the first acquisition module is specifically used to acquire a better pose at the current moment, including
第一获取单元,获取运动设备第一时刻到当前时刻的初始相对运动量;The first acquisition unit acquires the initial relative movement amount of the sports equipment from the first moment to the current moment;
第二获取单元,获取第一时刻的第一激光帧以及当前时刻的第二激光帧;The second acquiring unit is configured to acquire the first laser frame at the first moment and the second laser frame at the current moment;
第一计算单元,根据初始相对运动量,以及第一激光帧与第二激光帧,得到帧间相对运动量;The first calculation unit obtains the relative motion between frames according to the initial relative motion, and the first laser frame and the second laser frame;
第二计算单元,根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿;The second calculation unit calculates and obtains the optimal pose at the current moment according to the pose of the moving device at the first moment and the relative movement amount between frames;
本发明还提供另一种里程计装置,包括处理器以及用于存储处理器可执行的指令的存储器;The present invention also provides another odometer device, which includes a processor and a memory for storing instructions executable by the processor;
所述处理器被配置为:The processor is configured to:
获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图,Obtain the pose of the motion equipment at the current moment, the second laser frame at the current moment, and the local point cloud map,
根据当前时刻的位姿,以及第二激光帧与局部点云地图,得到当前时刻的最优位姿。本发明的有益效果如下:According to the pose at the current moment, as well as the second laser frame and the local point cloud map, the optimal pose at the current moment is obtained. The beneficial effects of the present invention are as follows:
本发明提供的里程计方法,通过码盘转数获得初始相对运动量及位姿,通过迭代近邻算法等进行激光帧间匹配,以此获得较准确的相对运动量和位姿。以上一步获得的位姿为初值,对激光帧与局部点云地图进行匹配,获得更准确的位姿及运动量,并对外输出。本方法无需先验全局地图,能有效抑制里程计的累积误差,对动态环境有较好的适应性;大范围闭环(200米)误差在1米之内。The odometer method provided by the present invention obtains the initial relative movement amount and pose through the rotation number of the code wheel, and performs laser frame-to-frame matching through an iterative neighbor algorithm to obtain more accurate relative movement amount and pose. The pose obtained in the previous step is the initial value, and the laser frame is matched with the local point cloud map to obtain a more accurate pose and motion, and output it externally. This method does not require a priori global map, can effectively suppress the cumulative error of the odometer, and has good adaptability to dynamic environments; the error of a large-scale closed loop (200 meters) is within 1 meter.
本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品可以存储在存储介质中,如ROM/RAM、磁碟、光盘等,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)。The essence of the technical solution of the present invention or the part that contributes to the prior art can be embodied in the form of software products, and the computer software products can be stored in storage media, such as ROM/RAM, magnetic disks, optical disks, etc., including Several instructions are used to make a computer device (it may be a personal computer, a server, or a network device, etc.).
本发明提供的里程计装置,解决了现有的现有里程计不精确以及累计误差的不足,可用于众多通用或专用的计算系统环境或配置中。例如:个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等。The odometer device provided by the invention solves the inaccuracy and accumulated errors of existing odometers, and can be used in many general or special computing system environments or configurations. Examples: personal computers, server computers, handheld or portable devices, tablet-type devices, multiprocessor systems, microprocessor-based systems, set-top boxes, programmable consumer electronics, network PCs, minicomputers, mainframe computers, including the above A distributed computing environment for any system or device, and more.
附图说明Description of drawings
图1是本发明里程计方法实施例1的流程示意图;Fig. 1 is the schematic flow sheet of embodiment 1 of odometer method of the present invention;
图2是本发明里程计方法实施例1获得较优当前位姿的流程示意图;Fig. 2 is a schematic flow diagram of obtaining a better current pose in Embodiment 1 of the odometer method of the present invention;
图3是本发明里程计方法实施例2的流程示意图;Fig. 3 is a schematic flow chart of embodiment 2 of the odometer method of the present invention;
图4是本发明里程计装置的模块构成示意图。Fig. 4 is a schematic diagram of the module structure of the odometer device of the present invention.
具体实施方式detailed description
结合附图以及具体实施方式,对本发明做进一步描述,需要说明的是,在不相冲突的前提下,以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例:The present invention will be further described in conjunction with the accompanying drawings and specific implementation methods. It should be noted that, on the premise of not conflicting, the various embodiments described below or the technical features can be combined arbitrarily to form new embodiments:
实施例一Embodiment one
图1是本实施例提供的里程计方法的流程示意图。Fig. 1 is a schematic flowchart of the odometer method provided in this embodiment.
本方法应用的场景中的设备包括运动设备,运算设备,以及设置在运动设备上的激光扫描设备与物理里程设备。运动设备可以是移动机器人、扫地机器人、机器玩具或者需要在室内移动并定位的智能家居等。运算设备用于对各种数据进行计算,数据来源为运动设备、激光扫描设备、物理里程设备等。物理里程设备用于检测获得运动设备的运动数据来估算运动设备位姿随时间改变量,包括位置改变量、角度改变量等,一般设置于运动设备上,例如码盘,其他光电编码器,或者其他可以测量在一段时间内运动设备的位移量的检测元件。The devices in the application scene of this method include sports equipment, computing equipment, and laser scanning equipment and physical mileage equipment set on the sports equipment. Sports equipment can be mobile robots, sweeping robots, robotic toys, or smart homes that need to be moved and positioned indoors. Computing equipment is used to calculate various data, and the data sources are sports equipment, laser scanning equipment, physical mileage equipment, etc. The physical mileage device is used to detect and obtain the motion data of the sports equipment to estimate the change in the pose of the sports equipment over time, including the position change, angle change, etc., and is generally set on the sports equipment, such as a code wheel, other photoelectric encoders, or Other sensing elements that measure the displacement of moving equipment over a period of time.
一般对运动设备如移动机器人设定有一定的活动区域。根据不同的运动设备的可以建立不同的运动模型,常见的有差分驱动的运动模型、全向移动的运动模型。在特定的实施例中具有相邻的两个时刻,即前一时刻和当前时刻,一般根据不同的运动模型以及已知的运动设备前一时刻的位姿(xt-1,yt-1,θt-1),可以获得运动设备当前时刻的位姿(xt,yt,θt),并且可以根据两个时刻的位姿计算得到相对运动量(Δxt,Δyt,Δθt)。例如对于双轮差分的运动模型,左右两轮的驱动电机均安装有码盘作为物理里程设备,可通过如下公式获得到进行当前时刻的位姿(xt,yt,θt)Generally, a certain activity area is set for sports equipment such as a mobile robot. Different motion models can be established according to different motion equipment, common ones are differential drive motion model and omnidirectional motion motion model. In a specific embodiment, there are two adjacent moments, that is, the previous moment and the current moment. Generally, according to different motion models and the known pose (x t-1 , y t-1 , θ t-1 ), the pose (x t , y t , θ t ) of the motion device at the current moment can be obtained, and the relative motion amount (Δx t , Δy t , Δθ t ) can be calculated according to the pose at two moments . For example, for the motion model of two-wheel differential, the drive motors of the left and right wheels are equipped with code discs as physical mileage devices, and the pose (x t , y t , θ t ) at the current moment can be obtained by the following formula
ΔDt=(ΔdL+ΔdR)/2ΔD t = (Δd L +Δd R )/2
Δθt=(ΔdL-ΔdR)/ωΔθ t = (Δd L -Δd R )/ω
θt=θt-1+Δθt θ t = θ t-1 + Δθ t
其中,左、右两轮之间的间距为ω,ΔDt为根据码盘的信息计算获得的运动设备的距离变化量,Δθt为运动设备的角度变化量,ΔdL、ΔdR分别为通过左右码盘获得的左右两轮的运动量;Among them, the distance between the left and right wheels is ω, ΔD t is the distance change of the sports equipment calculated according to the information of the code disc, Δθ t is the angle change of the sports equipment, Δd L and Δd R are respectively passed The amount of movement of the left and right wheels obtained by the left and right code discs;
再如全向移动的运动模型的四个驱动轮的驱动电机均安装有码盘,可通过如下公式获得到进行当前时刻的位姿(xt,yt,θt)Another example is that the driving motors of the four driving wheels of the omnidirectional moving motion model are all equipped with code discs, and the pose (x t , y t , θ t ) at the current moment can be obtained by the following formula
xt=xt-1+Δxx t = x t - 1 +Δx
yt=yt-1+Δyy t =y t-1 +Δy
θt=θt-1+Δθθ t = θ t-1 + Δθ
其中,Δd1,Δd2,Δd3,Δd4分别为通过码盘获得的四个驱动轮上的运动量,r为轮毂半径,rr为辊子半径,前后轴距为2lx,左右轮距为2ly;Among them, Δd 1 , Δd 2 , Δd 3 , and Δd 4 are the motions on the four driving wheels obtained by the code disc, r is the hub radius, r r is the roller radius, the front and rear wheelbase is 2l x , and the left and right wheelbase is 2l y ;
激光扫描设备的扫描方向为运动设备移动方向的前方,激光扫描设备扫描获得的每一激光帧数据通过现有技术以激光点云的形式呈现。设立全局坐标系,运动设备的位姿、激光扫描设备的位置以及获得的激光帧数据可以统一到全局坐标系中。The scanning direction of the laser scanning device is in front of the moving direction of the moving device, and each laser frame data scanned by the laser scanning device is presented in the form of a laser point cloud through the existing technology. Set up a global coordinate system, the pose of the motion equipment, the position of the laser scanning device, and the obtained laser frame data can be unified into the global coordinate system.
局部点云地图通过用激光扫描设备扫描局部环境形成,存储于运算设备中。局部概率栅格地图,则是把局部点云地图分成一系列栅格,建立局部概率栅格地图坐标系,其中每一栅格给定一个可能值,表示该栅格被占据的概率。点云地图坐标系、栅格地图坐标系与全局坐标系可以相互转换,以确定运动设备在地图坐标系中的位姿。The local point cloud map is formed by scanning the local environment with a laser scanning device and stored in the computing device. The local probability grid map is to divide the local point cloud map into a series of grids, and establish a local probability grid map coordinate system, in which each grid is given a possible value, indicating the probability that the grid is occupied. The point cloud map coordinate system, the grid map coordinate system and the global coordinate system can be converted to each other to determine the pose of the mobile device in the map coordinate system.
坐标系的设立、坐标变换以及坐标系内的点映射到其他坐标系均可以通过现有技术实现。The establishment of the coordinate system, the coordinate transformation and the mapping of the points in the coordinate system to other coordinate systems can all be realized by existing technologies.
如图1所示的里程计方法,包括以下步骤:The odometer method as shown in Figure 1 comprises the following steps:
步骤S110:获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图。Step S110: Obtain the pose of the motion device at the current moment, the second laser frame at the current moment, and the local point cloud map.
所述当前时刻为t,所述当前时刻的位姿是通过物理里程设备或者现有技术可获得当前时刻的初始位姿 The current moment is t, and the pose at the current moment is the initial pose at the current moment that can be obtained through physical mileage equipment or existing technologies
同时在当前时刻t,通过激光扫描设备扫描得到的第二激光帧,激光扫描设备将获得的激光帧数据发送给运算设备,具体的可以通过有线或无线的方式,也可以通过总控设备将激光帧数据转发给运算设备。At the same time, at the current time t, the laser scanning device scans the second laser frame data, and the laser scanning device sends the obtained laser frame data to the computing device. Specifically, it can be wired or wireless, or the master control device can send the laser frame data to the computing device. The frame data is forwarded to the computing device.
步骤S120:根据当前时刻的位姿,以及第二激光帧与局部点云地图,得到当前时刻的最优位姿。Step S120: Obtain the optimal pose at the current moment according to the pose at the current moment, as well as the second laser frame and the local point cloud map.
具体地,所述局部点云地图存储在运算设备中,运算设备以当前时刻的位姿为初值,对第二激光帧与局部点云地图进行点云匹配得到最优位姿,即根据以下公式获得的最优化解即为最优位姿所述公式为:Specifically, the local point cloud map is stored in the computing device, and the computing device uses the pose at the current moment as an initial value to perform point cloud matching on the second laser frame and the local point cloud map to obtain the optimal pose, that is, according to the following The optimal solution obtained by the formula is the optimal pose The formula is:
其中:为局部点云地图构成的曲面;pi为第二激光帧内的激光点;符号代表旋转-平移变换;符号代表点到局部点云地图构成的曲面的欧几里得投影;P为计算过程中的位姿,以当前时刻的位姿为初值,其最终的值即为最优位姿 in: is the surface formed by the local point cloud map; p i is the laser point in the second laser frame; the symbol Represents a rotation-translation transformation; the symbol Surfaces representing points to local point cloud maps The Euclidean projection of ; P is the pose during the calculation process, with the pose at the current moment as the initial value, and its final value is the optimal pose
所述第二激光帧与局部点云地图进行点云匹配可以通过现有技术实现,例如迭代近邻算法、暴力匹配等。所述最优位姿是由运算设备通过上述技术方案计算获得的当前时刻的位姿,比步骤S110直接通过现有技术或者物理里程设备获得的位姿更准确的。The point cloud matching of the second laser frame and the local point cloud map can be realized by existing technologies, such as iterative nearest neighbor algorithm, brute force matching, and the like. The optimal pose is the pose at the current moment calculated and obtained by the computing device through the above technical solution, which is more accurate than the pose directly obtained by the prior art or the physical mileage device in step S110.
在本实施例中,作为进一步改进,所述当前时刻的位姿还可以是通过以下技术方案获得的较优位姿用于代替通过物理里程设备或者现有技术可获得当前时刻的初始位姿图2是本发明里程计方法实施例1获得较优当前位姿的流程示意图;具体的,所述较优位姿的获取包括以下步骤:In this embodiment, as a further improvement, the pose at the current moment may also be a better pose obtained through the following technical solutions It is used to replace the initial pose obtained at the current moment through physical mileage equipment or existing technologies Fig. 2 is a schematic flow chart of obtaining a better current pose in Embodiment 1 of the odometer method of the present invention; specifically, the acquisition of the better pose includes the following steps:
步骤S111:获取运动设备第一时刻到当前时刻的初始相对运动量。Step S111: Obtain the initial relative exercise amount of the exercise device from the first moment to the current moment.
所述第一时刻为当前时刻之前的某一时间点,一般为当前时刻的前一时刻t-1。所述初始相对运动量用于描述运动设备从第一时刻t-1所处的位姿Pt-1=(xt-1,yt-1,θt-1)移动到当前时刻t所处的位姿的变化,包括运动设备的位置变化量和角度变化量,用表示。The first moment is a certain point in time before the current moment, generally a moment t-1 before the current moment. The initial relative movement amount is used to describe that the moving device moves from the pose P t-1 =(x t-1 , y t-1 , θ t-1 ) at the first moment t-1 to the current moment t The change of the pose, including the position change and angle change of the sports equipment, is used express.
其中,第一时刻t-1所处的位姿Pt-1=(xt-1,yt-1,θt-1)已知,优选地,所述第一时刻t-1所处的位姿Pt-1=(xt-1,yt-1,θt-1)为上一次里程计方法获得的最优位姿。在一具体实施中,可以根据第一时刻t-1所处的位姿Pt-1=(xt-1,yt-1,θt-1)与当前时刻t所处的初始位姿计算转化为初始相对运动量计算方法为现有技术,且在前面已经描述。Wherein, the pose P t-1 at the first moment t-1 = (x t-1 , y t-1 , θ t-1 ) is known, preferably, the position at the first moment t-1 The pose P t-1 = (x t-1 , y t-1 , θ t-1 ) is the optimal pose obtained by the last odometry method. In a specific implementation, according to the pose P t-1 =(x t-1 , y t-1 , θ t-1 ) at the first moment t-1 and the initial pose at the current moment t The calculation is converted into the initial relative motion The calculation method is prior art and has been described above.
通过物理里程设备或者其他现有技术获得的数据发送给运算设备,通过有线或无线的方式,也可以通过总控设备转发给运算设备进行计算。The data obtained through the physical mileage device or other existing technologies are sent to the computing device, and can also be forwarded to the computing device through the master control device for calculation through wired or wireless methods.
步骤S112:获取第一时刻的第一激光帧以及当前时刻的第二激光帧。Step S112: Obtain the first laser frame at the first moment and the second laser frame at the current moment.
在第一时刻t-1,激光扫描设备扫描得到的激光帧为第一激光帧,在当前时刻t,激光扫描设备扫描得到的激光帧为第二激光帧。At the first time t−1, the laser frame scanned by the laser scanning device is the first laser frame, and at the current time t, the laser frame scanned by the laser scanning device is the second laser frame.
步骤S113:根据初始相对运动量,以及第一激光帧与第二激光帧,得到帧间相对运动量。Step S113: According to the initial relative motion, and the first laser frame and the second laser frame, obtain the inter-frame relative motion.
具体地,以步骤S111中获得的初始相对运动量为初值,对第一激光帧与第二激光帧进行帧间匹配得到帧间相对运动量,即根据以下公式获得的最优化解即为帧间运动量,所述公式为:Specifically, with the initial relative motion obtained in step S111 as the initial value, inter-frame matching is performed on the first laser frame and the second laser frame to obtain the inter-frame relative motion, that is, the optimal solution obtained according to the following formula is the inter-frame motion , the formula is:
其中:为第一激光帧构成的曲面;pi为第二激光帧的激光点;q为计算过程中运动设备的相对运动量,以相对运动量为初值,其最终的值即为帧间相对运动量;符号代表旋转-平移变换;符号代表点到第一激光帧构成的曲面的欧几里得投影。in: is the curved surface formed by the first laser frame; p i is the laser point of the second laser frame; q is the relative motion of the moving device during the calculation process, with the relative motion as the initial value, and the final value is the relative motion between frames; symbol Represents a rotation-translation transformation; the symbol represents the surface formed from the point to the first laser frame Euclidean projection.
所述第一激光帧与第二激光帧的帧间匹配可以通过现有技术实现,例如迭代近邻算法、暴力匹配等。The frame-to-frame matching between the first laser frame and the second laser frame can be realized by existing technologies, such as iterative nearest neighbor algorithm, brute force matching, and the like.
所述帧间相对运动量是由运算设备在初始相对运动量的基础上计算获得的从第一时刻t-1所处的位姿移动到当前时刻t所处的位姿的变化量,相对于步骤S111中获得的初始相对运动量更准确。The inter-frame relative motion amount is the change amount calculated by the computing device on the basis of the initial relative motion amount from the pose at the first moment t-1 to the pose at the current moment t, relative to step S111 The initial relative motion obtained in is more accurate.
步骤S114:根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿。Step S114: Calculate and obtain a better pose at the current moment based on the pose of the moving device at the first moment and the relative motion between frames.
第一时刻运动设备的位姿已知,根据第一时刻t-1运动设备的位姿Pt-1=(xt-1,yt-1,θt-1),以及帧间相对运动量可以计算转化为当前时刻的较优位姿所述较优位姿相对于直接通过物理里程设备或其他现有技术获得的当前时刻的位姿更加准确。The pose of the moving device at the first moment is known, according to the pose of the moving device at the first moment t-1 P t-1 = (x t-1 , y t-1 , θ t-1 ), and the relative motion between frames Can calculate the better pose converted to the current moment The optimal pose Compared with the current pose obtained directly through physical mileage equipment or other existing technologies, it is more accurate.
进一步地,本发明还包括步骤S131:根据第一时刻运动设备的位姿以及最优位姿计算转化为最优相对运动量。Further, the present invention also includes step S131: calculating and converting into an optimal relative exercise amount according to the pose and the optimal pose of the sports equipment at the first moment.
第一时刻运动设备的位姿已知,根据第一时刻t-1运动设备的位姿Pt-1=(xt-1,yt-1,θt-1),以及最优位姿可以计算获得最优相对运动量 The pose of the sports equipment at the first moment is known, according to the pose of the sports equipment at the first moment t-1 P t-1 = (x t-1 , y t-1 , θ t-1 ), and the optimal pose The optimal relative exercise can be calculated
本发明中,步骤S111中,可以根据第一时刻t-1的位姿Pt-1=(xt-1,yt-1,θt-1)与当前时刻t的位姿的计算转化为初始相对运动量,计算公式如下:In the present invention, in step S111, according to the pose P t-1 = (x t-1 , y t-1 , θ t-1 ) at the first moment t-1 and the pose at the current moment t The calculation of is transformed into the initial relative movement amount, and the calculation formula is as follows:
xt=xt-1+cos(θt-1)Δxt-sin(θt-1)Δyt x t =x t-1 +cos(θ t-1 )Δx t -sin(θ t-1 )Δy t
yt=yt-1+sin(θt-1)Δxt+cos(θt-1)Δyt y t =y t-1 +sin(θ t-1 )Δx t +cos(θ t-1 )Δy t
θt=θt-1+Δθt θ t = θ t-1 + Δθ t
其中,用当前时刻的较优位姿代替当前时刻t的初始位姿可实现步骤S114中较优位姿与帧间相对运动量的计算转化;用最优位姿代替当前时刻t的位姿可实现步骤S131中最优位姿与最优相对运动量计算转化。同时,通过本发明还可以上述公式根据运动设备第一时刻的位姿以及最优位姿计算获得最优相对运动量。Among them, using the better pose at the current moment Replace the initial pose at the current moment t The calculation conversion of the optimal pose and the relative motion between frames can be realized in step S114; with the optimal pose Replace the pose at the current moment t The conversion between the optimal pose and the optimal relative motion in step S131 can be realized. At the same time, through the present invention, the above formula can also be used to calculate and obtain the optimal relative exercise amount according to the posture and the optimal posture of the sports equipment at the first moment.
本发明提供的里程计方法,通过码盘转数获得初始相对运动量及位姿,通过迭代近邻算法等进行激光帧间匹配,以此获得较准确的相对运动量和位姿。以上一步获得的位姿为初值,对激光帧与局部点云地图进行匹配,获得更准确的位姿及运动量,并对外输出。本方法无需先验全局地图,能有效抑制里程计的累积误差,对动态环境有较好的适应性;大范围闭环(200米)误差在1米之内。The odometer method provided by the present invention obtains the initial relative movement amount and pose through the rotation number of the code wheel, and performs laser frame-to-frame matching through an iterative neighbor algorithm to obtain more accurate relative movement amount and pose. The pose obtained in the previous step is the initial value, and the laser frame is matched with the local point cloud map to obtain a more accurate pose and motion, and output it externally. This method does not require a priori global map, can effectively suppress the cumulative error of the odometer, and has good adaptability to dynamic environments; the error of a large-scale closed loop (200 meters) is within 1 meter.
实施例二Embodiment two
图3是本发明里程计方法实施例2的流程示意图。Fig. 3 is a schematic flowchart of Embodiment 2 of the odometer method of the present invention.
本实施例的里程计方法包括The odometer method of the present embodiment includes
步骤S210:获取运动设备当前时刻的位姿、当前时刻的剔除激光动态点后的第二激光帧以及局部点云地图。Step S210: Obtain the current pose of the sports device, the current second laser frame after excluding laser dynamic points, and the local point cloud map.
引入局部概率栅格地图,将概率栅格地图的概率值作为删除动态点的依据。将第二激光帧映射到局部概率栅格地图,若激光点所在栅格的概率值小于静态栅格的最低概率值则为动态栅格,即该栅格内的激光点均为动态激光点,具体地,包括以下步骤:A local probability grid map is introduced, and the probability value of the probability grid map is used as the basis for deleting dynamic points. Map the second laser frame to the local probability grid map. If the probability value of the grid where the laser point is located is less than the lowest probability value of the static grid, it is a dynamic grid, that is, the laser points in the grid are all dynamic laser points. Specifically, the following steps are included:
步骤S211:根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标;Step S211: Obtain the position coordinates of the laser point in the local probability grid map in the second laser frame according to the pose at the current moment;
步骤S212:根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标;Step S212: Determine the grid coordinates of the laser point in the second laser frame on the local probability grid map according to the position coordinates;
步骤S213:根据栅格坐标所在栅格的概率值确定该激光点是否为动态激光点,如果是则剔除动态激光点。Step S213: Determine whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located, and if so, reject the dynamic laser point.
原理如下:The principle is as follows:
第二激光帧内激光点为激光点i,通过旋转-平移变换将第二激光帧映射到局部概率栅格地图得到激光点i的位置坐标Li,即 The laser point in the second laser frame is laser point i, and the second laser frame is mapped to the local probability grid map by rotation-translation transformation to obtain the position coordinate L i of laser point i, namely
局部概率栅格地图的原点坐标O,栅格大小reso,激光点i所在栅格的栅格坐标(m,n),则 The origin coordinate O of the local probability grid map, the grid size reso, and the grid coordinates (m, n) of the grid where the laser point i is located, then
则当0≤Bel(m,n)≤Belthres时,判定该栅格内激光点i为动态激光点。其中,Bel(m,n)为栅格的概率值,Belthres为静态栅格的最低概率值,Bel(m,n)、Belthres的值、动态激光点的剔除均通过现有技术实现,本发明不再赘述。Then when 0≤Bel(m,n)≤Bel thres , it is determined that the laser point i in the grid is a dynamic laser point. Among them, Bel(m, n) is the probability value of the grid, Bel thres is the lowest probability value of the static grid, Bel(m, n), the value of Bel thres , and the elimination of dynamic laser points are all realized through the existing technology, The present invention will not be described in detail.
步骤S220:根据当前时刻的位姿,以及剔除动态激光点后的第二激光帧与局部点云地图,得到当前时刻的最优位姿。Step S220: Obtain the optimal pose at the current moment according to the pose at the current moment, the second laser frame and the local point cloud map after removing the dynamic laser points.
具体地,以当前时刻的位姿为初值,对剔除动态点后的第二激光帧与局部点云地图进行点云匹配得到最优位姿,即根据以下公式获得的最优化解即为最优位姿所述公式为:Specifically, with the current pose as the initial value, point cloud matching is performed on the second laser frame after excluding dynamic points and the local point cloud map to obtain the optimal pose, that is, the optimal solution obtained according to the following formula is the optimal Excellent pose The formula is:
其中:为局部点云地图构成的曲面;为剔除动态激光点后的第二激光帧内的激光点;P为计算过程中的位姿,以当前时刻的位姿为初值,其最终的值即为最优位姿;符号代表旋转-平移变换;符号代表点到局部点云地图构成的曲面的欧几里得投影。in: A surface composed of a local point cloud map; is the laser point in the second laser frame after excluding the dynamic laser point; P is the pose during the calculation process, the pose at the current moment is taken as the initial value, and the final value is the optimal pose; the symbol Represents a rotation-translation transformation; the symbol Surfaces representing points to local point cloud maps Euclidean projection.
本实施例所述的里程计方法与实施例一中不同的是步骤S210与步骤S220中的第二激光帧为剔除动态激光点后的激光帧,步骤S210、步骤S220与步骤S211中的当前时刻的位姿也可以用通过步骤S111-步骤S114的方法获得的当前时刻的较优位姿。The difference between the odometer method described in this embodiment and the first embodiment is that the second laser frame in step S210 and step S220 is the laser frame after removing the dynamic laser point, and the current moment in step S210, step S220 and step S211 The pose of can also use the better pose at the current moment obtained through the method of step S111-step S114.
在另一实施例中,本发明的里程计方法还包括步骤S240:根据步骤S220获得的最优位姿与第二激光帧更新地图,包括更新局部概率栅格地图和更新局部点云地图。此步骤获得的局部概率栅格地图和局部点云地图应用于下一次里程计方法实现的过程,以获得更精确的里程数据,具体包括以下子步骤:In another embodiment, the odometry method of the present invention further includes step S240: updating the map according to the optimal pose obtained in step S220 and the second laser frame, including updating the local probability grid map and updating the local point cloud map. The local probability grid map and local point cloud map obtained in this step are applied to the next implementation of the odometry method to obtain more accurate odometry data, which specifically includes the following sub-steps:
步骤S241:将第二激光帧内的激光点映射到全局坐标系中得到激光点在全局坐标系的位置。其中,激光点i可以为剔除动态激光点后的第二激光帧内的激光点,激光点i在全局坐标系的位置通过旋转-平移变换将第二激光帧映射到全局坐标系中,即 Step S241: Map the laser point in the second laser frame to the global coordinate system to obtain the position of the laser point in the global coordinate system. Among them, the laser point i can be the laser point in the second laser frame after excluding the dynamic laser point, and the position of the laser point i in the global coordinate system Map the second laser frame into the global coordinate system by rotation-translation transformation, namely
步骤S242:将所述位置插入到局部点云地图,完成局部点云地图的更新;此步骤可以通过现有技术实现,这里不再赘述;Step S242: Insert the position into the local point cloud map to complete the update of the local point cloud map; this step can be realized by existing technologies, and will not be repeated here;
步骤S243:以光线追踪方式,更新局部概率栅格地图;包括更新位置点附近的栅格的概率值以及激光扫描设备和位置点连线上的概率值。具体地,一般以方差较小的高斯分布,更新位置点附近栅格的概率值;将激光扫描设备和位置点连线上的栅格概率置零,零表示该栅格被占据的概率为0。Step S243: Updating the local probability grid map by means of ray tracing; including updating the probability value of the grid near the location point and the probability value on the line between the laser scanning device and the location point. Specifically, the location point is generally updated with a Gaussian distribution with a small variance Probability values for nearby rasters; laser scan device and location points The grid probability on the connection line is set to zero, and zero means that the probability of the grid being occupied is 0.
在另一实施例中,里程计不断累积里程,作为本发明的又一种改进,还包括步骤S250根据运动设备的移动距离重置概率栅格地图。具体的,包括子步骤:In another embodiment, the odometer continuously accumulates mileage. As another improvement of the present invention, step S250 is further included to reset the probability grid map according to the moving distance of the sports equipment. Specifically, including sub-steps:
步骤S251:计算运动设备的移动距离,并判断是否到达设定距离;若到达设定距离则进行步骤S252、步骤S253、步骤S254进行重置地图,所述重置栅格地图包括重置栅格地图的概率值,重置地图原点、更新局部概率栅格地图。所述运动设备的移动距离通过本发明的里程计方法实现的里程计获得。Step S251: Calculate the moving distance of the sports equipment, and judge whether it has reached the set distance; if the set distance is reached, proceed to step S252, step S253, and step S254 to reset the map, and the reset grid map includes the reset grid The probability value of the map, reset the map origin, and update the local probability grid map. The moving distance of the sports equipment is obtained by the odometer realized by the odometer method of the present invention.
步骤S252:将所有栅格概率值均置为表示未知的概率值,一般-1表示该栅格的概率值未知,即该栅格不知道是否被占据;Step S252: set the probability values of all the grids to represent unknown probability values, generally -1 indicates that the probability value of the grid is unknown, that is, it is not known whether the grid is occupied;
步骤S253:根据最优位姿在地图中的位置确定地图原点;具体的,以最优位姿在地图中的位置或者平移一定距离的位置为地图原点;Step S253: Determine the map origin according to the position of the optimal pose in the map; specifically, take the position of the optimal pose in the map or a position shifted by a certain distance as the map origin;
步骤S254:用当前时刻最近的N帧激光帧以光线追踪方式更新局部概率栅格地图。更新方法与步骤S243一致,以此在下一次里程计方法中可以获得更准确的运动设备的位姿。Step S254: Update the local probability grid map by ray tracing with the latest N frames of laser frames at the current moment. The update method is consistent with step S243, so that a more accurate pose of the sports equipment can be obtained in the next odometer method.
实施例三Embodiment Three
图4是本发明里程计装置的模块构成示意图。Fig. 4 is a schematic diagram of the module structure of the odometer device of the present invention.
一种里程计装置,包括An odometer device comprising
第一获取模块,获取运动设备当前时刻的位姿;The first acquisition module acquires the pose of the sports equipment at the current moment;
第二获取模块,获取运动设备当前时刻的第二激光帧;The second acquisition module acquires the second laser frame at the current moment of the sports equipment;
第三获取模块,获取局部点云地图;The third acquisition module acquires the local point cloud map;
第一运算模块,根据当前时刻的位姿,以及第二激光帧与局部点云地图,计算得到当前时刻的最优位姿。The first calculation module calculates the optimal pose at the current moment according to the pose at the current moment, as well as the second laser frame and the local point cloud map.
在本实施例中,作为进一步改进,所述第一获取模块具体用于获取当前时刻的较优位姿位姿,具体地,所述第一获取模块包括In this embodiment, as a further improvement, the first acquisition module is specifically configured to acquire a better pose at the current moment, specifically, the first acquisition module includes
第一获取单元,获取运动设备第一时刻到当前时刻的初始相对运动量。The first acquisition unit acquires the initial relative exercise amount of the sports equipment from the first moment to the current moment.
第二获取单元,获取第一时刻的第一激光帧以及当前时刻的第二激光帧。The second acquiring unit acquires the first laser frame at the first moment and the second laser frame at the current moment.
第一计算单元,根据初始相对运动量,以及第一激光帧与第二激光帧,得到帧间相对运动量。The first calculation unit obtains the inter-frame relative motion according to the initial relative motion, and the first laser frame and the second laser frame.
第二计算单元,根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿。The second calculation unit calculates and obtains the optimal pose at the current moment according to the pose of the moving device at the first moment and the relative motion between frames.
进一步地,本发明的里程计装置还包括剔除模块,用于剔除第二激光帧内的动态激光点,所述第二获取模块获取剔除动态激光点后的第二激光帧。具体的,所述剔除模块包括:Further, the odometer device of the present invention further includes a removing module, configured to remove dynamic laser points in the second laser frame, and the second acquisition module obtains the second laser frame after removing the dynamic laser points. Specifically, the elimination module includes:
第一坐标单元,根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标;The first coordinate unit obtains the position coordinates of the laser point in the local probability grid map in the second laser frame according to the pose at the current moment;
第二坐标单元,根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标;The second coordinate unit determines the grid coordinates of the laser point in the second laser frame on the local probability grid map according to the position coordinates;
判断单元,根据栅格坐标所在栅格的概率值判断该激光点是否为动态激光点,The judging unit judges whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located,
剔除单元,判断单元的判断结果剔除动态激光点,即判断结果为动态激光点则剔除。The rejecting unit, the judgment result of the judging unit rejects the dynamic laser spot, that is, the judgment result is a dynamic laser spot, and then the dynamic laser spot is rejected.
进一步地,本发明的里程计装置还包括Further, the odometer device of the present invention also includes
用于根据最优位姿与第二激光帧更新局部点云地图以及局部概率栅格地图。具体地,所述地图更新模块包括:It is used to update the local point cloud map and the local probability grid map according to the optimal pose and the second laser frame. Specifically, the map update module includes:
映射单元,将第二激光帧内的激光点映射到全局坐标系中,得到激光点在全局坐标系的位置。通过旋转-平移变换,即通过公式将第二激光帧内的激光点映射到全局坐标系中。The mapping unit maps the laser point in the second laser frame to the global coordinate system to obtain the position of the laser point in the global coordinate system. Through the rotation-translation transformation, that is, the formula will The laser points within the second laser frame are mapped into the global coordinate system.
第一更新单元,将位置插入到局部点云地图,以此完成局部点云地图的更新。The first update unit inserts the position into the local point cloud map, so as to complete the update of the local point cloud map.
第二更新单元,以光线追踪方式,更新局部概率栅格地图。具体地,一般以方差较小的高斯分布,更新位置点附近栅格的概率值;将激光扫描设备和位置点连线上的栅格概率置零,零表示该栅格点未被占据的概率值。The second update unit updates the local probability grid map in a ray tracing manner. Specifically, the location point is generally updated with a Gaussian distribution with a small variance Probability values for nearby rasters; laser scan device and location points The grid probability on the connecting line is set to zero, and zero represents the probability value that the grid point is not occupied.
进一步的,本发明的里程计装置还包括地图重置模块,用于根据运动设备的移动距离重置局部概率栅格地图。所述地图重置模块包括Further, the odometer device of the present invention further includes a map reset module, configured to reset the local probability grid map according to the moving distance of the sports equipment. The map reset module includes
距离计算单元,计算运动设备的移动距离;所述计算方法为现有技术,或者通过本方法的里程计根据最优位姿计算获得。The distance calculation unit is used to calculate the moving distance of the sports equipment; the calculation method is the prior art, or is obtained through the odometer calculation of the method according to the optimal pose.
第二判断单元,判断所述移动距离是否到达设定距离;a second judging unit, judging whether the moving distance reaches a set distance;
若到达设定距离,所述地图重置模块通过概率值重置单元、原点重置单元、地图重置单元重置局部概率栅格地图。即概率值重置单元,根据判断结果将所有栅格概率值均置为表示未知的概率值;原点重置单元,根据判断结果将最优位姿在地图中的位置重置为地图原点;地图重置单元,根据判断结果,以光线追踪方式,利用当前时刻最近的N帧激光帧更新局部概率栅格地图。If the set distance is reached, the map reset module resets the local probability grid map through the probability value reset unit, the origin reset unit, and the map reset unit. That is, the probability value reset unit, which sets all the grid probability values as unknown probability values according to the judgment result; the origin reset unit, which resets the position of the optimal pose in the map to the map origin according to the judgment result; The reset unit updates the local probability grid map by using the latest N frames of laser frames at the current moment in the way of ray tracing according to the judgment result.
本实施例中的装置与前述实施例中的方法是基于同一发明构思下的两个方面,在前面已经对方法实施过程作了详细的描述,所以本领域技术人员可根据前述描述清楚地了解本实施中的系统的结构及实施过程,为了说明书的简洁,在此就不再赘述。The device in this embodiment and the method in the previous embodiment are based on two aspects under the same inventive concept. The implementation process of the method has been described in detail above, so those skilled in the art can clearly understand the present invention based on the foregoing description. The structure and implementation process of the system being implemented will not be repeated here for the sake of brevity.
为了描述的方便,描述以上装置时以功能分为各种模块分别描述。当然,在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。For the convenience of description, when describing the above devices, functions are divided into various modules and described separately. Of course, when implementing the present invention, the functions of each module can be implemented in one or more pieces of software and/or hardware.
通过以上的实施方式的描述可知,本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品可以存储在存储介质中,如ROM/RAM、磁碟、光盘等,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。It can be known from the above description of the implementation manners that those skilled in the art can clearly understand that the present invention can be implemented by means of software plus a necessary general hardware platform. Based on this understanding, the essence of the technical solution of the present invention or the part that contributes to the prior art can be embodied in the form of software products, and the computer software products can be stored in storage media, such as ROM/RAM, disk , CD, etc., including several instructions to make a computer device (which may be a personal computer, server, or network device, etc.) execute the methods described in various embodiments or some parts of the embodiments of the present invention.
描述的装置实施例仅仅是示意性的,其中所述作为分离部件说明的模块或单元可以是或者也可以不是物理上分开的,作为模块或单元示意的部件可以是或者也可以不是物理模块,既可以位于一个地方,或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下,即可以理解并实施。The described device embodiments are only illustrative, wherein the modules or units described as separate components may or may not be physically separated, and the components illustrated as modules or units may or may not be physical modules, either It can be located in one place, or it can be distributed over multiple network modules. Part or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment. It can be understood and implemented by those skilled in the art without creative effort.
本发明可用于众多通用或专用的计算系统环境或配置中。例如:个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等,如实施例四。The invention is applicable to numerous general purpose and special purpose computing system environments or configurations. Examples: personal computers, server computers, handheld or portable devices, tablet-type devices, multiprocessor systems, microprocessor-based systems, set-top boxes, programmable consumer electronics, network PCs, minicomputers, mainframe computers, including the above Distributed computing environment of any system or device, etc., such as Embodiment 4.
实施例四Embodiment four
如图所示的里程计装置,包括:处理器以及用于存储处理器可执行的指令的存储器;The odometer device as shown in the figure includes: a processor and a memory for storing instructions executable by the processor;
所述处理器被配置为:The processor is configured to:
获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图,Obtain the pose of the motion equipment at the current moment, the second laser frame at the current moment, and the local point cloud map,
根据当前时刻的位姿,以及第二激光帧与局部点云地图,得到当前时刻的最优位姿。According to the pose at the current moment, as well as the second laser frame and the local point cloud map, the optimal pose at the current moment is obtained.
本发明实施例提供的里程计,解决了现有的现有里程计不精确以及累计误差的不足。对于本领域的技术人员来说,可根据以上描述的技术方案以及构思,做出其它各种相应的改变以及变形,而所有的这些改变以及变形都应该属于本发明权利要求的保护范围之内。The odometer provided by the embodiment of the present invention solves the inaccurate and accumulated errors of existing odometers. For those skilled in the art, various other corresponding changes and modifications can be made according to the technical solutions and ideas described above, and all these changes and modifications should fall within the protection scope of the claims of the present invention.
Claims (17)
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710426765.5A CN107144292B (en) | 2017-06-08 | 2017-06-08 | Odometer method and odometer device for sports equipment |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710426765.5A CN107144292B (en) | 2017-06-08 | 2017-06-08 | Odometer method and odometer device for sports equipment |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107144292A true CN107144292A (en) | 2017-09-08 |
CN107144292B CN107144292B (en) | 2019-10-25 |
Family
ID=59779639
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710426765.5A Active CN107144292B (en) | 2017-06-08 | 2017-06-08 | Odometer method and odometer device for sports equipment |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107144292B (en) |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108253958A (en) * | 2018-01-18 | 2018-07-06 | 亿嘉和科技股份有限公司 | A kind of robot real-time location method under sparse environment |
CN108844553A (en) * | 2018-06-27 | 2018-11-20 | 广州视源电子科技股份有限公司 | Method and device for correcting mileage in robot moving process and robot |
CN109631919A (en) * | 2018-12-28 | 2019-04-16 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of hybrid navigation map constructing method for merging reflector and occupying grid |
CN110824496A (en) * | 2019-09-18 | 2020-02-21 | 北京迈格威科技有限公司 | Motion estimation method, motion estimation device, computer equipment and storage medium |
CN110887493A (en) * | 2019-11-29 | 2020-03-17 | 上海有个机器人有限公司 | Trajectory estimation method, medium, terminal and device based on local map matching |
CN110969649A (en) * | 2019-11-29 | 2020-04-07 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device of laser point cloud and map |
CN111609853A (en) * | 2019-02-25 | 2020-09-01 | 北京奇虎科技有限公司 | Three-dimensional map construction method, cleaning robot and electronic equipment |
CN111637877A (en) * | 2020-05-29 | 2020-09-08 | 拉扎斯网络科技(上海)有限公司 | Robot positioning method and device, electronic equipment and nonvolatile storage medium |
CN111696159A (en) * | 2020-06-15 | 2020-09-22 | 湖北亿咖通科技有限公司 | Feature storage method of laser odometer, electronic device and storage medium |
CN111735451A (en) * | 2020-04-16 | 2020-10-02 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
CN112113574A (en) * | 2020-03-02 | 2020-12-22 | 北京百度网讯科技有限公司 | Method, apparatus, computing device and computer-readable storage medium for positioning |
CN112344940A (en) * | 2020-11-06 | 2021-02-09 | 杭州国辰机器人科技有限公司 | Positioning method and device integrating reflective columns and grid map |
CN112700495A (en) * | 2020-11-25 | 2021-04-23 | 北京旷视机器人技术有限公司 | Pose determination method and device, robot, electronic device and storage medium |
CN113012191A (en) * | 2021-03-11 | 2021-06-22 | 中国科学技术大学 | Laser mileage calculation method based on point cloud multi-view projection graph |
CN115235477A (en) * | 2021-11-30 | 2022-10-25 | 上海仙途智能科技有限公司 | Vehicle positioning inspection method and device, storage medium and equipment |
CN115371678A (en) * | 2022-08-22 | 2022-11-22 | 广州足下科技有限公司 | A mobile robot autonomous navigation method |
CN116091593A (en) * | 2021-11-05 | 2023-05-09 | 追觅创新科技(苏州)有限公司 | Method and device for determining robot state, storage medium, electronic device |
CN117170354A (en) * | 2022-10-25 | 2023-12-05 | 中科南京软件技术研究院 | Wheel type robot positioning method and system |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106767827A (en) * | 2016-12-29 | 2017-05-31 | 浙江大学 | A kind of mobile robot point cloud map creating method based on laser data |
-
2017
- 2017-06-08 CN CN201710426765.5A patent/CN107144292B/en active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106767827A (en) * | 2016-12-29 | 2017-05-31 | 浙江大学 | A kind of mobile robot point cloud map creating method based on laser data |
Non-Patent Citations (1)
Title |
---|
赵梓乔等: "基于三维激光扫描仪的室内移动设备定位与建图", 《计算机与数字工程》 * |
Cited By (27)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108253958A (en) * | 2018-01-18 | 2018-07-06 | 亿嘉和科技股份有限公司 | A kind of robot real-time location method under sparse environment |
CN108844553A (en) * | 2018-06-27 | 2018-11-20 | 广州视源电子科技股份有限公司 | Method and device for correcting mileage in robot moving process and robot |
CN108844553B (en) * | 2018-06-27 | 2021-06-15 | 广州视源电子科技股份有限公司 | Method and device for correcting mileage in robot moving process and robot |
CN109631919A (en) * | 2018-12-28 | 2019-04-16 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of hybrid navigation map constructing method for merging reflector and occupying grid |
CN109631919B (en) * | 2018-12-28 | 2022-09-30 | 芜湖哈特机器人产业技术研究院有限公司 | Hybrid navigation map construction method integrating reflector and occupied grid |
CN111609853A (en) * | 2019-02-25 | 2020-09-01 | 北京奇虎科技有限公司 | Three-dimensional map construction method, cleaning robot and electronic equipment |
CN110824496A (en) * | 2019-09-18 | 2020-02-21 | 北京迈格威科技有限公司 | Motion estimation method, motion estimation device, computer equipment and storage medium |
CN110969649A (en) * | 2019-11-29 | 2020-04-07 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device of laser point cloud and map |
CN110969649B (en) * | 2019-11-29 | 2024-04-05 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device for laser point cloud and map |
CN110887493A (en) * | 2019-11-29 | 2020-03-17 | 上海有个机器人有限公司 | Trajectory estimation method, medium, terminal and device based on local map matching |
CN110887493B (en) * | 2019-11-29 | 2023-05-05 | 上海有个机器人有限公司 | Track calculation method, medium, terminal and device based on local map matching |
CN112113574A (en) * | 2020-03-02 | 2020-12-22 | 北京百度网讯科技有限公司 | Method, apparatus, computing device and computer-readable storage medium for positioning |
US11725944B2 (en) | 2020-03-02 | 2023-08-15 | Apollo Intelligent Driving Technology (Beijing) Co, Ltd. | Method, apparatus, computing device and computer-readable storage medium for positioning |
CN112113574B (en) * | 2020-03-02 | 2022-10-11 | 北京百度网讯科技有限公司 | Method, apparatus, computing device and computer-readable storage medium for positioning |
CN111735451A (en) * | 2020-04-16 | 2020-10-02 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
CN111735451B (en) * | 2020-04-16 | 2022-06-07 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
CN111637877A (en) * | 2020-05-29 | 2020-09-08 | 拉扎斯网络科技(上海)有限公司 | Robot positioning method and device, electronic equipment and nonvolatile storage medium |
CN111696159A (en) * | 2020-06-15 | 2020-09-22 | 湖北亿咖通科技有限公司 | Feature storage method of laser odometer, electronic device and storage medium |
CN112344940A (en) * | 2020-11-06 | 2021-02-09 | 杭州国辰机器人科技有限公司 | Positioning method and device integrating reflective columns and grid map |
CN112344940B (en) * | 2020-11-06 | 2022-05-17 | 杭州国辰机器人科技有限公司 | Positioning method and device integrating reflective columns and grid map |
CN112700495A (en) * | 2020-11-25 | 2021-04-23 | 北京旷视机器人技术有限公司 | Pose determination method and device, robot, electronic device and storage medium |
CN113012191B (en) * | 2021-03-11 | 2022-09-02 | 中国科学技术大学 | Laser mileage calculation method based on point cloud multi-view projection graph |
CN113012191A (en) * | 2021-03-11 | 2021-06-22 | 中国科学技术大学 | Laser mileage calculation method based on point cloud multi-view projection graph |
CN116091593A (en) * | 2021-11-05 | 2023-05-09 | 追觅创新科技(苏州)有限公司 | Method and device for determining robot state, storage medium, electronic device |
CN115235477A (en) * | 2021-11-30 | 2022-10-25 | 上海仙途智能科技有限公司 | Vehicle positioning inspection method and device, storage medium and equipment |
CN115371678A (en) * | 2022-08-22 | 2022-11-22 | 广州足下科技有限公司 | A mobile robot autonomous navigation method |
CN117170354A (en) * | 2022-10-25 | 2023-12-05 | 中科南京软件技术研究院 | Wheel type robot positioning method and system |
Also Published As
Publication number | Publication date |
---|---|
CN107144292B (en) | 2019-10-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107144292B (en) | Odometer method and odometer device for sports equipment | |
CN113436260B (en) | Mobile robot pose estimation method and system based on multi-sensor tight coupling | |
CN109084732B (en) | Positioning and navigation method, device and processing equipment | |
US11086016B2 (en) | Method and apparatus for tracking obstacle | |
Pan et al. | Voxfield: Non-projective signed distance fields for online planning and 3d reconstruction | |
CN111781608B (en) | Moving target detection method and system based on FMCW laser radar | |
CN115479598A (en) | Positioning and mapping method based on multi-sensor fusion and tight coupling system | |
CN111046776A (en) | Mobile robot traveling path obstacle detection method based on depth camera | |
CN104657981B (en) | Dynamic compensation method for three-dimensional laser distance metering data of mobile robot in moving process | |
WO2020063878A1 (en) | Data processing method and apparatus | |
CN107462897B (en) | Three-dimensional mapping method based on laser radar | |
CN114332348B (en) | A 3D Orbital Reconstruction Method Fused with LiDAR and Image Data | |
CN103412565A (en) | robot with global location rapid estimating capability and positioning method thereof | |
CN107941217A (en) | A kind of robot localization method, electronic equipment, storage medium, device | |
CN110412596A (en) | A robot positioning method based on image information and laser point cloud | |
JP7173471B2 (en) | 3D position estimation device and program | |
CN114325664B (en) | A robust lidar-insulator calibration method | |
CN114429432A (en) | A kind of multi-source information layered fusion method, device and storage medium | |
CN116429084A (en) | A 3D Synchronous Positioning and Mapping Method for Dynamic Environment | |
CN117288177A (en) | Laser SLAM method for solving dynamic ghost | |
CN115060268B (en) | Machine room fusion positioning method, system, equipment and storage medium | |
CN118155039B (en) | Method, device, medium and equipment for constructing positioning map by multi-source information fusion | |
CN117968682B (en) | Dynamic point cloud removing method based on multi-line laser radar and inertial measurement unit | |
CN114037800A (en) | Construction system, method and device of octree map and electronic equipment | |
CN111735443B (en) | Dense target track correlation method based on assignment matrix |
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 | ||
TR01 | Transfer of patent right |
Effective date of registration: 20220324 Address after: Room 101-110, 1st floor, building 8, Jinsheng Industrial Park, 611 Dongguan Road, Binjiang District, Hangzhou, Zhejiang 310000 Patentee after: HANGZHOU IPLUS TECH CO.,LTD. Address before: 310000 229, room 1, No. 475 Changhe Road, Changhe street, Binjiang District, Hangzhou, Zhejiang Patentee before: HANGZHOU NANJIANG ROBOTICS Co.,Ltd. |
|
TR01 | Transfer of patent right | ||
PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A odometer method and odometer device for sports equipment Granted publication date: 20191025 Pledgee: Guotou Taikang Trust Co.,Ltd. Pledgor: HANGZHOU IPLUS TECH CO.,LTD. Registration number: Y2024330002319 |
|
PE01 | Entry into force of the registration of the contract for pledge of patent right |