[发明专利]一种基于矩形标识的位姿估计方法、装置及机器人

权利要求书

1.一种基于矩形标识的位姿估计方法，其特征在于，所述方法包括：

标定摄像头的内参数和畸变参数；

获取摄像头拍摄到的图像，识别出矩形标识；

获取矩形标识的四个角点的图像坐标，根据需求建立世界坐标系，并通过测量获取矩形标识的四个角点在世界坐标系中的坐标；

通过摄像头的内参数和畸变参数，对矩形标识的四个角点的图像坐标进行畸变矫正，输出畸变校正之后的四个角点；

将畸变校正之后的四个角点中随机抽取三个角点作为P3P算法的求解点，另外一个角点作为校验点，产生四组点，对于TS4的情况，每组点得出两组解，通过剩余的点进行验证，从而得到8组解；

通过各自组点内剩余的点进行重投影误差的计算，选取重投影误差最小的点作为最终估计的位姿。

2.如权利要求1所述的方法，其特征在于，所述获取摄像头拍摄到的图像，识别出矩形标识具体包括：

获取摄像头拍摄到的图像；

将图像转换为灰度图；

采用自适应二值化操作和采用轮廓查找操作，找到图像中所有的矩形轮廓或者近似矩形的轮廓作为候选区域；

对候选区域进行透视变换，将其视角变换为正视图；

根据正视图识别出矩形标识。

3.如权利要求2所述的方法，其特征在于，所述找到图像中所有的矩形轮廓或者近似矩形的轮廓作为候选区域之后，所述方法还包括：

对候选区域进行滤波。

4.如权利要求2所述的方法，其特征在于，所述对候选区域进行透视变换，将其视角变换为正视图之后，所述方法还包括：采用大津法对正视图进行二值化处理。

5.如权利要求1所述的方法，其特征在于，针对每组点，所述对于TS4的情况，每组点得出两组解具体包括以下步骤：

S1051、利用向量的点乘公式和相似角的原理求解α角、β角和γ角，其中，α角是线段PC和PB之间的夹角，β角是线段PA和PC之间的夹角，γ角是线段PA和PB之间的夹角，P为摄像头的光心，A、B、C分别为世界坐标系中的三个点，AB的长度是c’，BC的长度是a’，AC的长度是b’；

S1052、已知α、β、γ角，利用余弦定理列出以下等式：

Y²+Z²-2YZcosα＝a'²

X²+Z²-2XZcosβ＝b'²

X²+Y²-2XYcosγ＝c'²

其中，X是PA的长度，Y是PB的长度，Z是PC的长度；

S1053、通过变量替换：

X＝xZ；Y＝yZ；c’²＝vZ²；

p＝2cosα、q＝2cosβ、r＝2cosγ

a’²＝ac’²＝avZ²

b’²＝bc’²＝bvZ²

保证P、A、B、C四点不共线，即p²+q²+r²-pqr-1≠0；由变量替换，可知：

y²Z²+Z²-yZ²p＝avZ²

x²Z²+Z²-xZ²q＝bvZ²

x²Z²+y²Z²-xyZ²r＝vZ²

上式两边同时除以Z²，可得：

y²+1-yp-av＝0

x²+1-xq-bv＝0

x²+y²-xyr-v＝0

由上式可得v＝x²+y²-xyr，将其带入前面两式，可得：

(1-a)y²-ax²+axyr-yp+1＝0

(1-b)x²-by²+bxyr-xq+1＝0

S1054、将上式按照零值分解TS4的情况进行分解，得到x和y的两组解：

(p²b+q²b-p²)x²+(-4bq+p²q)x+4b-p²＝0

py+qx-2＝0

a+b-1＝0

S1055、由v＝x²+y²-xyr，解出v，再由下式分别解出PA、PB、PC的长度X、Y和Z：

X＝xZ

Y＝yZ

S1056、根据PA、PB、PC的长度X、Y和Z，由相似三角形原理求得P点在摄像头坐标系下的坐标(X，Y，Z)：

其中，f_x、f_y是摄像头的焦距；

S1057、已知P点在世界坐标系下的坐标(X_w，Y_w，Z_w)，根据平面的单应性求出R和T矩阵。