基于单目视觉的水下机器人悬停定位技术与实现

机器人２００６年１１月连线所形成的夹角，求各个控制点到光心的距离的问题．这就是著名的Ｐ３Ｐ（Ｐｅｒｓｐｅｃｔｉｖｅ一３一Ｐｏｉｎｔ）问题．其数学描述为：设控制点Ａ、Ｂ、Ｃ与光心０之间３条连线的长度分别记为菇、Ｙ、ｚ，已知它们之间的夹角分别为Ｏ／，／３、ｙ，及ＩＡＢ１＝ｃ，ＪＡｃ｛＝ｂ，ｌＢｃＩ＝ｎ，求菇、Ｙ、ｚ．根据余弦定理，可以得到如下的约束方程：ｒｚ２＋ｙ２—２并ｙｃ。

ｓａ２ｃ２｛戈２＋ｚ２—２ｘｚｃｏｓ／３＝ｂ２（１）【ｙｚ＋Ｚ２—２弘ｃ。

０：５２式（１）是一个互相耦合的非线性方程，其解法的时间复杂度很高，且存在多解和算法的鲁棒性差等问题，因此限制了其工程应用．我们针对实际应用时，３个控制点可以人为选取或布置这一先决条件，证明了控制点与摄像机间位置与Ｐ３Ｐ问题解的唯一性的关系．设３个控制点Ａ、Ｂ、ｃ为等腰三角形的顶点，在它们所构成的平面ＡＢＣ上，过Ａ点作直线ＡＫ垂直于ＡＣ，ＡＪ垂直于ＡＢ，过Ｂ点作直线ＢＰ垂直于ＡＢ，过ｃ点作直线ＣＱ垂直于ＡＣ，即与ＣＱ相交于Ｍ，射线ＡＫ、ＡＪ所夹的平面区域称为三，射线脚和＾卯所夹的平面区域称为口，则当光心或光心在平面。

．。

ｃ上的垂足落在区域三或区域口中时，Ｐ３Ｐ问题有唯一解．图１给出了其示意图，具体证明请见文『１３１．图１Ｐ３Ｐ问题解的唯一性示意图Ｆｉｇ．１ＴｈｅｓｋｅｔｃｈｍａｐｏｆｔｈｅｕｎｉｑｕｅｓｏｌｕｔｉｏｎｃｏｎｄｉｔｉｏｎｏｆＰ３Ｐｐｒｏｂｌｅｍ在解决了解的唯一性问题后，我们又得到了满足唯一性条件的快速求解方法¨４｜，从而使Ｐ３Ｐ问题的工程应用成为可能，并成功地应用到本文后面的视觉定位悬停演示实验中．２．２基于单应矩阵的运动估计当控制点间的位置关系（即模型）未知时，由于目标与摄像机间的位姿关系的．但当控制点位于同一平面上时，根据摄像机在两个不同位姿下采集的两帧图像（即两个不同视点的图像），仍旧可以估计出摄像机的运动趋势，再结合水下机器人的控制，完全可以实现水下机器人的悬停．假定空间中的一点Ｍ＝（ｘ，ｙ，ｚ）。

位于平面仃：（ｎ。

，ｎ，，ｎ。

）。

·Ｍ＝ｄ上，其中ｄ为光心到平面７ｒ的距离，ｃ。

、ｃ：分别为两视点的光心，Ｍ在两个像平面的成像分别为ｍ。

、ｍ：，两视点间的运动参数为露、ｒ，其中旋转矩阵Ｒ为一般性旋转，即像机绕ｘ、ｌ，、ｚ轴均有旋转，令Ｍ７＝ＲＭ＋Ｔ＝（ｘ７，】，’，ｚ’）。

