CN101008571A

CN101008571A - Three-dimensional environment perception method for mobile robot

Info

Publication number: CN101008571A
Application number: CN 200710034343
Authority: CN
Inventors: 蔡自兴; 邹小兵; 王璐; 段琢华; 于金霞; 文志强; 陈白帆; 郑敏捷
Original assignee: Central South University
Current assignee: Central South University
Priority date: 2007-01-29
Filing date: 2007-01-29
Publication date: 2007-08-01

Abstract

一种移动机器人三维环境感知方法，由环境信息的获取、环境信息自适应滤波、环境信息的坐标变换和三维环境感知四部分组成。本发明通过设计一个感知平台来获取机器人周围环境信息，该感知平台由二维激光雷达、旋转云台和步进电机三部分组成，通过对步进电机的控制，使感知平台在俯仰和水平方向转动获取环境信息。针对环境信息的噪声干扰，提出了一种动态自适应滤波器，实现实时动态滤波除噪。针对感知平台，提出了坐标变换公式，实现从环境信息到高度图转换。针对简单的高度图，提出三维环境感知方法，对高度图进行地形平坦性分析，分割出环境地图中的可行进区域与障碍区域。移动机器人三维环境感知方法的实现，能为机器人的避障提供局部环境地图。A three-dimensional environment perception method for a mobile robot is composed of four parts: acquisition of environment information, adaptive filtering of the environment information, coordinate transformation of the environment information and three-dimensional environment perception. The present invention obtains the surrounding environment information of the robot by designing a sensing platform. The sensing platform is composed of three parts: two-dimensional laser radar, rotating platform and stepping motor. Turn for environmental information. Aiming at the noise interference of environmental information, a dynamic adaptive filter is proposed to realize real-time dynamic filtering and denoising. For the perception platform, a coordinate transformation formula is proposed to realize the conversion from environmental information to height map. Aiming at the simple height map, a three-dimensional environment perception method is proposed, and the terrain flatness analysis is performed on the height map, and the travelable area and the obstacle area in the environment map are segmented. The realization of the mobile robot's three-dimensional environment perception method can provide a local environment map for the robot's obstacle avoidance.

Description

A three-dimensional environment perception method for mobile robots

技术领域technical field

本发明涉及移动机器人的外部信息感知及导航方法，是一种非结构化环境下的环境感知方法。The invention relates to an external information perception and navigation method of a mobile robot, which is an environment perception method in an unstructured environment.

背景技术Background technique

长期以来缺乏必要的获取三维环境信息手段。多目计算机视觉在三维建模过程中，易受到光照、分辨率、焦距调节、关注点选择等多种因素影响。近年来发展起来的激光雷达三维成像技术，能够有效地解决视觉技术在获得深度信息的难题。激光测距系统包括单点的测距传感器、在平面上进行线扫描的二维激光雷达以及能够对一个区域进行面扫描的三维激光雷达。在结构化的运行环境中，激光雷达通常被水平安置在移动机器人的某个高度上实现在二维平面上的障碍检测。在非结构化环境中，由于可能存在不同高度的障碍物，因此需要激光雷达能够实现面扫描的功能。但是目前能够进行三维环境测量的激光雷达系统还很昂贵，而且体积、重量都还不适应于一般移动机器人上的应用。For a long time, there has been a lack of necessary means to obtain 3D environmental information. In the process of 3D modeling, multi-eye computer vision is easily affected by various factors such as illumination, resolution, focus adjustment, and focus point selection. The lidar 3D imaging technology developed in recent years can effectively solve the problem of visual technology in obtaining depth information. The laser ranging system includes a single-point ranging sensor, a two-dimensional laser radar that scans a line on a plane, and a three-dimensional laser radar that can scan an area. In a structured operating environment, the lidar is usually placed horizontally at a certain height of the mobile robot to detect obstacles on a two-dimensional plane. In an unstructured environment, since there may be obstacles of different heights, lidar is required to be able to realize the function of area scanning. However, the current laser radar system capable of measuring the three-dimensional environment is still very expensive, and its volume and weight are not yet suitable for applications on general mobile robots.

发明内容Contents of the invention

为克服二维激光雷达的不足，本发明设计并实现了一种基于二维激光雷达的移动机器人三维环境感知方法。该方法使机器人不仅能获取其周围的环境信息，而且能感知三维环境，区分环境地图中的可行区域与障碍区域。In order to overcome the shortcomings of the two-dimensional laser radar, the present invention designs and implements a three-dimensional environment perception method for mobile robots based on the two-dimensional laser radar. This method enables the robot not only to obtain the environment information around it, but also to perceive the three-dimensional environment, and to distinguish the feasible area and the obstacle area in the environment map.

