[發明專利]一種目標跟隨機器人及跟隨方法

在審

• 申請號: 202010005542.3
• 申請日: 2020-01-03
• 公開（公告）號: CN111198496A
• 公開（公告）日: 2020-05-26
• 發明（設計）人: 王滔;祝義朋;朱世強;徐志偉
• 申請（專利權）人: 浙江大學
• 主分類號: G05B11/42
• 分類號: G05B11/42;G01S13/86
• 代理公司: 杭州浙科專利事務所(普通合伙) 33213
• 代理人: 孫孟輝

1.一種目標跟隨方法，其特征在于，包括如下步驟：

步驟一，識別圖像中的目標；

利用神經網絡完成目標識別，將圖像識別結果輸出；所述圖像識別結果包括像素坐標、類別信息；

步驟二，融合圖像識別結果與雷達點云信息，得到所述目標的雷達點云信息；

(1)轉換雷達點云坐標至像素點坐標；

雷達坐標系中點云坐標(x,y,z)與圖像坐標系下對應像素點坐標(u,v)之間的轉換關系表示如下：

相機的內參矩陣相機與雷達間的外參是已知參數，所述Z_c是所述像素點(u,v)所在成像平面與所述雷達坐標系中點云坐標(x,y,z)與之間的垂直距離，所述R是外參矩陣中的旋轉部分，所述T是外參矩陣中的平移部分，所述f是相機焦距，所述(u₀,v₀)表示圖像坐標系原點在像素坐標系中的坐標，所述dx、所述dy分別是每個像素在圖像平面x和y方向上的物理尺寸；

(2)匹配雷達點云與圖像識別結果；

根據所述雷達點云坐標對應的所述像素點坐標(u,v)，匹配所述圖像識別結果，為每個雷達點云賦予其對應的所述目標的類別信息，得到新的雷達點云信息，所述新的雷達點云信息包括位置信息(x,y,z)、類別信息；

步驟三，對抗復雜環境下多個同類別目標干擾的處理；

(1)分析所述目標附近識別結果；

設置搜索范圍閾值，在所述閾值內搜索目標的雷達點云，并記錄多個與所述目標類別相同的雷達點；

(2)利用雷達速度維信息區分相同類別的雷達點，得到所述目標位置；

雷達可以得到所述雷達點云速度信息，對所述目標雷達點云的速度信息進行歷史記錄，以最近時刻的速度為基準，尋找所述類別相同的雷達點中速度最接近的雷達點，判斷出下一時刻的目標位置。

2.根據權利要求1所述的一種目標跟隨方法，其特征在于，所述速度信息是徑向速度。

3.根據權利要求1所述的一種目標跟隨方法，其特征在于，還包括步驟四，PID算法得到跟隨控制指令；

(1)在前進速度方向上采用PID算法計算速度控制指令；

采用PID算法對機器人前進速度進行控制，所述目標位置坐標(x,y,z)與所述機器人跟隨的前進速度V_t的轉換關系，表述如下：

所述t是當前時刻，所述d_t是所述t時刻下所述機器人與所述目標之間的距離，所述d_i是i時刻下所述機器人與所述目標之間的距離，所述d_O是所述機器人跟隨時期望目標保持的距離，所述K_p、K_i、K_d是控制參數，所述z_t和x_t分別是t時刻下所述目標的z坐標與x坐標；速度指令被傳遞給所述機器人底盤，控制所述機器人對目標跟隨的距離；

(2)在水平旋轉方向上采用PID算法計算角速度控制指令；

采用PID算法對所述機器人旋轉角速度進行控制，所述目標位置坐標(x,y,z)與所述機器人跟隨的旋轉角速度ω_t的轉換關系，表述如下：

所述t是當前時刻，所述θ_t是t時刻下所述機器人前進方向與所述目標之間的水平夾角，θ_i是i時刻下所述機器人前進方向與所述目標之間的水平夾角，所述K_p、K_i、K_d是控制參數，所述z_t和x_t分別是t時刻下所述目標的z坐標與x坐標；角速度指令被傳遞給機器人底盤，控制機器人在角度上完成目標跟隨；

速度和角速度指令被機器人底盤同步執行。

4.根據權利要求1所述的一種目標跟隨方法，其特征在于，所述雷達是毫米波雷達。

5.根據權利要求1所述的一種目標跟隨方法，其特征在于，所述圖像是通過單目相機獲取的圖像。