基于摄像头的激光测距仪

DIY基于摄像头的激光测距仪 前言本文是由RoboticFan网友Rockets翻译的一篇由国外机器人爱好者撰写的激光测距仪的文章。小编认为这篇文章具有相当的实用型和可操作性，发散一下思维能力，可以发现有很多应用。介绍有 很多现成的测距组件包括超声波、红外线、甚至是激光测距仪。这些设备运行的很好，但是对于飞行机器人来说，重量是一个主要考虑因素。一个可行的办法是增加 现有组件的功能，并安装在机身上。例如微型空中机器人的有效载荷是100g。它能利用USB连接的摄像头（或mini无线摄像头）执行视觉任务，例如避障 等。更好的是，如采用两个摄像头，能提供立体的机器视觉，这样能增强避障性能，因为双镜头提供了视觉深度。但缺点是需要增加另外一个摄像头的重量。这篇文 章就是讨论如何利用一个激光笔和一个摄像头来提供一个单镜头机器视觉和测距的。这个项目很大一部分是基于下面这个教程的/ubr/ff03laser.php工作原理下图显示了如何将激光点投射到目标物上，并在摄像头上显示。摄像头和激光点的距离是可以通过计算而得出的。公式很简单，因此这个技术在需要很快运行的机器视觉应用上是适合的。介绍一下工作原理。一束激光被投射到目标物上，并在摄像头上被显示。激光束被认为是理想的平行于摄像头的中心光轴。激光点由摄像头的其余部分所捕获。一个 简单的计算就是寻找最亮点。如果设激光点就是这个场景的最亮点（似乎在室内我的激光发射器确实是最亮的），那么这个点的位置在图帧中的位置是确定的。然后 我们只需要计算这个点在沿着y轴的距离，就能计算出目标物离摄像头的距离，激光点距离中心越近，离目标物越远。如同公式所示，距离D是可以被计算出来的。为了计算这个等式，我们需要知道激光器和摄像头之间的距离h，这是个常数，还有角度，角度可以计算。其中：pfc从焦平面到中心的像素数量rpc单个像素的弧度ro弧度补偿（弥补对齐错误）代入上式，我们得到：这样，从图像中就能将焦平面到激光点像素数计算出来。那其他的常数怎么办呢？我们需要执行一个校准来得到这些数据。为了校准这个系统，我们需要收集一系列测量的数据，每次测得的目标物的距离和这个激光点离中心点的像素数。数据如下 使用下面的公式，我们能够利用激光器和摄像头之间的距离h和真实距离计算出真实的角度：actual真实角度Dactual真实距离（测量得出）现在我们有了公式中的每个数值，我们可以利用一个关系式来计算点离中点的像素数。我用了一个线性关系式。这个公式看起来很有用， 从我的校正数据中，我计算出: Offset (ro) = -0.056514344 radians Gain (rpc) = 0.0024259348 radians/pixel 使用: 下表是列举了根据上面ro和rpc值计算出的距离值，实际距离值和误差： RoboticFan所需零部件我的测距仪没有多少部件。我使用一块硬纸板来固定激光发射器和摄像头。摄像头和激光发射器被平行的布置在一起。我组装的测距仪是这样的软件我通过两个方式编写了这个软件，一个是vc+，一个是VB。你能发现VB版本的软件会比VC的软件更容易一些，但是各有取舍。VC+版本能够自由的加入其他软件中？VB版本需要第三方软件支持（在Visual Studio中）Visual Basicvb_laser_ranger.zip这里可以下载到我的VB版本软件。要使用上面的程序，你必须要安装VideoOCX ActiveX 控件主程序如下：Private Sub exit_Click() only if running.If (Timer1.Enabled) ThenTimer1.Enabled = False Stop TimerVideoOCX.StopVideoOCX.CloseEnd IfEndEnd SubPrivate Sub Start_Click() Init VideoOCX Control, allocate memory and start grabbingIf (Not Timer1.Enabled) ThenStart.Caption = Stop Disable internal error messages in VideoOCXVideoOCX.SetErrorMessages False Init controlIf (Not VideoOCX.Init) Then Init failed. Display error message and end subMsgBox VideoOCX.GetLastErrorString, vbOKOnly, VideoOCX ErrorEndElse Allocate memory for global image handlecapture_image = VideoOCX.GetColorImageHandle result_image = VideoOCX_Processed.GetColorImageHandleTimer1.Enabled = True Start capture timer Start Capture modeIf (Not VideoOCX.Start) Then Start failed. Display error message and end subMsgBox VideoOCX.GetLastErrorString, vbOKOnly, VideoOCX ErrorEndEnd IfEnd IfElseStart.Caption = StartTimer1.Enabled = False Stop TimerVideoOCX.StopVideoOCX.CloseEnd IfEnd SubPrivate Sub Timer1_Timer() Timer for capturing - handles videoOCXToolsDim matrix As VariantDim height, width As IntegerDim r, c As IntegerDim max_r, max_c As IntegerDim max_red As IntegerDim gain, offset As VariantDim h_cm As VariantDim range As IntegerDim pixels_from_center As Integer Calibrated parameter for pixel to distance conversiongain = 0.0024259348offset = -0.056514344h_cm = 5.842max_red = 0 Capture an imageIf (VideoOCX.Capture(capture_image) Then VideoOCX.Show capture_image Matrix transformation initializationmatrix = VideoOCX.GetMatrix(capture_image)height = VideoOCX.GetHeightwidth = VideoOCX.GetWidth Image processing code The laser dot should not be seen above the middle row (with a little pad)For r = height / 2 - 20 To height - 1 Our physical setup is roughly calibrated to make the laser dot in the middle columns.dont bother lookng too far awayFor c = width / 2 - 25 To width / 2 + 24 Look for the largest red pixel value in the scene (red laser)If (matrix(c, r, 2) max_red) Thenmax_red = matrix(c, r, 2)max_r = rmax_c = cEnd IfNext cNext r Calculate the distance for the laser dot from middle of framepixels_from_center = max_r - 120 Calculate range in cm based on calibrated parametersrange = h_cm / Tan(pixels_from_center * gain + offset) Print laser dot position row and column to screenrow_val.Caption = max_rcol_val.Caption = max_c Print range to laser illuminated object to screenrange_val.Caption = range Draw a red vertical line to intersect targetFor r = 0 To height - 1matrix(max_c, r, 2) = 255Next r Draw a red horizontal line to intersect targetFor c = 0 To width - 1matrix(c, max_r, 2) = 255Next cVideoOCX.ReleaseMatrixToImageHandle (capture_image)RoboticFanEnd IfVideoOCX.Show capture_imageEnd Sub截屏如下:Visual C+我的代码是基于Paul Oh教授的教程。你需要注意，当跟进这个教程的时候，一些必要的文件也许不再正常连接或丢失，他们可以在下面的位置下载。qcsdk.exeqc543enu.exe根据 TRIPOD 的教程的说明，可以在其源程序中插入一段用户自己的图像处理代码，在这里，我插入了下面的代码: void CTripodDlg:doMyImageProcessing(LPBITMAPINFOHEADER lpThisBitmapInfoHeader)/ doMyImageProcessing: This is where youd write your own image processing code/ Task: Read a pixels grayscale value and process accordinglyunsigned int W, H; / Width and Height of current frame pixelsunsigned int row, col; / Pixels row and col positionsunsigned long i; / Dummy variable for row-column vectorunsigned int max_row; / Row of the brightest pixelunsigned int max_col; / Column of the brightest pixelBYTE max_val = 0; / Value of the brightest pixel/ Values used for calculating range from captured image data/ these values are only for a specific camera and laser setupconst double gain = 0.0024259348; / Gain Constant used for converting/ pixel offset to angle in radiansconst double offset = -0.056514344; / Offset Constantconst double h_cm = 5.842; / Distance between center of camera and laserdouble range; / Calculated range unsigned int pixels_from_center; / Brightest pixel location from center/ not bottom of framechar str80; / To print messageCDC *pDC; / Device context need to print messageRoboticFanW = lpThisBitmapInfoHeader-biWidth; / biWidth: number of columnsH = lpThisBitmapInfoHeader-biHeight; / biHeight: number of rowsfor (row = 0; row H; row+) for (col = 0; col = max_val) max_val = *(m_destinationBmp + i);max_row = row;max_col = col;/ After each frame, reset max pixel value to zeromax_val = 0;for (row = 0; row H; row+) for (col = 0; col TextOut(75, 580, str);ReleaseDC(pDC);完整的代码可以在下面下载到：LaserRange.zip可执行文件可在下面下载到：LaserRange.exe注意，为了执行这个文件，你可能需要qcsdk和qc543这两个驱动文件。下面是摄像头激光测距仪的工作截图，注意它是如何工作的。在第二个例子中，有两个激光点，其中的一个是激光点在摄像头里面的反射，这个反射点由于没有那么强烈的，所以不适用于运算法则。将来的工作一个重要的改进就是将点改为线，这样可以计算每个光柱的距离而不是单个的光柱。这样的设置可以使车辆能够探测最大的前进距离，同样的，障碍物的最小距离也可以被探测到。相关链接：此文的英文原文可以在emtionmk的blog找到：英文原文/twd25/webcam_laser_ranger.htmlDetails of the Laser Range finderSo, i would guess that at least 20 individuals asked me how to make a laser range finder at the 2003 fire fighting contest. By Sunday i was getting pretty tired of answering the same question so i decided to make this web site. Since in my college it seems that every single project is done in groups i have become used to sharing just about everything i come up with, so you are in luck. I hope that whoever reads this overview will be inspired to make their own system for a fire fighting robot to advance the contest.PartsThe following are the parts that i used to make the ranger. Substitutions are always possible. LIS-1024 Linear image sensor by Photon Vision Systems Green Laser Diode Module A lens assembly from miniature camera TLC0820 parallel 8 bit flash Analog to Digital converter 4047 oscillator, 74HCT00 quad NAND gate. other little stuff. The basic principle of this ranger is to measure distance through triangulation. A laser is used to illuminate the object you would like to measure to and a linear image sensor or other sensor is used to measure the angle to the spot. The laser and image sensor are mounted at a known distance apart pointed in the same direction.Because the average hobbyist doesnt have access to a wide variety of lenses, lasers or image sensors, many calculations are irrelevant because you couldnt get parts to fit what you calculate. The parts that i used were what i could get but some things should still be kept in mind if a choice exists.The more resolution your image sensor has the better the accuracy. The image sensor i have has 1024 pixels and provides decent resolution and accuracy. If you were to use something easy like a cheap camera that may have only 300 pixels in a line your resolution will be severely effected. More is better but be careful of the price. I am not very much into optics and i pretty much experimented with the lens to get it to work. I know it has to be about as wide as the active area of your sensor and with a focal length of about 1cm. Other than that you can just mess with the sensors position under the lens and the focus until you get something that works.The microcontoller i used to drive the sensor had internal analog to digital converters but i chose to use an external IC. It seemed that the fastest i could configure the integrated ADC to run, it would take about 75 microseconds per sample, at low resolution. The TI part can make a conversion in about 5 microseconds allowing for at least 100 complete distance measurements per second.I chose a green laser for 3 reasons. The trinity gym is very red, leaving the possibility open to put a filter on the sensor. Its really bright so the ranger can see pretty far without strange amplification. Its really cool. You could probably use cheaper red lasers just fine. You may have to amplify the video signal though if not enough light is reflected. In fact i would probably try amplifying the signal instead of using a green laser again, it is way too bright to use regularly.ElectronicsDownload the schematicsThe video signal with a short range object. Top is video, bottom is sync Interfacing to the image sensor is specific to the actual sensor you have. I will discuss the PVS sensor although others will be similar. This sensor has several inputs, some for clocks and some to configure the operating mode of the device. I hard wired the mode to be DPR Mode, previous pixel reset which basically means that each pixel integrates for one frame, is read and reset to integrate again. Because the clock speed applied to the sensors input is important for obtaining valid data, i could not have the clock start and stop whenever convenient, rather it had to be free running. I used a 4047 multivibrator for the job, set to 100 - 130 khz. Its speed is not horribly important which is good because it is not a horribly accurate device. Depending on the temperature of the capacitor in its RC circuit the frequency varies by around 5 khz. This speed is limited to about 130 khz because of the speed of the ADC which usually takes 5 microseconds to make a conversion, but could take 8 or 10 at times. The way i see it, 100 readings per second is much faster than most other sensors available to hobbyists and is much more data then the robots software can deal with anyway.Sampling the signal 70% through. Top is pixel clock, bottom is ADC clock The output of the sensor is an analog voltage relative to the intensity of the light striking a given pixel. Each time the clock input is cycled, a new voltage is outputted representing the next pixel. The SYNC line is asserted to signal the first pixel. The data sheet recommends to sample the output at least 60% through the clock cycle to avoid sampling noise caused by the next pixel being selected. I used two NAND gates and both outputs of the oscillator to create some kind of combinational logic to sample about 70% through. That signal is fed to the analog to digital converter, which will then start the conversion and assert its INT line when done, which is usually within 5 microseconds.At the start of each frame the sensor asserts its SYNC line for 1 clock cycle. When the microcontoller wants to acquire a new measurement it will wait for this signal. Once asserted it will watch for the ADC to signal a conversion is complete. Once complete the microcontroller can load in the data through the parallel connection. The microcontroller keeps track of the first pixel over a certain threshold and the last pixel over that threshold to be used in calculations later.A closeup of the video signal. Notice the steps which are the individual pixels.Once the edges of the peak are found we can begin calculating the real distance. From the edges the center of the point is calculated. I just picked the point in the middle of the edges but could have played around with a weighted average.There are a few things you have to know to convert a pixel to a distance. Firstly, you need to know the base side of your right triangle. Measure from the center of the laser to the center of your image sensors lens. Next you need to know how many pixels are spanned per degree of incoming light. To find this you can play with some right triangles. Measure the distance to an object and do the trig to find the angle at the sensor. Move the object and do it again. Take the difference in raw readings and divide by the difference in degrees.You also need to know the smallest raw reading you can get, representing the closest distance you can measure, and the angle at that distance. Just play with holding things up to the sensor until it doesnt see it and measure that distance and work out the angle.Basically distance = base * tan(angle)to convert the raw reading to angle i used: angle = (raw reading - smallest raw reading) / pixels per degree) + smallest degreeSimple as that. download this spreadsheet to try your results. I tweaked the constants with this sheet to minimize overall error. Pixels per degree is the most important number and only needs to be changed by a few thousands to effect accuracy.TroubleshootingAlot of things can cause you sensor to not work or not work well. Frankly, i dont remember most of the things i had to troubleshot through because that was back in November at the beginning of the very long construction of this robot. But i can tell you to watch out for noise. I didnt implement the low pass filter like the PVS data sheet suggested because i was not too worried about the accuracy of the pixel data, as long as it was close. But a few percent of the readings are incorrect and detecting false peaks are probably to blame. Also, make sure the system is optically as accurate as possible. If the laser does not point perpendicular to the base of your sensor, error for distances will vary wildly depending on range. Also make sure that the l