该发明解决其技术问题所采用的技术方案是：该发明由环境信息的获取、环境信息自适应滤波、环境信息的坐标变换和三维环境感知四部分组成，每部分的内容如下。The technical solution adopted by the invention to solve its technical problems is: the invention consists of four parts: acquisition of environmental information, adaptive filtering of environmental information, coordinate transformation of environmental information and three-dimensional environmental perception. The contents of each part are as follows.

1环境信息的获取1 Acquisition of Environmental Information

通过设计一个感知平台来获取机器人周围环境信息。该感知平台由二维激光雷达、旋转云台和步进电机三部分组成。该平台通过二个高精密的电控转台实现水平与俯仰角度的精确转动，传感器转动云台为机器人的“头部”。云台由水平转台与俯仰转台组合而成，激光雷达的信息处理与传感器云台的控制由一台工控机(IPC)实现。IPC配置了一块PCI插槽的高速串行通讯接口卡，与激光雷达通讯。一块基于ISA插槽的步进电机控制卡对传感器云台的水平与俯仰转动进行控制。在工控机端设置转台控制策略，控制程序根据获取的外界信息，选择合适的控制策略，控制平台的俯仰。By designing a perception platform to obtain the environment information of the robot. The perception platform consists of three parts: two-dimensional laser radar, rotating pan-tilt and stepping motor. The platform realizes the precise rotation of horizontal and pitch angles through two high-precision electronically controlled turntables, and the sensor rotating platform is the "head" of the robot. The pan/tilt is composed of a horizontal turntable and a pitch turntable. The information processing of the laser radar and the control of the sensor pan/tilt are realized by an industrial computer (IPC). The IPC is equipped with a high-speed serial communication interface card with a PCI slot to communicate with the lidar. A stepper motor control card based on the ISA slot controls the pan and tilt rotation of the sensor gimbal. The turntable control strategy is set on the industrial computer, and the control program selects an appropriate control strategy based on the obtained external information to control the pitch of the platform.

2环境信息的自适应滤波2 Adaptive filtering of environmental information

激光雷达的测距信息往往会包含一定的噪声干扰。传感器测量过程中的干扰主要包含了有源干扰与混合像素干扰二类，此外由于障碍物的遮挡，存在激光雷达扫描的盲区；激光雷达的扫描点存在间隙，当机器人以一定速度前进或转动时，激光雷达按25Hz的频率扫描环境，也可能会存在一定的扫描间隙。所以必须对激光雷达的测距信息进行滤波。The ranging information of lidar often contains certain noise interference. The interference in the sensor measurement process mainly includes two types of active interference and mixed pixel interference. In addition, due to the obstruction of obstacles, there is a blind area for lidar scanning; there is a gap in the scanning point of lidar. When the robot advances or rotates at a certain speed , the lidar scans the environment at a frequency of 25Hz, and there may also be a certain scanning gap. Therefore, the ranging information of the lidar must be filtered.

3环境信息的坐标变换3 Coordinate transformation of environmental information

通过二个步骤实现激光雷达测量环境信息的三维坐标变换。第一步，将测量信息映射到以机器人为参考中心，机器人车体平台为参考面的三维坐标系中。设传感器云台中激光雷达扫描圆心处的坐标系为{O₂}，云台面绕坐标系{O₁}的y₁轴做俯仰角为γ_p的转动。云台的水平转动等效为围绕机器人参考中心坐标系{O_r}绕z_r轴的转动，设转动角度为γ_h。O₁在坐标系{O_r}中沿z_r平移d₀；O₂在坐标系{O₁}中沿z₁平移d₁，沿x₁平移d₂。The three-dimensional coordinate transformation of the laser radar measurement environment information is realized through two steps. The first step is to map the measurement information to a three-dimensional coordinate system with the robot as the reference center and the robot car body platform as the reference plane. Assume that the coordinate system at the center of the lidar scanning circle in the sensor pan/tilt is {O ₂ }, and the pan/tilt rotates around the _y1 axis of the coordinate system {O ₁ } with a pitch angle of γ _p . The horizontal rotation of the gimbal is equivalent to the rotation around the robot reference center coordinate system {O _r } around the z _r axis, and the rotation angle is γ _h . O ₁ translates d ₀ along z _r in the coordinate system {O _r }; O ₂ translates d ₁ along z ₁ and d ₂ along x ₁ in the coordinate system {O ₁ }.

