CN102798391B - Laser image positioning navigation device - Google Patents

Abstract

Translated fromChinese

本发明公开了一种激光影像定位导航装置，由若干个激光管、CCD感光器件、DSP图像处理器、主控制器和串口输出端子、定位孔组成；所述CCD感光器件与DSP图像处理器连接，DSP图像处理器与主控制器连接，主控制器将解算的位置和航向信息通过串口输出端子输出。所述若干个激光管产生的若干个激光束在地面汇聚成一个光斑，CCD感光器件对激光斑进行快速成像，DSP图像处理器对每帧图像分析对比，分别计算出帧与帧之间在X和Y方向的像素偏移量，根据CCD感光器件的分辨率计算出定位装置的实际位移偏量，主控制器根据几何关系确定出轮式机器人的准确航向和位置，快速的成像和高度的分辨率使得该定位装置的精度非常高。该定位导航装置不需要人工标志物，对几乎所有路况环境都能实现准确定位，普适性很强，整个装置结构相对简单，稳定性很高。

The invention discloses a laser image positioning and navigation device, which is composed of several laser tubes, a CCD photosensitive device, a DSP image processor, a main controller, a serial port output terminal, and a positioning hole; the CCD photosensitive device is connected with the DSP image processor , the DSP image processor is connected with the main controller, and the main controller outputs the calculated position and heading information through the serial port output terminal. Several laser beams generated by the several laser tubes are converged into a light spot on the ground, and the CCD photosensitive device performs fast imaging on the laser spot, and the DSP image processor analyzes and compares each frame of images, and calculates the distance between frames and frames respectively. and the pixel offset in the Y direction, calculate the actual displacement offset of the positioning device according to the resolution of the CCD photosensitive device, and the main controller determines the accurate heading and position of the wheeled robot according to the geometric relationship, fast imaging and height resolution The rate makes the positioning device very high accuracy. The positioning and navigation device does not need artificial markers, can realize accurate positioning for almost all road conditions, has strong universality, and the whole device has a relatively simple structure and high stability.

Description

Translated fromChinese

一种激光影像定位导航装置A laser image positioning and navigation device

技术领域technical field

本发明涉及一种激光影像定位导航装置，尤其涉及一种适用于轮式机器人精确定位导航的定位导航装置。The invention relates to a laser image positioning and navigation device, in particular to a positioning and navigation device suitable for precise positioning and navigation of a wheeled robot.

背景技术Background technique

近年来，国内机器人技术有了迅速的发展，特别是轮式机器人，可靠性高，适应性强，在家庭服务、医院巡护和娱乐方面应用极其广泛。然而，定位导航技术对于移动机器人是至关重要的，它增强了机器人的自主性和智能性，对机器人技术的发展起着不可或缺的作用。In recent years, domestic robot technology has developed rapidly, especially wheeled robots, which are highly reliable and adaptable, and are widely used in home services, hospital patrols and entertainment. However, positioning and navigation technology is crucial for mobile robots, which enhances the robot's autonomy and intelligence, and plays an indispensable role in the development of robotics.

