本申請(qǐng)涉及移動(dòng)機(jī)器人，特別是涉及一種融合多傳感器的定位方法及移動(dòng)設(shè)備。

背景技術(shù)：

1、定位技術(shù)對(duì)于移動(dòng)設(shè)備來說無疑是十分重要的基礎(chǔ)，許多傳感器都可以達(dá)到輔助定位的目的，例如，里程計(jì)、激光雷達(dá)、超聲波、攝像頭、wifi、藍(lán)牙等等。為了提高穩(wěn)定性，傳感器的混合使用在機(jī)器人領(lǐng)域也是極為常見。

2、例如，自動(dòng)導(dǎo)向車(automated?guided?vehicle，agv)使用的最普遍的定位方法是對(duì)激光雷達(dá)和底盤的輪式里程計(jì)信息進(jìn)行融合，解算激光雷達(dá)(或車體)的位置和姿態(tài)信息。

3、但是，目前的多傳感器定位方法中的傳感器可能存在定位誤差，導(dǎo)致最終的定位結(jié)果不準(zhǔn)確。

技術(shù)實(shí)現(xiàn)思路

1、為了解決上述問題，本申請(qǐng)至少提供一種融合多傳感器的定位方法及移動(dòng)設(shè)備。

2、本申請(qǐng)第一方面提供了一種融合多傳感器的定位方法，應(yīng)用于移動(dòng)設(shè)備，移動(dòng)設(shè)備至少部署有第一傳感器和第二傳感器，方法包括：獲取第一傳感器采集到的第一感知數(shù)據(jù)和第二傳感器采集到的第二感知數(shù)據(jù)；基于第一感知數(shù)據(jù)計(jì)算當(dāng)前時(shí)刻移動(dòng)設(shè)備的位姿，得到第一位姿；以及，基于第二感知數(shù)據(jù)計(jì)算當(dāng)前時(shí)刻移動(dòng)設(shè)備的位姿，得到第二位姿；計(jì)算第一位姿與第二位姿之間的位姿差，利用位姿差設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)；基于融合權(quán)重參數(shù)，對(duì)第一位姿和第二位姿進(jìn)行融合，得到移動(dòng)設(shè)備的目標(biāo)位姿。

3、在一實(shí)施例中，計(jì)算第一位姿與第二位姿之間的位姿差，包括：計(jì)算第一位姿中的位置坐標(biāo)與第二位姿中的位置坐標(biāo)之間的差距，得到位置誤差；計(jì)算第一位姿中的姿態(tài)參數(shù)與第二位姿中的姿態(tài)參數(shù)之間的差距，得到姿態(tài)誤差；將位置誤差和姿態(tài)誤差，作為第一位姿與第二位姿之間的位姿差。

4、在一實(shí)施例中，利用位姿差設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)，包括：對(duì)位置誤差和預(yù)設(shè)位置誤差閾值進(jìn)行比較，以及對(duì)姿態(tài)誤差和預(yù)設(shè)姿態(tài)誤差閾值進(jìn)行比較，得到比較結(jié)果；基于比較結(jié)果確定第二傳感器的工作狀態(tài)，工作狀態(tài)包括異常工作狀態(tài)和正常工作狀態(tài)；基于第二傳感器的工作狀態(tài)，設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)。

5、在一實(shí)施例中，基于比較結(jié)果確定第二傳感器的工作狀態(tài)，包括：若比較結(jié)果為位置誤差大于預(yù)設(shè)位置誤差閾值，或者姿態(tài)誤差大于預(yù)設(shè)姿態(tài)誤差閾值，則將第二位姿添加至錯(cuò)誤數(shù)據(jù)集合；否者，將第二位姿添加至正確數(shù)據(jù)集合；若錯(cuò)誤數(shù)據(jù)集合中的第二位姿的數(shù)量大于第一預(yù)設(shè)數(shù)量閾值，則判斷為第二傳感器的工作狀態(tài)為異常工作狀態(tài)；若錯(cuò)誤數(shù)據(jù)集合中的第二位姿的數(shù)量小于第二預(yù)設(shè)數(shù)量閾值，且正確數(shù)據(jù)集合中的第二位姿的數(shù)量大于第三預(yù)設(shè)數(shù)量閾值，則判斷為第二傳感器的工作狀態(tài)為正常工作狀態(tài)。

6、在一實(shí)施例中，基于第二傳感器的工作狀態(tài)，設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)，包括：若第二傳感器的工作狀態(tài)為正常工作狀態(tài)，則設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)為第一權(quán)重參數(shù)；若第二傳感器的工作狀態(tài)為異常工作狀態(tài)，則設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)為第二權(quán)重參數(shù)。