激光雷达的扫描面为x₂-y₂坐标面上以O₂为圆心-90～+90°范围的辐射扇面。测量数据表示为极坐标测量值(ρ_i，j，λ_i，j)，ρ_i，j为测量环境信息，λ_i，j为极角(以x₂为极轴)。下标i表示以主程序运行周期(40ms)为度量的时刻。在时刻i，激光雷达扫描的数据包中包含了361个测量数据。下标j代表了一个数据包中测量点的编号，将测量值(ρ_i，j，λ_i，j)转化为向量 $u |_{O_{2}} = [ρ_{i, j} {\cos λ}_{i, j}, ρ_{i, j} \sin λ_{i, j}, 0,1]^{T} .$ The scanning surface of the lidar is a radiation sector in the range of -90° to +90° on the x ₂ -y ₂ coordinate plane with O ₂ as the center. The measurement data are expressed as polar coordinate measurement values (ρ _{i, j} , λ _{i, j} ), where ρ _{i, j} is the measurement environment information, and λ _{i, j} is the polar angle (with x ₂ as the polar axis). The subscript i represents the moment measured by the running cycle of the main program (40ms). At time i, the lidar scan data packet contains 361 measurement data. The subscript j represents the number of the measurement point in a data packet, and the measurement value (ρ _{i, j} , λ _{i, j} ) is converted into a vector $u |_{o_{2}} = [ρ_{i, j} {\cos λ}_{i, j}, ρ_{i, j} \sin λ_{i, j}, 0,1]^{T} .$

第二步，考虑移动机器人在三维地形曲面上的坐标变化，把测量信息映射到全局坐标系{O}中。移动机器人的姿态可以用状态(x_i，y_i，z_i，θ_i，α_i，φ_i)来表示，其中θ_i表示机器人的航向角，α_i表示机器人的俯仰角，φ_i表示机器人的横滚角。In the second step, considering the coordinate change of the mobile robot on the three-dimensional terrain surface, the measurement information is mapped to the global coordinate system {O}. The attitude of a mobile robot can be represented by the state (xi _, y _, zi _, _θi , _αi , _φi ), where _θi represents the heading angle of the robot, _αi represents the pitch angle of the robot, and _φi represents the roll angle.

经坐标变换以后，用二维数组A_[m][n]来记录平面上环境地形的高度信息。数组A_[m][n]的数值就代表了地形曲面与基准面的相对高度。After the coordinate transformation, use the two-dimensional array A _{[m] [n]} to record the height information of the environmental terrain on the plane. The value of the array A _[m][n] represents the relative height between the terrain surface and the reference plane.

4三维环境感知4 3D environment perception

移动机器人在运动过程中，环境感知平台以一定的角度(如-45°)对环境进行探测，机器人前方的环境被感知平台检测转化环境信息，经过自适应滤波和坐标变换，转化为环境信息的高度图，三维环境地图的创建必须通过累积这种高度图来完成。在感知三维环境之前，必须针对通过感知平台获得的三维环境高度图进行滤波除噪和栅格缺损插补，以降低测量噪声和减少扫描间隙造成的物体表面缺损。三维环境感知的主要目的是累计高度图，对高度图进行地形平坦性分析，分割出环境地图中的可行进区域与障碍区域。During the movement of the mobile robot, the environment perception platform detects the environment at a certain angle (such as -45°), and the environment in front of the robot is detected by the perception platform and transformed into environmental information. Heightmaps, the creation of a 3D environment map must be done by accumulating such heightmaps. Before sensing the 3D environment, filtering and denoising and grid defect interpolation must be performed on the 3D environment height map obtained through the sensing platform to reduce measurement noise and reduce object surface defects caused by scanning gaps. The main purpose of 3D environment perception is to accumulate the height map, analyze the terrain flatness of the height map, and segment the travelable area and obstacle area in the environmental map.

本发明具有较好的灵活性，能够实现非结构环境下的障碍检测与地形分析，为机器人的避障提供局部环境地图。The invention has good flexibility, can realize obstacle detection and terrain analysis in non-structural environment, and can provide a local environment map for robot obstacle avoidance.

附图说明Description of drawings

图1移动机器人三维环境感知方法的流程图；Fig. 1 is a flowchart of a three-dimensional environment perception method for a mobile robot;

图2激光雷达与旋转云台的示意图；Figure 2 Schematic diagram of lidar and rotating pan/tilt;

图3激光雷达测量系统的坐标变换示意图。Fig. 3 Schematic diagram of coordinate transformation of lidar measurement system.

具体实施方式Detailed ways

下面结合附图和实施方式对本发明进一步说明。The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

附图2为激光雷达与转动云台的示意图，1是俯仰旋转云台，2是激光雷达，3是扫描光心，4是水平转动中心，5是俯仰转动中心，6是步进电机，7是水平旋转平台，8是步进电机。Accompanying drawing 2 is the schematic diagram of lidar and rotating pan-tilt, 1 is pitching and rotating pan-tilt, 2 is lidar, 3 is scanning optical center, 4 is horizontal rotation center, 5 is pitch rotation center, 6 is stepper motor, 7 is a horizontal rotary platform, and 8 is a stepper motor.