目前，针对轮式机器人的定位导航技术主要有视觉导航、里程计定位导航和基于标志物的定位导航等。其中，视觉导航对环境变化的抗干扰能力很差，而且定位精度很低，故此很少使用视觉定位导航；里程计是轮式机器人定位导航中常用的技术，它是对固定在轮子上的码盘进行脉冲计数，由于胎压、地面打滑等因素会导致里程计存在严重的积累误差，需要其他传感器技术对其修正，定位精度不是很理想；基于标志物的定位技术是在已知固定位置贴上人工标志物，通过相应传感器对标志物进行识别间接获得位置信息，如专利号为201110260388.5的中国专利，使用红外发射二极管制作点阵路标，并贴附于室内天花板上，广角红外摄像机固定在移动机器人身上，向上拍摄红外路标，通过机器人身上的计算机进行图像分析、实时计算出机器人的位姿。这种方法具有一定的局限性，其点阵路标属于有源标签，每一个标签都是一块电路板，并需要电源供电，成本高，安装、使用不方便，其次，其图像处理使用机载工控计算机，体积较大，成本较高，对不配置工控计算机的中小型机器人根本无法使用。At present, the positioning and navigation technologies for wheeled robots mainly include visual navigation, odometer positioning and navigation, and marker-based positioning and navigation. Among them, the anti-interference ability of visual navigation to environmental changes is very poor, and the positioning accuracy is very low, so visual positioning and navigation is rarely used; odometer is a commonly used technology in positioning and navigation of wheeled robots. Due to factors such as tire pressure and ground slippage, there will be serious accumulated errors in the odometer, which need to be corrected by other sensor technologies, and the positioning accuracy is not very ideal; the positioning technology based on markers is based on a known fixed position. Place artificial markers, and indirectly obtain location information through corresponding sensors to identify markers. For example, the Chinese patent No. 201110260388.5 uses infrared emitting diodes to make dot matrix road signs and attaches them to the indoor ceiling. The wide-angle infrared camera is fixed on the mobile On the robot, the infrared road signs are shot upwards, and the computer on the robot performs image analysis and calculates the robot's pose in real time. This method has certain limitations. The dot matrix road signs are active tags. Each tag is a circuit board and needs power supply. The cost is high, and installation and use are inconvenient. Secondly, the image processing uses airborne industrial control Computers are large in size and high in cost, and cannot be used at all for small and medium-sized robots that are not equipped with industrial computers.

发明内容Contents of the invention

本发明的目的就是为了解决上述问题，提供了一种激光影像定位导航装置，它适用于轮式机器人的室内外定位导航，具有提高定位的精度、稳定性和普适性的优点，并且该装置对实施环境无特殊要求。The purpose of the present invention is to solve the above problems, providing a laser image positioning and navigation device, which is suitable for indoor and outdoor positioning and navigation of wheeled robots, and has the advantages of improving positioning accuracy, stability and universality, and the device There are no special requirements for the implementation environment.

为了实现上述目的，本发明采用的技术方案是：In order to achieve the above object, the technical scheme adopted in the present invention is:

一种激光影像定位导航装置，它包括控制板，在控制板上设有DSP图像处理器，DSP图像处理器分别与CCD感光器件和主控制器连接，主控制器与串口输出端子连接，将解算的位置和航向信息通过串口输出端子输出；CCD感光器件的周向均匀分布有多个激光组件，各激光组件发出的激光束在地面汇聚成一个光斑，该光斑与CCD感光器件处于同一轴线上并保证CCD感光器件感光效果最好，DSP图像处理器对每帧光斑图像分析对比，分别计算出帧与帧之间在X和Y方向的像素偏移量，根据CCD感光器件的分辨率计算出定位装置的实际位移偏量。A laser image positioning and navigation device, which includes a control board, on which a DSP image processor is arranged, the DSP image processor is respectively connected with a CCD photosensitive device and a main controller, and the main controller is connected with a serial port output terminal, and the solution The calculated position and heading information is output through the serial port output terminal; there are multiple laser components evenly distributed in the circumferential direction of the CCD photosensitive device, and the laser beams emitted by each laser component converge into a spot on the ground, which is on the same axis as the CCD photosensitive device And to ensure that the CCD photosensitive device has the best photosensitive effect, the DSP image processor analyzes and compares each frame of the spot image, and calculates the pixel offset between the frames in the X and Y directions, and calculates it according to the resolution of the CCD photosensitive device. The actual displacement offset of the positioning device.

所述激光组件为激光管，激光管焦距可调。The laser component is a laser tube, and the focal length of the laser tube is adjustable.

所述CCD感光器件的拍摄速度为2000帧/秒。The shooting speed of the CCD photosensitive device is 2000 frames/second.

所述控制板的四角设有定位孔。The four corners of the control board are provided with positioning holes.

所述定位装置的实际位移偏量的计算为：The calculation of the actual displacement offset of the positioning device is:

光斑图像帧与帧之间在X和Y方向的像素偏移量VPi_x和VPi_y，根据CCD感光器件的分辨率F计算出定位装置的实际位移偏量Vx和Vy：The pixel offsets VPi_x and VPi_y in the X and Y directions between the spot image frame and the frame, calculate the actual displacement offset Vx and Vy of the positioning device according to the resolution F of the CCD photosensitive device:

$\left\{\begin{array}{c}\mathrm{<span><span>Vx</span>Vx</span>}<span><span>=</span>=</span>\mathrm{<span><span>2.54</span>2.54</span>}<span><span>\&divide;</span>\&divide;</span>\mathrm{<span><span>F</span>f</span>}<span><span>\&times;</span>\&times;</span>\mathrm{<span><span>VP</span>VP</span>}{\mathrm{<span><span>i</span>i</span>}}_{\mathrm{<span><span>x</span>x</span>}}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>cm</span>cm</span>}<span><span>)</span>)</span>\\ \mathrm{<span><span>Vy</span>Vy</span>}<span><span>=</span>=</span>\mathrm{<span><span>2.54</span>2.54</span>}<span><span>\&divide;</span>\&divide;</span>\mathrm{<span><span>F</span>f</span>}<span><span>\&times;</span>\&times;</span>\mathrm{<span><span>VP</span>VP</span>}{\mathrm{<span><span>i</span>i</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>cm</span>cm</span>}<span><span>)</span>)</span>\end{array}\right.<span><span>.</span>.</span>$

所述主控制器根据几何关系确定出轮式机器人的准确航向和位置：The main controller determines the exact heading and position of the wheeled robot according to the geometric relationship:

$\left\{\begin{array}{c}\mathrm{<span><span>\&theta;</span>\&theta;</span>}<span><span>=</span>=</span>\mathrm{<span><span>\&Sigma;</span>\&Sigma;</span>}\mathrm{<span><span>arctan</span>arctan</span>}\frac{\mathrm{<span><span>Vy</span>Vy</span>}}{\mathrm{<span><span>Vx</span>Vx</span>}}\\ \mathrm{<span><span>x</span>x</span>}<span><span>=</span>=</span>\mathrm{<span><span>\&Sigma;</span>\&Sigma;</span>}\sqrt{\mathrm{<span><span>V</span>V</span>}{\mathrm{<span><span>x</span>x</span>}}^{\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>\mathrm{<span><span>V</span>V</span>}{\mathrm{<span><span>y</span>the\; y</span>}}^{\mathrm{<span><span>2</span>2</span>}}}\mathrm{<span><span>cos</span>cos</span>}<span><span>(</span>(</span>\mathrm{<span><span>\&theta;</span>\&theta;</span>}<span><span>+</span>+</span>\mathrm{<span><span>arctan</span>arctan</span>}\frac{\mathrm{<span><span>Vy</span>Vy</span>}}{\mathrm{<span><span>Vx</span>Vx</span>}}<span><span>)</span>)</span>\\ \mathrm{<span><span>y</span>the\; y</span>}<span><span>=</span>=</span>\mathrm{<span><span>\&Sigma;</span>\&Sigma;</span>}\sqrt{\mathrm{<span><span>V</span>V</span>}{\mathrm{<span><span>x</span>x</span>}}^{\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>\mathrm{<span><span>V</span>V</span>}{\mathrm{<span><span>y</span>the\; y</span>}}^{\mathrm{<span><span>2</span>2</span>}}}\mathrm{<span><span>sin</span>sin</span>}<span><span>(</span>(</span>\mathrm{<span><span>\&theta;</span>\&theta;</span>}<span><span>+</span>+</span>\mathrm{<span><span>arctan</span>arctan</span>}\frac{\mathrm{<span><span>Vy</span>Vy</span>}}{\mathrm{<span><span>Vx</span>Vx</span>}}<span><span>)</span>)</span>\end{array}\right.$

其中，θ为机器人航向角度，x和y分别是机器人在平面坐标系中的横、纵坐标。Among them, θ is the heading angle of the robot, and x and y are the horizontal and vertical coordinates of the robot in the plane coordinate system, respectively.

本发明的有益效果：Beneficial effects of the present invention:

快速的成像和高度的分辨率使得该定位装置的精度非常高，通过对轮式机器人进行测试，灵敏度在毫米级，定位误差在1％以内，完全满足机器人定位技术的要求，这为机器人准确智能的导航奠定了坚实的基础。Fast imaging and high resolution make the positioning device very high in accuracy. Through the test of the wheeled robot, the sensitivity is at the millimeter level, and the positioning error is within 1%, which fully meets the requirements of robot positioning technology. This is an accurate and intelligent robot. The navigation has laid a solid foundation.

采用激光影像技术进行定位，精度远远高于现有的机器人定位技术，不需要人工标志物，对几乎所有路况环境都能实现准确定位，普适性很强，整个装置结构相对简单，稳定性很高。Using laser imaging technology for positioning, the accuracy is much higher than the existing robot positioning technology, no artificial markers are required, and accurate positioning can be achieved for almost all road conditions. It is highly universal, and the entire device structure is relatively simple and stable. very high.

附图说明Description of drawings

图1为本发明的硬件结构框图；Fig. 1 is a block diagram of hardware structure of the present invention;

图2为激光影像原理图；Figure 2 is a schematic diagram of laser imaging;

图3为激光影像定位导航装置工作流程图。Fig. 3 is a working flow diagram of the laser image positioning and navigation device.

图中，1、DSP图像处理器，2、主控制器，3、CCD感光器件，4、激光管，5、串口输出端子，6、定位孔，7、光斑。In the figure, 1. DSP image processor, 2. Main controller, 3. CCD photosensitive device, 4. Laser tube, 5. Serial port output terminal, 6. Positioning hole, 7. Light spot.

具体实施方式Detailed ways

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

如图1、2，一种激光影像定位导航装置，包括激光管4、CCD感光器件3、DSP图像处理器1和主控制器2，所述激光管4是均匀分布在与CCD感光孔同心圆环上的，根据该装置在使用过程中距离地面的实际高度调节激光管4的倾斜角度，以确保若干个激光束在地面汇聚成一个光斑7，这样，光斑7与CCD感光孔就会处于同一轴线上，使CCD感光器件3达到最好的感光效果，CCD感光器件3与DSP图像处理器1连接，DSP图像处理器1与主控制器2连接，主控制器2将解算的位置和航向信息通过串口输出端子5输出，定位孔6在将定位导航装置安装在轮式机器人上时起固定作用。As shown in Figures 1 and 2, a laser image positioning and navigation device includes a laser tube 4, a CCD photosensitive device 3, a DSP image processor 1 and a main controller 2, and the laser tube 4 is evenly distributed in a circle concentric with the CCD photosensitive hole On the ring, adjust the inclination angle of the laser tube 4 according to the actual height of the device from the ground during use to ensure that several laser beams converge into a spot 7 on the ground, so that the spot 7 and the photosensitive hole of the CCD will be in the same position. On the axis, make the CCD photosensitive device 3 achieve the best photosensitive effect, the CCD photosensitive device 3 is connected with the DSP image processor 1, and the DSP image processor 1 is connected with the main controller 2, and the main controller 2 will calculate the position and heading The information is output through the serial port output terminal 5, and the positioning hole 6 plays a fixed role when the positioning and navigation device is installed on the wheeled robot.

CCD感光器件3以高达2000帧/秒的速度对光斑7进行快速成像，由于该装置在移动的过程中，光斑7经过地面反射的影像会有细微的差别，高分辨率、快速成像的CCD感光器件3能够完全捕捉到这种差别，DSP图像处理器1对每帧图像的差别分析对比，可以分别计算出帧与帧之间在X和Y方向的像素偏移量VPi_x和VPi_y，根据CCD感光器件3的分辨率F=800CPI可以计算出定位装置的实际位移偏量Vx和Vy：The CCD photosensitive device 3 can quickly image the light spot 7 at a speed of up to 2000 frames per second. Because the device is moving, the image of the light spot 7 reflected by the ground will have subtle differences. The high-resolution, fast imaging CCD photosensitive The device 3 can fully capture this difference. The DSP image processor 1 analyzes and compares the difference of each frame of images, and can calculate the pixel offsets VPi_x and VPi_y between frames in the X and Y directions respectively, according to The resolution F=800CPI of the CCD photosensitive device 3 can calculate the actual displacement deviation Vx and Vy of the positioning device:

$\left\{\begin{array}{c}\mathrm{<span><span>Vx</span>Vx</span>}<span><span>=</span>=</span>\mathrm{<span><span>2.54</span>2.54</span>}<span><span>\&divide;</span>\&divide;</span>\mathrm{<span><span>800</span>800</span>}<span><span>\&times;</span>\&times;</span>\mathrm{<span><span>VP</span>VP</span>}{\mathrm{<span><span>i</span>i</span>}}_{\mathrm{<span><span>x</span>x</span>}}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>cm</span>cm</span>}<span><span>)</span>)</span>\\ \mathrm{<span><span>Vy</span>Vy</span>}<span><span>=</span>=</span>\mathrm{<span><span>2.54</span>2.54</span>}<span><span>\&divide;</span>\&divide;</span>\mathrm{<span><span>800</span>800</span>}<span><span>\&times;</span>\&times;</span>\mathrm{<span><span>VP</span>VP</span>}{\mathrm{<span><span>i</span>i</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>cm</span>cm</span>}<span><span>)</span>)</span>\end{array}\right.$

