[發明專利]一種為第三方提供空間定位信息的自跟隨機器人裝置及方法

有效

• 申請號: 201710149250.5
• 申請日: 2017-03-14
• 公開（公告）號: CN106933096B
• 公開（公告）日: 2020-12-18
• 發明（設計）人: 周言明;黃昌正;陳曦
• 申請（專利權）人: 廣州幻境科技有限公司
• 主分類號: G05B11/42
• 分類號: G05B11/42;G05D1/12
• 代理公司: 廣州市紅荔專利代理有限公司 44214
• 代理人: 吝秀梅;李彥孚

1.一種為第三方提供空間定位信息的自跟隨機器人裝置，包括視覺單元、云臺、控制裝置和行走輪，其特征在于：視覺單元安裝在云臺上，云臺安裝在控制裝置的上部，4個行走輪安裝在控制裝置的兩側，控制裝置包括運動機構、主板、運動距離測量機構、通信單元、第三方系統和定制光學標記，運動機構、運動距離測量機構和通信單元分別連接主板，第三方系統連接通信單元，定制光學標記連接第三方系統；

所述的自跟隨機器人裝置為第三方提供空間定位信息的方法具體包括以下步驟：

步驟S1)、基于雙目紅外視覺單元的單光點標識定位計算

在標定好的雙目視覺系統的相機鏡片上貼上紅外濾光片，過濾除標識點外的光點信息，通過雙目視覺系統拍攝的差異圖片，對匹配的光點標識進行三維空間的定位；其計算如下：

設兩個鏡頭中心點的中點作為雙目系統的坐標原點，中心點距離為b；光軸方向為z軸正方向，左相機鏡頭中點到右相機鏡頭中點的方向為x軸正方向；

設相機待測的光點標識三維坐標為(x,y,z)，設左右攝像機拍攝到的點在成像平面的坐標分別為(x_l,y_l)、(x_r,y_r)，兩個攝像機的焦距為f，根據相似三角形的性質可以知道：

求解得到：

步驟S2)、多標記點的姿態計算

設在立體視覺中被測量四個點A、B、C、D的齊次化坐標為：P'_D＝(x₁,y₁,z₁,1)，P'_C＝(x₂,y₂,z₂,1)，P'_B＝(x₃,y₃,z₃,1)，P'_A＝(x₄,y₄,z₄,1)；

構建P_D＝(0,0,0,1)，P_C＝(|DC|,0,0,1)，P_B＝(0,|DB|,0,1)，P_A＝(0,0,|DA|,1)；

設所求的旋轉信息基于機器人坐標系的旋轉矩陣為R₂，偏移矩陣為t₂；根據下式：

P＝[R₂|t₂]_3x4P'；

根據旋轉關系，將P和P'帶入上式，等式右邊代入測量點坐標，等式左邊代入對應的構建點的坐標，求解12條線性方程組，即可求解基于機器人坐標系的4個點的旋轉矩陣和偏移矩陣的12個參數[R₂|t₂]_3x4；

步驟S3)、軌道設計

根據具體場地，設定有向的連續軌道，軌道的高度是恒定的，以機器人的起始位置為世界坐標系的原點；

機器人只能在設定的軌道上作沿著軌道向前或沿著軌道向后運動，其視覺平面始終垂直軌道的切線；

機器人在軌道上的實時運動中，由于軌道是預設的，根據機器人在軌道的三維坐標在世界坐標系下的偏移矩陣t₁，和機器人和軌道切線的關系，可以知道其在世界坐標系下旋轉矩陣R₁；

步驟S4)、移動跟隨

設定機器人需要保持的相對距離r(t)；機器人實時測量與人的相對距離c(t)；通過PID控制器，計算差值e(t)＝r(t)-c(t)和調整量

根據PID控制器的調整量，在預設定的軌跡上選擇向前或者向后運動，在實時測量的距離c(t)和設定距離r(t)的差值穩定在可接受范圍內停止機器人的運動；

步驟S5)、坐標系變換

根據步驟S2)得出的目標在機器人坐標系的旋轉矩陣R₂、偏移矩陣t₂和步驟S3)中得到的機器人在世界坐標下的旋轉矩陣R₁，偏移矩陣t₁，計算目標在世界坐標系的旋轉矩陣R和偏移矩陣t，即可確定目標在大型場景中的位置和姿態信息；即

R＝R₁R₂；