二维激光雷达2装载在俯仰旋转云台1上，由工控机(IPC)通过步进电机6控制激光雷达2绕俯仰转动中心5在垂直方向旋转。激光雷达2、俯仰转动中心5、步进电机6都安装在水平旋转平台7的上方，由工控机(IPC)通过步进电机8，控制激光雷达绕在水平转动中心4在水平方向旋转。The two-dimensional laser radar 2 is loaded on the tilting and rotating platform 1, and the industrial computer (IPC) controls the laser radar 2 to rotate in the vertical direction around the pitching rotation center 5 through the stepping motor 6. The laser radar 2, the pitch rotation center 5, and the stepper motor 6 are all installed above the horizontal rotation platform 7, and the industrial computer (IPC) controls the laser radar to rotate in the horizontal direction around the horizontal rotation center 4 through the stepper motor 8.

附图3包括激光雷达扫描圆心处的坐标系{O₂}，云台面绕坐标系{O₁}和全局坐标系{O}。Attached Figure 3 includes the coordinate system {O ₂ } at the center of the laser radar scanning circle, the gimbal surface circumscribing coordinate system {O ₁ } and the global coordinate system {O}.

1环境信息的获取1 Acquisition of Environmental Information

通过设计一个感知平台来获取机器人周围环境信息。该感知平台由二维激光雷达、旋转云台和步进电机三部分组成。该平台通过二个高精密的电控转台实现水平与俯仰角度的精确转动，传感器转动云台为机器人的“头部”。云台曰水平转台与俯仰转台组合而成，该平台可以实现水平方向±150°、俯仰方向-50～+15°的扫描运动。旋转台的驱动电机采用24VDC的步进电机驱动，步距角为1.8°。驱动器采用10倍细分方式，即一个驱动脉冲产生0.18°的电机旋转。同时旋转台的机械减速比为180∶1，即电机旋转180周时转台旋转1周。一个步进脉冲驱动转台旋转0.001°，从电气控制上保障了旋转台位置的精度。激光雷达及其安装部件的重量约为6Kg，水平转动的最大速度为16°/s，俯仰转动的最大速度为8°/s，旋转台所能达到的重复定位精度为0.01°By designing a perception platform to obtain the environment information of the robot. The perception platform consists of three parts: two-dimensional laser radar, rotating pan-tilt and stepping motor. The platform realizes the precise rotation of horizontal and pitch angles through two high-precision electronically controlled turntables, and the sensor rotating platform is the "head" of the robot. The cloud platform is a combination of a horizontal turntable and a pitch turntable. The platform can realize the scanning movement of ±150°in the horizontal direction and -50～+15°in the pitching direction. The drive motor of the rotary table is driven by a 24VDC stepper motor with a step angle of 1.8°. The driver adopts a 10-fold subdivision method, that is, one driving pulse generates a 0.18° motor rotation. At the same time, the mechanical reduction ratio of the turntable is 180:1, that is, the turntable rotates 1 turn when the motor rotates 180 turns. A step pulse drives the turntable to rotate 0.001°, which ensures the accuracy of the turntable position from the electrical control. The weight of the lidar and its installation parts is about 6Kg, the maximum speed of horizontal rotation is 16°/s, the maximum speed of pitch rotation is 8°/s, and the repeat positioning accuracy that the rotary table can achieve is 0.01°

激光雷达的信息处理与传感器云台的控制由一台工控机(IPC)实现。IPC配置了一块PCI插槽的高速串行通讯接口卡，与激光雷达通过RS422接口实现500K波特率下的通讯。一块基于ISA插槽的步进电机控制卡PCL839对传感器云台的水平与俯仰转动进行控制。在工控机端设置转台控制策略，控制程序根据获取的外界信息，选择合适的控制策略，控制平台的俯仰。The information processing of the lidar and the control of the sensor pan/tilt are realized by an industrial computer (IPC). The IPC is equipped with a high-speed serial communication interface card with a PCI slot, and communicates with the lidar at a 500K baud rate through the RS422 interface. A stepper motor control card PCL839 based on the ISA slot controls the pan and pitch rotation of the sensor pan/tilt. The turntable control strategy is set on the industrial computer, and the control program selects an appropriate control strategy based on the obtained external information to control the pitch of the platform.

激光雷达采用180°/0.5°模式，每次扫描获得361个测量数据。每个测量数据为2个字节，包含起始码与校验码的数据包长度为732个字节。在500K波特率的通讯速率下，传输延迟时间约为13ms，激光雷达的扫描时间为26.67ms，激光雷达的扫描频率约为25Hz，即每秒可以获得25×361＝9025个测量点的数据。The lidar adopts 180°/0.5° mode, and obtains 361 measurement data per scan. Each measurement data is 2 bytes, and the length of the data packet including the start code and check code is 732 bytes. At the communication rate of 500K baud rate, the transmission delay time is about 13ms, the scanning time of the laser radar is 26.67ms, and the scanning frequency of the laser radar is about 25Hz, that is, the data of 25×361=9025 measurement points can be obtained per second .

