[發明專利]一種基于ROS2的安全導航方法

在審

• 申請號: 202211267375.5
• 申請日: 2022-10-17
• 公開（公告）號: CN115792942A
• 公開（公告）日: 2023-03-14
• 發明（設計）人: 柯真東;徐晴川;吳雨璁;余嘉杰;遲文政;王松;柏超月
• 申請（專利權）人: 安徽樂聚人工智能應用技術服務有限公司;蘇州大學
• 主分類號: G01S17/86
• 分類號: G01S17/86;G01C21/20
• 代理公司: 蘇州市中南偉業知識產權代理事務所(普通合伙) 32257
• 代理人: 王廣浩

1.一種基于ROS2的安全導航方法，其特征在于，包括以下步驟：

S1、將機器人置于未知室內環境中；

S2、控制機器人在未知室內環境中運動，并建立柵格地圖；

S3、將機器人置于已知室內環境中，加載所述柵格地圖，并定位機器人在已知室內環境中的位置；

S4、根據所述柵格地圖信息規劃出機器人當前位置到目標點的全局路徑；

S5、通過深度相機獲取深度圖像數據，并將深度圖像數據轉換為激光雷達數據；

S6、將所述激光雷達獲取的激光雷達數據和所述深度圖像數據轉換的激光雷達數據融合，利用融合后的激光雷達數據構造局部代價地圖，并通過動態窗口法在所述全局路徑上規劃出局部最優路徑；

S7、利用超聲波傳感器獲取的超聲波傳感器數據和激光雷達獲取的激光雷達數據計算得到最近障礙物離機器人的距離，并根據距離調節機器人移動速度；

S8、利用所述全局路徑、局部最優路徑和移動速度進行機器人導航。

2.如權利要求1所述的基于ROS2的安全導航方法，其特征在于，步驟S2包括：

S21、通過手柄控制機器人在未知室內環境中運動，通過建圖功能包和機器人攜帶的激光雷達為未知室內環境建立柵格地圖。

3.如權利要求1所述的基于ROS2的安全導航方法，其特征在于，步驟S2還包括：在所述柵格地圖上設置虛擬墻；步驟包括：

S22、設置障礙物起始點和終點，得到起始點坐標(x₁，y₁)和終點坐標(x₂，y₂)；

S23、定義參數m＝x₂-x₁，由以下公式計算直線方程：

b＝y₁-k*x₁ (2)

y＝k*x+b (3)；

S24、判斷x₁與x₂的大小；若x₁x₂，則令x＝x₁+1，將x代入直線方程，求解得到y，將得到的坐標(x,y)存入點集；若x仍然小于x₂，則再令x＝x+1，并求解得到新的坐標(x,y)存入點集，由此反復迭代，直至所得到的xx₂；若x₁x₂，則令x＝x₂+1，將x代入直線方程，求解得到y，將得到的坐標(x,y)存入點集；若x仍然小于x₁，則再令x＝x+1，并求解得到新的坐標(x,y)存入點集，由此反復迭代，直至所得到的xx₁；最終得到直線方程上一組密集點集，并將點集中的點體積膨脹，進而得到一堵虛擬墻。

4.如權利要求1所述的基于ROS2的安全導航方法，其特征在于，步驟S5包括：

S51、通過深度相機獲取深度圖像數據；

S52、給定一個深度圖像點，將給定點的坐標轉換到深度相機坐標系下；

S53、將給定深度圖像點的深度數據根據如下公式映射到激光數據槽中：

其中，laser[n]為給定深度圖像點轉換的雷達激光數據；z和x分別為該點投影到與深度相機同一平面后距相機光心的z軸與x軸上的距離；

S54、按從左到右的順序橫向掃描同一高度的深度圖像點，并循環步驟S52和S53直到該高度下的深度圖像點已經全部掃描完畢，得到雷達數據dep_range序列；

S55、設置縱向掃描的高度范圍；

S56、按從上到下的順序在高度范圍內循環步驟S54直到范圍內的全部深度圖像點掃描完畢，每一次執行步驟S54得到的新的dep_range序列中的值會與上一次循環得到的dep_range序列中的值作比較，取兩者之間的較小值生成一個新的dep_range序列；

S57、執行完步驟S56，得到最終的dep_range序列作為深度圖像數據轉換的激光雷達數據。