[發明專利]運動裝置的軌跡數據處理方法及其處理系統

有效

• 申請號: 201910615659.0
• 申請日: 2019-07-09
• 公開（公告）號: CN110345936B
• 公開（公告）日: 2021-02-09
• 發明（設計）人: 蔡龍生;趙明
• 申請（專利權）人: 上海有個機器人有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C21/20
• 代理公司: 上海光華專利事務所(普通合伙) 31219
• 代理人: 王華英

1.一種運動裝置的軌跡數據處理方法，其特征在于，包括：

獲取運動裝置外部環境輪廓的相繼兩幀點云，以及所述兩幀點云之間編碼器的相對平移量和慣性測量單元的角度改變量，其中，分別將所述兩幀點云標記為參考幀點云和當前幀點云；

通過所述相對平移量和所述角度改變量構建初始變換矩陣；

利用所述初始變換矩陣對所述當前幀點云進行數據變換，以獲取過渡幀點云；

利用所述參考幀點云和所述過渡幀點云得到對應點對集合；

利用所述對應點對集合得到所述運動裝置的軌跡數據；

其中，所述利用所述參考幀點云和所述過渡幀點云得到對應點對集合的步驟包括：

以獲取所述參考幀點云時所述運動裝置的朝向為參考視線，對于所述過渡幀點云中的每一個點，在所述參考視線附近區域搜索所述參考幀點云中與該點對應的點列；其中，以獲取所述參考幀點云時所述運動裝置的朝向為參考視線進行對應點搜索時，需要設置點數范圍以及預設距離閾值，將獲取所述當前幀點云時所述運動裝置的朝向與所述參考視線之間夾角定義為視線夾角，將激光掃描裝置兩根激光線束之間夾角定義為線束夾角，所述點數范圍根據所述視線夾角和所述線束夾角來確定；所述預設距離閾值是根據兩幀點云之間所述運動裝置的運行距離進行設置；

計算每一個點與所述參考幀點云中與該點對應的點列之間的距離，選取距離最小且滿足預設距離閾值的兩點建立對應點對；

遍歷所述過渡幀點云中的所有點，以建立所述對應點對集合。

2.根據權利要求1所述的運動裝置的軌跡數據處理方法，其特征在于，所述利用所述對應點對集合得到所述運動裝置的軌跡數據的步驟包括：

建立關于所述對應點對集合中所有對應點對誤差度量的目標函數；

優化求解所述目標函數，以獲得目標變量增量；

根據所述目標變量增量更新所述初始變換矩陣，以獲取所述運動裝置的軌跡數據。

3.根據權利要求2所述的運動裝置的軌跡數據處理方法，其特征在于，所述優化求解所述目標函數，以獲得目標變量增量的步驟包括：

關于目標變量線性優化所述目標函數，以得到關于所述目標變量的方程組；

求解關于所述目標變量的方程組，以獲得目標變量增量。

4.根據權利要求2所述的運動裝置的軌跡數據處理方法，其特征在于，所述軌跡數據處理方法還包括：

判斷所述軌跡數據是否滿足終止條件的步驟；

若“否”，則將獲取的所述軌跡數據作為初始變換矩陣，返回所述利用所述初始變換矩陣對所述當前幀點云進行數據變換，以獲取過渡幀點云的步驟進行迭代，直到所述軌跡數據滿足終止條件，將滿足所述終止條件時的所述軌跡數據作為最終數據并輸出。

5.根據權利要求4所述的運動裝置的軌跡數據處理方法，其特征在于，所述將獲取的所述軌跡數據作為初始變換矩陣，返回所述利用所述初始變換矩陣對所述當前幀點云進行數據變換，以獲取過渡幀點云的步驟進行迭代時，在利用所述參考幀點云和所述過渡幀點云得到對應點對集合的步驟中，采用的預設距離閾值比上次的所述預設距離閾值小。

6.根據權利要求1所述的運動裝置的軌跡數據處理方法，其特征在于，所述運動裝置包括機器人。

7.根據權利要求1所述的運動裝置的軌跡數據處理方法，其特征在于，所述以獲取所述參考幀點云時所述運動裝置的朝向為參考視線，對于所述過渡幀點云中的每一個點，在所述參考視線附近區域搜索所述參考幀點云中與該點對應的點列的步驟包括：

以所述參考幀點云所在位置的機器人朝向為參考視線，對于所述過渡幀點云中的每一個點，從所述參考視線開始往兩邊逐點搜索所述參考幀點云中與該點對應的點列。