激光雷达的测距信息往往会包含一定的噪声干扰。传感器测量过程中的干扰主要包含了有源干扰与混合像素干扰二类，此外由于障碍物的遮挡，存在激光雷达扫描的盲区；激光雷达的扫描点存在间隙，当机器人以一定速度前进或转动时，激光雷达按25Hz的频率扫描环境，也可能会存在一定的扫描间隙。所以必须对激光雷达的测距信息进行滤波。本发明提出一种在线滚动的动态自适应滤波(Dynamic adaptive filter，DAF)方法，该方法针对测量值(ρ_i，j，λ_i，j)，其中ρ_i，j为测量环境信息，λ_i，j为极角，建立如下的数据分析窗口：The ranging information of lidar often contains certain noise interference. The interference in the sensor measurement process mainly includes two types of active interference and mixed pixel interference. In addition, due to the obstruction of obstacles, there is a blind area for lidar scanning; there is a gap in the scanning point of lidar. When the robot advances or rotates at a certain speed , the lidar scans the environment at a frequency of 25Hz, and there may also be a certain scanning gap. Therefore, the ranging information of the lidar must be filtered. The present invention proposes an online rolling dynamic adaptive filter (DAF) method, the method is aimed at the measured value (ρ _{i, j} , λ _{i, j} ), where ρ _{i, j} is the measurement environment information, λ _{i , j} is the polar angle, establish the following data analysis window:

ρ_i-1，j-1，ρ_i-1，j，ρ_i-1，j+1 ρ _{i-1, j-1} , ρ _{i-1, j} , ρ _{i-1, j+1}

ρ_i，j-1，ρ_i，j，ρ_i，j+1 (1)ρ _{i, j-1} , ρ _{i, j} , ρ _{i, j+1} (1)

ρ_i+1，j-1，ρ_i+1，j，ρ_i+1，j+1 ρ _{i+1, j-1} , ρ _{i+1, j} , ρ _{i+1, j+1}

i表示一个数据包的编号，一个数据包有361个测量数据，采样时间间隔为40ms；j是同一组数据中的序列号。上述9个测量值具有时间与空间上最大的相关性，在数据窗口中计算出ρ_i，j与邻近测量值之差Δρ_min：i represents the number of a data packet, a data packet has 361 measurement data, and the sampling time interval is 40ms; j is the serial number in the same group of data. The above nine measured values have the largest correlation in time and space, and the difference Δρ _min between ρ _{i, j} and the adjacent measured values is calculated in the data window:

Δρ_min＝min{|ρ_t+i，s+i-ρ_i，j|，t，s＝-1，0，1，且t，s不同时为0}Δρ _min ＝min{|ρ _{t+i, s+i} -ρ _{i, j} |, t, s=-1, 0, 1, and t, s are not 0 at the same time}

Δρ_min是时间与空间上相邻测量的最小差值，简称邻近差值。如果Δρ_min＞δ(ρ，v)，则测量值ρ_i，j就被当作测量噪声而不进入动态环境知识库中进行高度图的计算，δ(ρ，v)由(2)式确定。Δρ _min is the minimum difference between adjacent measurements in time and space, referred to as adjacent difference. If Δρ _min > δ(ρ, v), the measured value ρ _{i, j} will be regarded as measurement noise and not entered into the dynamic environment knowledge base for the calculation of the height map, and δ(ρ, v) is determined by formula (2) .

$δ δ ((ρ ρ,, v v)) = = σ σ ((ρ ρ)) + + \frac{11}{2525} ((| | {v v}_{goal goal} | | + + | | {v v}_{robot robot} | |)) - - - - - - ((22))$

σ(ρ)是激光雷达测距的标准差，通过对激光雷达在不同环境测距数据的统计分析获得。v_goal是在环境中可能存在的动态目标的运动速度，V_robot是机器人自身的运动速度。σ(ρ) is the standard deviation of lidar ranging, which is obtained by statistical analysis of ranging data of lidar in different environments. v _goal is the moving speed of the dynamic target that may exist in the environment, and V _robot is the moving speed of the robot itself.

通过二个步骤实现激光雷达测量环境信息的三维坐标变换。第一步，将测量信息映射到以机器人为参考中心，机器人车体平台为参考面的三维坐标系中。设传感器云台中激光雷达扫描圆心处的坐标系为{O₂}，云台面绕坐标系{O₁}的y₁轴做俯仰角为γ_p的转动。云台的水平转动等效为围绕机器人参考中心坐标系{O_r}绕z_r轴的转动，设转动角度为γ_h。O₁在坐标系{O_r}中沿z_r平移d₀；O₂在坐标系{O₁}中沿z₁平移d₁，沿x₁平移d₂，如附图3。The three-dimensional coordinate transformation of the laser radar measurement environment information is realized through two steps. The first step is to map the measurement information to a three-dimensional coordinate system with the robot as the reference center and the robot car body platform as the reference plane. Assume that the coordinate system at the center of the lidar scanning circle in the sensor pan/tilt is {O ₂ }, and the pan/tilt rotates around the _y1 axis of the coordinate system {O ₁ } with a pitch angle of γ _p . The horizontal rotation of the gimbal is equivalent to the rotation around the robot reference center coordinate system {O _r } around the z _r axis, and the rotation angle is γ _h . O ₁ translates d ₀ along z _r in the coordinate system {O _r }; O ₂ translates d 1 along z ₁ and d ₂ along x ₁ in the coordinate system {O ₁ }, as shown in Figure 3 _.