7、在一實(shí)施例中，第一傳感器包括激光雷達(dá)，第一位姿由多個(gè)雷達(dá)觀測(cè)信息組成；基于第一位姿和第二位姿的融合權(quán)重參數(shù)，對(duì)第一位姿和第二位姿進(jìn)行融合，得到移動(dòng)設(shè)備的目標(biāo)位姿，包括：計(jì)算第一位姿中雷達(dá)觀測(cè)信息的可信度；基于第一位姿中雷達(dá)觀測(cè)信息的可信度，對(duì)第一位姿中的雷達(dá)觀測(cè)信息進(jìn)行篩選，得到待融合的第一位姿；基于第一位姿和第二位姿的融合權(quán)重參數(shù)，對(duì)待融合的第一位姿和第二位姿進(jìn)行融合，得到移動(dòng)設(shè)備的目標(biāo)位姿。

8、在一實(shí)施例中，雷達(dá)觀測(cè)信息包括姿態(tài)參數(shù)；計(jì)算第一位姿中雷達(dá)觀測(cè)信息的可信度，包括：獲取激光雷達(dá)相對(duì)移動(dòng)設(shè)備的部署位置坐標(biāo)；利用部署位置坐標(biāo)和第一位姿中的姿態(tài)參數(shù)，計(jì)算得到第一位姿的位置偏差；基于位置偏差確定第一位姿中雷達(dá)觀測(cè)信息的可信度。

9、在一實(shí)施例中，姿態(tài)參數(shù)包括橫滾角、俯仰角和偏航角；基于第一位姿中雷達(dá)觀測(cè)信息的可信度，對(duì)第一位姿中的雷達(dá)觀測(cè)信息進(jìn)行篩選，得到待融合的第一位姿，包括：若位置偏差大于預(yù)設(shè)偏差閾值，則認(rèn)為第一位姿中雷達(dá)觀測(cè)信息的可信，保留第一位姿的所有雷達(dá)觀測(cè)信息，得到待融合的第一位姿；若位置偏差不大于預(yù)設(shè)偏差閾值，則認(rèn)為第一位姿中存在不可信的雷達(dá)觀測(cè)信息，刪除第一位姿的姿態(tài)參數(shù)中的橫滾角、俯仰角，保留第一位姿的姿態(tài)參數(shù)中的偏航角，得到待融合的第一位姿。

10、在一實(shí)施例中，第一傳感器包括激光雷達(dá)，第二傳感器包括里程計(jì)。

11、本申請(qǐng)第二方面提供了一種融合多傳感器的定位裝置，部署于移動(dòng)設(shè)備，移動(dòng)設(shè)備至少設(shè)置有第一傳感器和第二傳感器，裝置包括：感知數(shù)據(jù)獲取模塊，用于獲取第一傳感器采集到的第一感知數(shù)據(jù)和第二傳感器采集到的第二感知數(shù)據(jù)；位姿計(jì)算模塊，用于基于第一感知數(shù)據(jù)計(jì)算當(dāng)前時(shí)刻移動(dòng)設(shè)備的位姿，得到第一位姿；以及，基于第二感知數(shù)據(jù)計(jì)算當(dāng)前時(shí)刻移動(dòng)設(shè)備的位姿，得到第二位姿；權(quán)重設(shè)置模塊，用于計(jì)算第一位姿與第二位姿之間的位姿差，利用位姿差設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)；位姿融合模塊，用于基于融合權(quán)重參數(shù)，對(duì)第一位姿和第二位姿進(jìn)行融合，得到移動(dòng)設(shè)備的目標(biāo)位姿。

12、本申請(qǐng)第三方面提供了一種移動(dòng)設(shè)備，包括存儲(chǔ)器和處理器，處理器用于執(zhí)行存儲(chǔ)器中存儲(chǔ)的程序指令，以實(shí)現(xiàn)上述融合多傳感器的定位方法。

13、本申請(qǐng)第四方面提供了一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有程序指令，程序指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)上述融合多傳感器的定位方法。

14、上述方案，通過獲取第一傳感器采集到的第一感知數(shù)據(jù)和第二傳感器采集到的第二感知數(shù)據(jù)；基于第一感知數(shù)據(jù)計(jì)算當(dāng)前時(shí)刻移動(dòng)設(shè)備的位姿，得到第一位姿；以及，基于第二感知數(shù)據(jù)計(jì)算當(dāng)前時(shí)刻移動(dòng)設(shè)備的位姿，得到第二位姿；計(jì)算第一位姿與第二位姿之間的位姿差，利用位姿差設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)；基于融合權(quán)重參數(shù)，對(duì)第一位姿和第二位姿進(jìn)行融合，得到移動(dòng)設(shè)備的目標(biāo)位姿，可以根據(jù)各個(gè)傳感器位姿的位姿差，確定各個(gè)傳感器位姿的準(zhǔn)確性，從而動(dòng)態(tài)調(diào)整各個(gè)傳感器位姿的融合權(quán)重，提高定位精度，保證定位的魯棒性。