主控制器根据几何关系可以确定出轮式机器人的准确航向和位置：The main controller can determine the exact heading and position of the wheeled robot according to the geometric relationship:

$\left\{\begin{array}{c}\mathrm{<span><span>\&theta;</span>\&theta;</span>}<span><span>=</span>=</span>\mathrm{<span><span>\&Sigma;</span>\&Sigma;</span>}\mathrm{<span><span>arctan</span>arctan</span>}\frac{\mathrm{<span><span>Vy</span>Vy</span>}}{\mathrm{<span><span>Vx</span>Vx</span>}}\\ \mathrm{<span><span>x</span>x</span>}<span><span>=</span>=</span>\mathrm{<span><span>\&Sigma;</span>\&Sigma;</span>}\sqrt{\mathrm{<span><span>V</span>V</span>}{\mathrm{<span><span>x</span>x</span>}}^{\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>\mathrm{<span><span>V</span>V</span>}{\mathrm{<span><span>y</span>the\; y</span>}}^{\mathrm{<span><span>2</span>2</span>}}}\mathrm{<span><span>cos</span>cos</span>}<span><span>(</span>(</span>\mathrm{<span><span>\&theta;</span>\&theta;</span>}<span><span>+</span>+</span>\mathrm{<span><span>arctan</span>arctan</span>}\frac{\mathrm{<span><span>Vy</span>Vy</span>}}{\mathrm{<span><span>Vx</span>Vx</span>}}<span><span>)</span>)</span>\\ \mathrm{<span><span>y</span>the\; y</span>}<span><span>=</span>=</span>\mathrm{<span><span>\&Sigma;</span>\&Sigma;</span>}\sqrt{\mathrm{<span><span>V</span>V</span>}{\mathrm{<span><span>x</span>x</span>}}^{\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>\mathrm{<span><span>V</span>V</span>}{\mathrm{<span><span>y</span>the\; y</span>}}^{\mathrm{<span><span>2</span>2</span>}}}\mathrm{<span><span>sin</span>sin</span>}<span><span>(</span>(</span>\mathrm{<span><span>\&theta;</span>\&theta;</span>}<span><span>+</span>+</span>\mathrm{<span><span>arctan</span>arctan</span>}\frac{\mathrm{<span><span>Vy</span>Vy</span>}}{\mathrm{<span><span>Vx</span>Vx</span>}}<span><span>)</span>)</span>\end{array}\right.$

其中，θ为机器人航向角度，x和y分别是机器人在平面坐标系中的横、纵坐标。快速的成像和高度的分辨率使得该定位装置的精度非常高，通过对轮式机器人进行测试，灵敏度在毫米级，定位误差在1％以内，完全满足机器人定位技术的要求。Among them, θ is the heading angle of the robot, and x and y are the horizontal and vertical coordinates of the robot in the plane coordinate system, respectively. Rapid imaging and high resolution make the positioning device very high in accuracy. Through the test of the wheeled robot, the sensitivity is at the millimeter level and the positioning error is within 1%, which fully meets the requirements of robot positioning technology.

如图3，基于激光影像技术的定位导航装置在定位导航过程中，采用封闭循环，不断上传的工作方式，装置上电后，系统中的传感器、控制器分别进行初始化，CCD感光器件3首先对光斑7进行快速成像，将每一帧图像送到DSP图像处理器1中进行分析计算，得到每帧图像的像素偏移量，主控制器2按照一定的频率不断读取偏移量信息，并根据几何关系解算出轮式机器人的位置和航向，然后通过串口输出端子5将位置和航向数据上传到机器人控制器，该定位导航装置周而复始的计算出机器人的实时位置和航向，为机器人导航提供基础。As shown in Figure 3, the positioning and navigation device based on laser imaging technology adopts a closed loop and continuous uploading working mode during the positioning and navigation process. After the device is powered on, the sensors and controllers in the system are initialized respectively, and the CCD photosensitive device 3 first The light spot 7 performs rapid imaging, and sends each frame of image to the DSP image processor 1 for analysis and calculation to obtain the pixel offset of each frame of image. The main controller 2 continuously reads the offset information according to a certain frequency, and Calculate the position and heading of the wheeled robot according to the geometric relationship, and then upload the position and heading data to the robot controller through the serial port output terminal 5. The positioning and navigation device repeatedly calculates the real-time position and heading of the robot, providing a basis for robot navigation .

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。Although the specific implementation of the present invention has been described above in conjunction with the accompanying drawings, it does not limit the protection scope of the present invention. Those skilled in the art should understand that on the basis of the technical solution of the present invention, those skilled in the art do not need to pay creative work Various modifications or variations that can be made are still within the protection scope of the present invention.

Claims (5)

1. a laser image positioning navigation device, it is characterized in that, it comprises control panel, on control panel, be provided with DSP image processor, DSP image processor is connected with CCD sensor devices and master controller respectively, master controller is connected with serial ports lead-out terminal, and the position of resolving and course information are exported by serial ports lead-out terminal; CCD sensor devices be circumferentially evenly distributed with multiple laser modules, the laser beam that each laser module sends pools a hot spot on ground, this hot spot and CCD sensor devices are on same axis and ensure that CCD sensor devices photosensitive effect is best, DSP image processor contrasts every frame light spot image analysis, calculate respectively the pixel-shift amount in X and Y-direction between frame and frame, calculate the actual displacement deviator of locating device according to the resolution of CCD sensor devices;

Being calculated as of the actual displacement deviator of described locating device:

Between light spot image frame and frame at the pixel-shift amount VPi of X and Y-direction_xand VPi_y, calculate actual displacement deviator Vx and the Vy of locating device according to the resolution F of CCD sensor devices:

$\left\{\begin{array}{c}\mathrm{Vx}=2.54\&divide;F\&times;{\mathrm{VPi}}_x......\left(\mathrm{cm}\right)\\ \mathrm{Vy}=2.54\&divide;F\&times;{\mathrm{VPi}}_y.....\left(\mathrm{cm}\right)\end{array}\right..$

2. a kind of laser image positioning navigation device as claimed in claim 1, is characterized in that, described laser module is laser tube, and laser tube focal length is adjustable.

3. a kind of laser image positioning navigation device as claimed in claim 1, is characterized in that, the shooting speed of described CCD sensor devices was 2000 frame/seconds.

4. a kind of laser image positioning navigation device as claimed in claim 1, is characterized in that, four jiaos of described control panel are provided with pilot hole.

5. a kind of laser image positioning navigation device as claimed in claim 1, is characterized in that, described master controller is determined accurate course and the position of wheeled robot according to geometric relationship:

$\left\{\begin{array}{c}\mathrm{\&theta;}=\mathrm{\&Sigma;}\arctan \frac{\mathrm{Vy}}{\mathrm{Vx}}\\ x=\mathrm{\&Sigma;}\sqrt{V{x}^2+V{y}^2}\cos (\mathrm{\&theta;}+\arctan \frac{\mathrm{Vy}}{\mathrm{Vx}})\\ y=\mathrm{\&Sigma;}\sqrt{V{x}^2+V{y}^2}\sin \begin{array}{}\end{array}(\mathrm{\&theta;}+\arctan \frac{\mathrm{Vy}}{\mathrm{Vx}})\end{array}\right.$

Wherein, θ is robot course heading, and x and y are respectively horizontal stroke, the ordinate of robot in plane coordinate system.