激光雷达的扫描面为x₂-y₂坐标面上以O₂为圆心-90~+90°范围的辐射扇面。测量数据表示为极坐标测量值(ρ_i，j，λ_i，j)，ρ_i，j为测量环境信息，λ_i，j为极角(以x₂为极轴)。下标i表示以主程序运行周期(40ms)为度量的时刻。在时刻i，激光雷达扫描的数据包中包含了361个测量数据。下标j代表了一个数据包中测量点的编号，将测量值(ρ_i，j，λ_i，j)转化为向量

The scanning surface of the laser radar is a radiation fan in the range of -90~+90° on the x ₂ -y ₂ coordinate plane with O ₂ as the center. The measurement data are expressed as polar coordinate measurement values (ρ _{i, j} , λ _{i, j} ), where ρ _{i, j} is the measurement environment information, and λ _{i, j} is the polar angle (with x ₂ as the polar axis). The subscript i represents the moment measured by the running cycle of the main program (40ms). At time i, the lidar scan data packet contains 361 measurement data. The subscript j represents the number of the measurement point in a data packet, and the measurement value (ρ _{i, j} , λ _{i, j} ) is converted into a vector

设在时刻j传感器转台的状态为{γ_h，γ_p}，将向量u|_O2转换为机器人参考系{O_r}中的向量u|_Or。以下公式中Tran()表示坐标平移变换，Rot()表示围绕某坐标轴进行旋转变换，在变换矩阵中s表示正弦函数sin()，c表示余弦函数cos()：Assuming that the state of the sensor turntable at time j is {γ _h , γ _p }, transform the vector u| _O2 into the vector u| _Or in the robot reference frame {O _r }. In the following formula, Tran() represents coordinate translation transformation, Rot() represents rotation transformation around a certain coordinate axis, in the transformation matrix, s represents the sine function sin(), and c represents the cosine function cos():

$u u {| |}_{{O o}_{r r}} = = \overset{{O o}_{r r}}{{O o}_{22}} T T \cdot &Center Dot; u u {| |}_{{O o}_{22}} - - - - - - ((33))$

其中：in:

$\overset{{O o}_{r r}}{{O o}_{22}} T T = = Trans Trans ((0,0 0,0,, {d d}_{00})) Rot Rot (({z z}_{r r},, {γ γ}_{h h})) Rot Rot (({v v}_{11},, {γ γ}_{p p})) Trans Trans (({d d}_{22},, 00,, {d d}_{11})) - - - - - - ((44))$

经过计算得到：Calculated to get:

$\overset{{O o}_{r r}}{{O o}_{22}} T T = = [\begin{matrix} {cγ cγ}_{h h} {cγ cγ}_{p p} & - - {sγ sγ}_{h h} & {cγ cγ}_{h h} {sγ sγ}_{p p} & {d d}_{22} {cγ cγ}_{p p} {cγ cγ}_{h h} + + {d d}_{11} {sγ sγ}_{p p} {cγ cγ}_{h h} \\ s the s {γ γ}_{h h} {cγ cγ}_{p p} & {cγ cγ}_{h h} & {sγ sγ}_{h h} {sγ sγ}_{p p} & {d d}_{22} {cγ cγ}_{p p} {sγ sγ}_{h h} + + {d d}_{11} {sγ sγ}_{p p} {sγ sγ}_{h h} \\ - - {sγ sγ}_{p p} & 00 & {cγ cγ}_{p p} & - - {d d}_{22} {sγ sγ}_{p p} + + {d d}_{11} {cγ cγ}_{p p} + + {d d}_{00} \\ 00 & 00 & 00 & 11 \end{matrix}]$

第二步，考虑移动机器人在三维地形曲面上的坐标变化，把测量信息映射到全局坐标系{O}中。移动机器人的姿态可以用状态(x_i，y_i，z_i，θ_i，α_i，φ_i)来表示，其中θ_i表示机器人的航向角，α_i表示机器人的俯仰角，φ_i表示机器人的横滚角，如附图3。In the second step, considering the coordinate change of the mobile robot on the three-dimensional terrain surface, the measurement information is mapped to the global coordinate system {O}. The attitude of a mobile robot can be represented by the state (xi _, y _, zi _, _θi , _αi , _φi ), where _θi represents the heading angle of the robot, _αi represents the pitch angle of the robot, and _φi represents the roll angle, as shown in Figure 3.