15、應(yīng)當(dāng)理解的是，以上的一般描述和后文的細(xì)節(jié)描述僅是示例性和解釋性的，而非限制本申請(qǐng)。

技術(shù)特征：

1.一種融合多傳感器的定位方法，其特征在于，應(yīng)用于移動(dòng)設(shè)備，所述移動(dòng)設(shè)備至少部署有第一傳感器和第二傳感器，包括：

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述計(jì)算所述第一位姿與所述第二位姿之間的位姿差，包括：

3.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述利用所述位姿差設(shè)置所述第一位姿和所述第二位姿的融合權(quán)重參數(shù)，包括：

4.根據(jù)權(quán)利要求3所述的方法，其特征在于，所述基于所述比較結(jié)果確定所述第二傳感器的工作狀態(tài)，包括：

5.根據(jù)權(quán)利要求3所述的方法，其特征在于，所述基于所述第二傳感器的工作狀態(tài)，設(shè)置所述第一位姿和所述第二位姿的融合權(quán)重參數(shù)，包括：

6.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述第一傳感器包括激光雷達(dá)，所述第一位姿由多個(gè)雷達(dá)觀測(cè)信息組成；所述基于所述第一位姿和所述第二位姿的融合權(quán)重參數(shù)，對(duì)所述第一位姿和所述第二位姿進(jìn)行融合，得到所述移動(dòng)設(shè)備的目標(biāo)位姿，包括：

7.根據(jù)權(quán)利要求6所述的方法，其特征在于，所述雷達(dá)觀測(cè)信息包括姿態(tài)參數(shù)；所述計(jì)算所述第一位姿中雷達(dá)觀測(cè)信息的可信度，包括：

8.根據(jù)權(quán)利要求7所述的方法，其特征在于，所述姿態(tài)參數(shù)包括橫滾角、俯仰角和偏航角；所述基于所述第一位姿中雷達(dá)觀測(cè)信息的可信度，對(duì)所述第一位姿中的雷達(dá)觀測(cè)信息進(jìn)行篩選，得到待融合的第一位姿，包括：

9.根據(jù)權(quán)利要求1至8任一項(xiàng)所述的方法，其特征在于，所述第一傳感器包括激光雷達(dá)，所述第二傳感器包括里程計(jì)。

10.一種移動(dòng)設(shè)備，其特征在于，所述移動(dòng)設(shè)備包括存儲(chǔ)器和處理器，處理器用于執(zhí)行存儲(chǔ)器中存儲(chǔ)的程序指令，以實(shí)現(xiàn)如權(quán)利要求1-9任一項(xiàng)所述方法中的步驟。

技術(shù)總結(jié)
本申請(qǐng)公開了一種融合多傳感器的定位方法及移動(dòng)設(shè)備，該融合多傳感器的定位方法包括：獲取第一傳感器采集到的第一感知數(shù)據(jù)和第二傳感器采集到的第二感知數(shù)據(jù)；基于第一感知數(shù)據(jù)計(jì)算當(dāng)前時(shí)刻移動(dòng)設(shè)備的位姿，得到第一位姿；以及，基于第二感知數(shù)據(jù)計(jì)算當(dāng)前時(shí)刻移動(dòng)設(shè)備的位姿，得到第二位姿；計(jì)算第一位姿與第二位姿之間的位姿差，利用位姿差設(shè)置第一位姿和第二位姿的融合權(quán)重參數(shù)；基于融合權(quán)重參數(shù)，對(duì)第一位姿和第二位姿進(jìn)行融合，得到移動(dòng)設(shè)備的目標(biāo)位姿。本申請(qǐng)可以根據(jù)各個(gè)傳感器位姿的位姿差，確定各個(gè)傳感器位姿的準(zhǔn)確性，從而動(dòng)態(tài)調(diào)整各個(gè)傳感器位姿的融合權(quán)重，提高定位精度，保證定位的魯棒性。

技術(shù)研發(fā)人員：石鵬,季博文,胡鯤,蔣旭平,盧維
受保護(hù)的技術(shù)使用者：浙江華睿科技股份有限公司
技術(shù)研發(fā)日：
技術(shù)公布日：2024/10/10