，则有：日＝Ａ·（足＋吉砌１）（２）其中日为单应矩阵，Ａ＝Ｚ／Ｚ’为常数因子．式（２）给出了单应矩阵与两视点间相对位置和姿态之间的约束关系．只要已知平面上４个以上的控制点，就可以根据控制点在两视点图像上的图像坐标间的对应关系，求解出单应矩阵日¨５｜．再对获得的单应矩阵日按式（２）中等号右面的公式进行分解，以求解出运动参数曰和ｚ一般情况下，这一分解过程存在多解现象，最多有４个解，其中有２个满足空间约束．通常情况下的多解情形可以通过施加约束ｎ１’Ｍ＞０，即成像在摄像机的前方，来消去其中的一半，而对剩余两个解的二义性，则需要引入其它的约束．我们通过对水下机器人进行实际运动分析，利用水下机器人的运动约束，实现了对平面单应矩阵的正确分解，获得了两视点间的六维运动参数．其具体分解方法在文［１５］中有详细论述．由此可见，利用基于单应矩阵的运动估计方法，可以获得摄像机当前位姿与理想位姿之间的六维运动参数，并可表示为３个位置量和３个姿态角，只是此时的姿态角是绝对量，而位置量则仅是一个没有量纲的相对量，与实际的位置量相差一个比例因子．虽然无法得到两视点间精确的位置关系，这一无量纲的位置量仍旧可以反映两视点间相对平移运动的趋势，以此相对位置量和精确的姿态量作为水下机器人控制系统的输入，仍旧可以用视觉伺服的方法使水下机器人悬停在理想位姿上．３演示验证实验系统（ｒ１１ｌｅｄｅｍｏｎｓｔｒａｔｉｏｎｓｙｓ－ｔｅｍ）为验证上述视觉测量方法的有效性，我们在室内实验水池中，以自行研制的水下试验平台为实验第２８卷第６期郝颖明等：基于单目视觉的水下机器人悬停定位技术与实现６５９统．该水下试验平台具有开放式的框架结构，采用了基于总线的分布式控制系统，可实现进退、侧移、浮潜和回转４个自由度的运动．演示验证系统从空间结构上可划分为水面控制系统、水下自动驾驶系统、水面图像处理系统３个部分，图２给出了其空间结构图．水面控制系统由水面控制计算机系统、水面控制盒、直流电源、操作单杆和监视器等构成，是整个演示验证系统的信息汇合中心，所有设备集中放置在标准的１９英寸机柜中．水（ａ）水下实验载体下自动驾驶系统由水下主控计算机系统、相应的传感器以及分线盒构成，根据水面控制系统的控制指令，实现水下载体的运动控制．水面图像处理系统包括摄像机、观察目标、图像采集卡和图像处理计算机，完成水下图像的采集、处理以及载体位姿的计算．图像处理计算机与水面控制计算机之间通过串口相连，水下摄像机的视频信号通过双绞线送到水面图像处理计算机中，以图像采集卡完成模拟／数字信号的转换．图２演示验证实验系统结构图Ｆｉｇ．２Ｓｔｒｕｃｔｕｒｅｏｆｔｈｅｄｅｍｏｎｓｔｒａｔｉｏｎｓｙｓｔｅｍ（ｂ）视觉定位观察目标（ｃ）视觉伺服观察目标图３实验载体与观察目标Ｆｉｇ．３Ｔｈｅｕｎｄｅｒｗａｔｅｒｒｏｂｏｔａｎｄｔａｒｇｅｔ在演示验证实验中，我们将观察目标固定于实验水池的侧壁上，而将水下摄像机固定在实验载体的前面，即采用前视摄像机．针对两种不同的视觉悬停方法，设计了不同的观察目标，并分别进行了演示验证实验．在基于模型的单目视觉定位悬停演示验证实验中，将观察目标设计成由４个白色小球组成机器人２００６年１１月的正四面体，这样，无论观察目标处于怎样的姿态，都不会影响视觉测量的结果．在基于平面特征的视觉伺服悬停演示实验中，为保证４个控制点在同一平面上，利用黑色平板上随机分布的４个白色点目标作为观察目标．图３分别给出了实验载体（包括水下摄像机）和两种视觉悬停方法的观察目标的图像．４演示实验结果（Ｔｈｅｅｘｐｅｒｉｍｅｎｔｒｅｓｕｌｔ）利用上述演示验证实验系统，我们分别进行了两种视觉悬停定位方法的演示验证实验．在基于单目视觉的悬停定位演示实验中，采用２．１节的测量原理实现观察目标与实验载体间位姿关系的测量，作为控制系统闭环控制的输入．实验过程为：首先利用操纵杆控制水下载体的运动，以保证观察目标在摄像机的视场内，然后通过水面控制台输入要求的理想位姿，建立闭环控制，则水下载体自动运动到该理想位姿后保持悬停在该位姿上．此时，用外力推动载体使其偏离理想位姿，则机器人仍旧能够返回并悬停在理想位姿，当在载体前面或侧面旰｛水流冲击载体时，载体仍旧保持悬停，但当打开闭．滞０时，载体会随着水流的冲击而运动．针对该方法，我们分别进行了多次三自由度（两个水平方向的平移和一个水平方向的旋转）和四自由度（加入下潜和上浮）的演示验证实验，均取得了良好的实验效果．在基于平面特征的视觉伺服悬停演示验证实验中，采用２．２节的测量原理实现当前位姿与理想位姿间相对位置和相对姿态关系的测量，作为控制系统闭环控制的输入．由于无法得到观察目标与水下载体间精确的位姿关系，实验过程与前面的略有不同．首先利用操纵杆控制水下载体的运动，以保证观察目标在摄像机的视场内，在图像处理计算机的软件界面中，通过人机交互的方式确定水下载体的理想悬停位置，并构成闭环控制，此时载体保持悬停在理想位置．如果给载体一个位置扰动，让载体偏离理想悬停位置，则载体在闭环控制下仍旧能够返回并悬停在理想位置，当在载体前面或侧面用水流冲击载体时，载体保持悬停，但当打开闭环控制时，载体会随着水流的冲击而运动．为验证算法与控制点模型无关，多次随意改变了控制点的分布，均取得了很好的实验效果．图４给出了演示实验的图像，其中：图４（ａ）为水下定位实验加水流冲击的图像，图４（ｂ）为水下视觉伺服演示实验的图像处理界面，其中的白框表示控制点的理想位置，灰框表示控制点的当前位置，当两框重合时意味着完全回到理想位置．（ａ）定位实验水流冲击（ｂ）视觉伺服演示实验的图像处理界面图４演示实验结果Ｆｉｇ．４Ｔｈｅｒｅｓｕｈｓｏｆｄｅｍｏｎｓｔｒａｔｉｏｎｅｘｐｅｒｉｍｅｎｔｓ图５给出了某次视觉定位演示验证实验载体位置变化趋势图，从图中可以看出水下载体首先调整到（一３ｍ，０），稳定后再调整到（一２．５ｍ，一０．５ｍ），在该位置稳定后，加入位置扰动，载体出视场，经过闭环控制返回（一２．５ｍ，０．５ｍ），最后又调整到（一２．５ｍ，０．５ｍ）．基于单目视觉的水下机器人悬停定位技术与实现作者：郝颖明， 吴清潇， 周船， 李硕， 朱枫， HAO Ying-ming， WU Qing-xiao， ZHOU Chuan ， LI Shuo， ZHU Feng作者单位：郝颖明,吴清潇,HAO Ying-ming,WU Qing-xiao(中国科学院沈阳自动化研究所,辽宁,沈阳,110016;中国科学院研究生院,北京,100049)， 周船,李硕,朱枫,ZHOU Chuan,LI Shuo,ZHUFeng(中国科学院沈阳自动化研究所,辽宁,沈阳,110016)刊名：机器人英文刊名：ROBOT年，卷(期)：2006,28(6)被引用次数：4次1.Marks R L;Wang H H;Lee M J Automatic visual station keeping of an underwater robot[外文会议] 19942.Josko A C Performance limitations in underwater acoustic telemetry[外文期刊] 1990(03)3.Negahdaripour S;Shokrollahi A;Fox J Improved methods for undersea optical stationkeeping[外文会议] 19914.Jin L;Xu X;Negahdaripour S A real-time vision-based stationkeeping system for underwater robotics applications[外文会议] 19965.Negahdaripour S;Xu X;Khamene A3-D motion and depth estimation from sea-floor images for mosaic-based station-keeping and navigation of ROVs/AUVs and high-resolution sea-floor mapping[外文会议] 19986.Negahdaripour S;Xu X;Jin L Direct estimation of motion from sea floor images for automaticstation-keeping of submersible platform 1999(03)7.Korntey N L;Stephen M R;Stephen D F Station keeping of an ROV using vision technology[外文会议] 19978.Lots J F;Lane D M;Trucco E Application of 2 1/2 D visual servoing to tinderwater vehicle station-keeping[外文会议] 20009.Lots J F;Lane D M;Trucco E A 2-D visual servoing for underwater vehicle station keeping 200110.Cufi X;Garcia R;Ridao P An approach to vision-based station keeping for an unmanned underwater vehicle[外文会议] 200211.吴清潇;李硕;郝颖明基于模型的水下机器人视觉悬停定位技术[期刊论文]-高技术通讯 2005(10)12.Li S;Tang S H;Wu Q X Positioning of an underwater vehicle based on model-known vision system[外文会议] 200513.周鑫;朱枫关于P3P问题解的唯一性条件的几点讨论[期刊论文]-计算机学报 2003(12)14.郝颖明;朱枫;欧锦军目标位姿测量中的三维视觉方法[期刊论文]-中国图象图形学报 2002(12)15.周船基于单目视觉的高精度定位及运动估计研究 20051.李文涛.葛彤悬停式AUV控制策略研究[期刊论文]-海洋工程 2011(1)2.秦丽娟.朱枫一种新的基于直线的定位方法[期刊论文]-自动化学报 2008(2)3.郝颖明.朱枫.欧锦军.吴清潇.周静.付双飞基于点特征的位姿测量系统鲁棒性分析[期刊论文]-计算机应用2008(7)4.邢国强.闫雪.侯代文基于验前信息可信度的鱼雷实航工作可靠度 Bayes 融合评估方法[期刊论文]-鱼雷技术2012(2)本文链接：/Periodical_jqr200606019.aspx。