机器人的航向角由光纤陀螺输出，俯仰角与横滚角由机器人平台上的倾角仪输出。坐标平移到(x_i，y_i，z_i)，再绕平移后的z轴方向旋转θ(航向角)。绕旋转后的y轴方向旋转α(俯仰角)，最后围绕新坐标的x_r轴旋转φ_i′(横滚角)：The heading angle of the robot is output by the fiber optic gyroscope, and the pitch and roll angles are output by the inclinometer on the robot platform. The coordinates are translated to (x _i , y _i , zi ₎ , and then rotated around the translated z-axis direction by θ (the heading angle). Rotate α (pitch angle) around the rotated y-axis direction, and finally rotate φ _i ′ (roll angle) around the x _r axis of the new coordinates:

${φ φ}_{i i}^{' '} = = arcsin arcsin \frac{{sin sin φ φ}_{i i}}{{cos cos α α}_{i i}} - - - - - - ((55))$

φ_i是倾角传感器直接测量的机器人坐标系y_r轴与参考水平面的夹角，φ_i′则是考虑俯仰角旋转后围绕x_r轴的横滚转动。经过上述坐标变换，最后得到在世界坐标系{O}中的位置向量：φ _i is the angle between the y _r axis of the robot coordinate system measured directly by the inclination sensor and the reference horizontal plane, and φ _i ′ is the roll rotation around the x _r axis after considering the pitch angle rotation. After the above coordinate transformation, the position vector in the world coordinate system {O} is finally obtained:

$u u {| |}_{O o} = = {[[{x x}_{O o | | i i,, j j},, {y the y}_{O o | | i i,, j j},, {z z}_{O o | | i i,, j j},, 11]]}^{T T} = = \overset{O o}{{O o}_{r r}} T T \cdot \cdot u u {| |}_{{O o}_{r r}} - - - - - - ((66))$

其中： $\overset{O}{O_{r}} T = Trans (x_{i}, y_{i}, z_{i}) Rot (z, θ_{i}) Rot (y, α_{i}) Rot (x, {φ^{'}}_{i}) - - - (7)$ in: $\overset{o}{o_{r}} T = Trans (x_{i}, {the y}_{i}, z_{i}) Rot (z, θ_{i}) Rot (the y, α_{i}) Rot (x, {φ^{'}}_{i}) - - - (7)$

经过计算得到：Calculated to get:

$\overset{O o}{{O o}_{r r}} T T = = [\begin{matrix} {cθ cθ}_{i i} {cα cα}_{i i} & {cθ cθ}_{i i} {sα sα}_{i i} {sφ sφ}^{' '}_{i i} - - {sθ sθ}_{i i} {cφ cφ}^{' '}_{i i} & {cθ cθ}_{i i} {sα sα}_{i i} {cφ cφ}_{i i}^{' '} + + {sθ sθ}_{i i} {sφ sφ}^{' '}_{i i} & {x x}_{i i} \\ {sθ sθ}_{i i} {cα cα}_{i i} & {sθ sθ}_{i i} {cα cα}_{i i} {sφ sφ}_{i i}^{' '} & {sθ sθ}_{i i} {sα sα}_{i i} {sφ sφ}_{i i}^{' '} - - {cθ cθ}_{i i} {cφ cφ}_{i i}^{' '} & {y the y}_{i i} \\ - - {sα sα}_{i i} & {cα cα}_{i i} {sφ sφ}_{i i}^{' '} & {cα cα}_{i i} {cφ cφ}_{i i}^{' '} & {z z}_{i i} \\ 00 & 00 & 00 & 11 \end{matrix}]$

经坐标变换以后，用二维数组A_[m][n]来记录平面上环境地形的高度信息。数组A_[m][n]的数值就代表了地形曲面与基准面的相对高度。m，n代表了在基准面投影上相应的栅格坐标，在基准面上采用3cm解析度的栅格，在高度方向上采用2cm的解析度，则：After the coordinate transformation, use the two-dimensional array A _{[m] [n]} to record the height information of the environmental terrain on the plane. The value of the array A _[m][n] represents the relative height between the terrain surface and the reference plane. m and n represent the corresponding grid coordinates on the datum projection, use a 3cm resolution grid on the datum plane, and use a 2cm resolution in the height direction, then:

${{\begin{matrix} m m = = ((int int)) \frac{{x x}_{O o | | i i,, j j}}{33} \\ n no = = ((int int)) \frac{{y the y}_{O o | | i i,, j j}}{33} \\ {A A}_{[[m m]] [[n no]]} = = ((int int)) \frac{{z z}_{O o | | i i,, j j}}{22} \end{matrix} - - - - - - ((88))$

4三维环境感知4 3D environment perception

移动机器人在运动过程中，环境感知平台以一定的角度(如-45°)对环境进行探测，机器人前方的环境被感知平台检测转化环境信息，经过自适应滤波和坐标变换，转化为环境信息的高度图，三维环境地图的创建必须通过累积这种高度图来完成。在感知三维环境之前，必须针对通过感知平台获得的三维环境高度图进行滤波除噪和栅格缺损插补，以降低测量噪声和减少扫描间隙造成的物体表面缺损。三维环境感知的主要目的是累计高度图，对高度图进行地形平坦性分析，分割出环境地图中的可行进区域与障碍区域。三维环境感知方法如下：During the movement of the mobile robot, the environment perception platform detects the environment at a certain angle (such as -45°), and the environment in front of the robot is detected by the perception platform and transformed into environmental information. Heightmaps, the creation of a 3D environment map must be done by accumulating such heightmaps. Before sensing the 3D environment, filtering and denoising and grid defect interpolation must be performed on the 3D environment height map obtained through the sensing platform to reduce measurement noise and reduce object surface defects caused by scanning gaps. The main purpose of 3D environment perception is to accumulate the height map, analyze the terrain flatness of the height map, and segment the travelable area and obstacle area in the environmental map. The three-dimensional environment perception method is as follows:

1)计算地形高度变化梯度的模，如(9)式，式中g[m][n]表示平面坐标(m，n)上相应的地形高度变化梯度的模。1) Calculate the modulus of the terrain height change gradient, such as formula (9), where g[m][n] represents the modulus of the corresponding terrain height change gradient on the plane coordinates (m, n).

g_[m][n]＝max{|A_[m][n]-A_[m+i][n+j]| i，j＝-1，0，1} (9)g _[m][n] =max{|A _[m][n] -A _[m+i][n+j] | i, j=-1, 0, 1} (9)

2)环境地图创建。环境地图用map_[m][n]表示，初值为-1时，表示未知区域栅格；为0时表示可行进的自由区域栅格；大于等于1表示障碍区域栅格。运用公式(10)创建环境地图，区分环境地图中的可行进区域与障碍区域。2) Environment map creation. The environment map is represented by map _[m][n] . When the initial value is -1, it means an unknown area grid; Use the formula (10) to create the environment map, and distinguish the travelable area and the obstacle area in the environment map.

${map map}_{[[m m]] [[n no]]} = = \{\begin{matrix} {map map}_{[[m m]] [[n no]]} + + 22,, if if {g g}_{[[m m]] [[n no]]} > > 22 \\ O o,, if if {g g}_{[[m m]] [[n no]]} \leq \leq 22 \end{matrix}, - - - - - - ((1010))$

Claims

1. A three-dimensional environment perception method for a mobile robot, characterized in that: obtain environmental information through a perception platform, reduce noise interference through dynamic adaptive filtering, obtain an environmental height map after coordinate transformation, accumulate and map the environmental height map to obtain Local 3D environment map;

The perception platform consists of three parts: two-dimensional laser radar, rotating pan-tilt and stepping motor. Through the control of the stepping motor, the sensing platform can be rotated in the pitch and horizontal directions to obtain environmental information. The adjacent difference in the dynamic adaptive filtering method The way is:

Δρ _min = min{|ρ _{t+i, s+i} -ρ _{i, j} |, t, s=-1, 0, 1, and t, s are not 0 at the same time} If Δρ _min >δ(ρ, v ), then the measured value ρ _{i, j} is regarded as measurement noise and does not enter the dynamic environment knowledge base for the calculation of the height map. The value of δ(ρ, v) is given by the formula:

δ (ρ, ν) = σ (ρ) + \frac{1}{25} (| ν_{goal} | + | ν_{robot} |)

Determine, where σ(ρ) is the standard deviation of the lidar ranging, v _goal is the moving speed of the dynamic target in the environment, v _robot is the moving speed of the robot itself, and the coordinate transformation formula of the environment information is:

In the three-dimensional perception method, the application formula: g _[m][n] = max{|A _[m][n] -A _[m+i][n+j] |}, where i, j=-1, 0, 1, calculate the gradient modulus of terrain height change, where g _[m][n] represents the plane coordinate m, and the modulus of the corresponding terrain height change gradient on n, using the formula:

{map map}_{[[m m]] [[n no]]} = = \{\begin{matrix} {map map}_{[[m m]] [[n no]]} + + 22 & ,, & if if & {g g}_{[[m m]] [[n no]]} > > 22 \\ 00 & ,, & if if & {g g}_{[[m m]] [[n no]]} \leq \leq 22 \end{matrix}

Distinguish between the travelable area and the obstacle area of the environment. The initial value of the environment map map _[m][n] is -1, which means an unknown area grid. Obstacle area raster.