本技術(shù)涉及智能駕駛，特別涉及一種基于車輛位置和狀態(tài)的仿真測試方法。

背景技術(shù)：

1、隨著礦業(yè)自動化和智能化的發(fā)展，無人駕駛礦卡逐漸成為提高礦山生產(chǎn)效率和安全性的關(guān)鍵技術(shù)。無人駕駛礦卡通過自動規(guī)劃行駛路徑和控制車輛運動，可以實現(xiàn)24小時不間斷作業(yè)，減少駕駛員的勞動強度和安全風險，提高礦石運輸?shù)男屎涂煽啃浴?/p>

2、然而，無人駕駛礦卡的實際應用離不開高精度、強適應性的規(guī)劃和控制算法。這些算法需要根據(jù)礦卡的實時位置、姿態(tài)和速度等狀態(tài)信息，生成合適的轉(zhuǎn)向、加速和制動等控制指令，引導礦卡沿預定路徑行駛。為保證算法的可靠性和安全性，需要在實際礦卡和礦山環(huán)境中反復測試和驗證。但是，實車測試存在諸多局限性：一方面，礦山現(xiàn)場環(huán)境惡劣，存在碰撞、傾覆等安全隱患；另一方面，實車測試成本高昂，效率低下，且受礦山生產(chǎn)計劃的制約，難以全面覆蓋各種工況。因此，如何在保證測試全面性和安全性的同時，提高無人駕駛礦卡算法測試的效率，成為亟待解決的問題。

3、在相關(guān)技術(shù)中，比如中國專利文獻cn117031979a中提供了一種基于仿真環(huán)境的規(guī)劃算法測試方法及系統(tǒng)。該方案可以包括：仿真配置模塊獲取實際交通狀況信息；根據(jù)實際交通狀況信息，得到與實際交通狀況信息對應的仿真場景；云控規(guī)劃模塊獲取仿真場景中目標仿真車輛的車輛信息；獲取仿真場景中目標仿真車輛預設(shè)范圍內(nèi)的第一交通參與者的第一交通參與者信息；基于待測試的軌跡規(guī)劃算法，根據(jù)車輛信息以及第一交通參與者信息，生成針對目標仿真車輛的規(guī)劃控制指令；仿真模塊根據(jù)規(guī)劃控制指令，控制目標仿真車輛行駛，得到仿真測試結(jié)果。但是該方案需要先獲取實際交通狀況信息，然后根據(jù)實際交通狀況信息生成對應的仿真場景。這種依賴于實際數(shù)據(jù)的方式會限制仿真場景的生成效率和靈活性。當實際交通狀況數(shù)據(jù)量較大或更新頻率較高時，仿真場景的生成過程會成為測試流程的瓶頸，降低整體測試效率。

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

1、針對現(xiàn)有技術(shù)中無人駕駛礦卡算法測試主要依賴實車和礦山現(xiàn)場測試，存在測試效率低問題，本技術(shù)提供了一種基于車輛位置和狀態(tài)的仿真測試方法，它可以通過在仿真環(huán)境下對無人駕駛礦卡的規(guī)劃和控制算法進行測試，提高了測試效率。

2、技術(shù)方案，本技術(shù)的目的通過以下技術(shù)方案實現(xiàn)。

3、本技術(shù)提供一種基于車輛位置和狀態(tài)的仿真測試方法，包括：建立目標仿真車輛，目標仿真車輛為阿克曼轉(zhuǎn)向車輛或鉸接車；根據(jù)車輛配置參數(shù)和控制指令中的參數(shù)，計算目標仿真車輛的行駛參數(shù)；行駛參數(shù)包括速度、加速度、橫擺角速度、航向角和位置；訂閱目標仿真車輛的位姿話題和速度話題，獲取目標仿真車輛的位置、姿勢和速度；基于目標仿真車輛的位置、姿勢和速度，配置預設(shè)的云端控制指令；當目標仿真車輛當前的位置、姿勢和速度滿足預設(shè)的條件時，仿真系統(tǒng)向目標仿真車輛發(fā)送配置的云端控制指令；根據(jù)車輛配置參數(shù)和控制指令中的參數(shù)，得到仿真測試結(jié)果。

4、其中，在機器人操作系統(tǒng)（ros）中，話題（topic）是一種異步通信機制，用于在節(jié)點之間傳輸數(shù)據(jù)。位姿話題是一種特定類型的話題，用于發(fā)布機器人或車輛的位置和姿態(tài)信息。通常，位姿話題包含以下數(shù)據(jù)：位置（position）：通常由三維坐標（x，y，z）表示，指定了機器人或車輛在參考坐標系下的位置。姿態(tài)（orientation）：通常由四元數(shù)（quaternion）或歐拉角（euler?angles）表示，指定了機器人或車輛相對于參考坐標系的旋轉(zhuǎn)方向。

5、位姿話題的常見格式包括geometry_msgs/pose、nav_msgs/odometry等。訂閱位姿話題，可以獲取機器人或車輛的實時位置和姿態(tài)信息，用于定位、導航、控制等目的。速度話題是ros中另一種常見的話題類型，用于發(fā)布機器人或車輛的速度信息。速度話題通常包含以下數(shù)據(jù)：線速度（linear?velocity）：通常由三維向量（vx，vy，vz）表示，指定了機器人或車輛在參考坐標系下的平移速度。角速度（angular?velocity）：通常由三維向量（ωx，ωy，ωz）表示，指定了機器人或車輛相對于參考坐標系的旋轉(zhuǎn)速度。速度話題的常見格式包括geometry_msgs/twist、nav_msgs/odometry等。訂閱速度話題，可以獲取機器人或車輛的實時速度信息，用于運動控制、速度估計、碰撞檢測等目的。

6、阿克曼轉(zhuǎn)向是一種常見的四輪車輛轉(zhuǎn)向機構(gòu)，廣泛應用于汽車、卡車等車輛。阿克曼轉(zhuǎn)向的特點是，當車輛轉(zhuǎn)向時，內(nèi)側(cè)前輪的轉(zhuǎn)角大于外側(cè)前輪，使得四個車輪的轉(zhuǎn)彎中心位于同一點，從而減小輪胎的側(cè)偏磨損，提高轉(zhuǎn)向穩(wěn)定性。鉸接車是一種由兩個或多個剛性車體通過鉸接點連接而成的特殊車輛，常見于長距離運輸、建筑工程、農(nóng)業(yè)機械等領(lǐng)域。鉸接車的典型代表包括半掛車、全掛車、鉸接式公交車等。

7、進一步的，當目標仿真車輛為阿克曼轉(zhuǎn)向車輛時，車輛配置參數(shù)和控制指令中的參數(shù)包括：控制指令中的參數(shù)包含油門踏板開度、制動踏板開度、期望加速度、擋位和前輪轉(zhuǎn)角；車輛配置參數(shù)包含最大驅(qū)動力、最大制動力、車輛質(zhì)量、地面坡度、滾動阻力系數(shù)、車輛固有阻力系數(shù)和車輛軸距；

8、進一步的，當目標仿真車輛為鉸接車時，車輛配置參數(shù)和控制指令中的參數(shù)包括：控制指令中的參數(shù)包含油門踏板開度、制動踏板開度、期望加速度、擋位和前輪轉(zhuǎn)角；車輛配置參數(shù)包含最大驅(qū)動力、最大制動力、車輛質(zhì)量、地面坡度、滾動阻力系數(shù)、車輛固有阻力系數(shù)和前后軸到鉸接點距離。

9、進一步的，當目標仿真車輛為阿克曼轉(zhuǎn)向車輛時，計算目標仿真車輛的行駛參數(shù)，包括：根據(jù)控制指令中的油門踏板開度、制動踏板開度、期望加速度和檔位，以及車輛配置參數(shù)中的最大驅(qū)動力、最大制動力和車輛質(zhì)量，計算目標仿真車輛的縱向加速度；根據(jù)控制指令中的前輪轉(zhuǎn)角，以及車輛配置參數(shù)中的車輛軸距，計算目標仿真車輛的橫擺角速度；根據(jù)計算得到的縱向加速度和上一時刻車速，計算當前時刻的車速v；根據(jù)上一時刻車輛航向角和計算得到的車輛橫擺角速度，計算當前時刻的車輛航向角；根據(jù)上一時刻車輛的位置、計算得到的當前車速v和當前車輛航向角，計算當前的位置。

10、進一步的，計算阿克曼轉(zhuǎn)向車輛的橫擺角速度，通過如下公式：，其中，表示當前時刻的車頭航向角，l表示車輛軸距，v代表當前時刻的車速。

11、進一步的，當目標仿真車輛為鉸接車時，計算目標仿真車輛的行駛參數(shù)，包括：根據(jù)控制指令中的油門踏板開度、制動踏板開度、期望加速度和檔位，以及車輛配置參數(shù)中的最大驅(qū)動力、最大制動力和車輛質(zhì)量，計算目標仿真車輛的縱向加速度；根據(jù)控制指令中的前輪轉(zhuǎn)角，以及車輛配置參數(shù)中的前后軸到鉸接點距離，計算目標仿真車輛的鉸接角和車頭橫擺角速度，并將車頭橫擺角速度作為車輛的橫擺角速度；根據(jù)車輛的橫擺角速度和鉸接角，計算車頭航向角和車尾航向角，并將車頭航向角作為車輛航向角；根據(jù)計算得到的縱向加速度和上一時刻車速，計算當前時刻的車速v；根據(jù)上一時刻車輛航向角和計算得到的車輛橫擺角速度，計算當前時刻的車頭航向角；根據(jù)上一時刻車輛的位置、計算得到的當前車速v和當前車輛航向角，計算當前的位置。

12、進一步的，計算鉸接車的縱向加速度，通過如下公式：，其中，表示油門踏板開度對應的輪上驅(qū)動力；表示制動踏板開度對應的輪上制動力；表示車輛受到的縱向阻力，包括滾動阻力、空氣阻力、固有阻力和坡度阻力；m表示車輛質(zhì)量。

13、進一步的，計算鉸接車的橫擺角速度，通過如下公式：

14、，其中，和分別為前后軸到鉸接點的距離；表示前后車廂之間的相對轉(zhuǎn)角，即鉸接角；表示鉸接角偏轉(zhuǎn)速度；車速v通過如下公式計算：，其中，為仿真時間步長；

15、車輛當前航向角通過如下公式計算：，其中，為車輛當前航向角；車輛當前位置通過如下公式計算：。

16、進一步的，基于目標仿真車輛的位置、姿勢和速度，配置預設(shè)的云端控制指令，包括：獲取目標仿真車輛的當前位置，并根據(jù)預設(shè)的目標仿真車輛的預期坐標、航向角及對應誤差范圍，判斷目標仿真車輛是否位于預設(shè)位置；獲取目標仿真車輛的當前狀態(tài)，當前狀態(tài)包含車輛是否處于運動狀態(tài)，以及距離上一次收到云端控制指令的時間間隔，并根據(jù)預設(shè)的車輛運動狀態(tài)條件和最小時間間隔條件，判斷目標仿真車輛是否處于預設(shè)狀態(tài)；如果目標仿真車輛同時滿足預設(shè)位置和預設(shè)狀態(tài)，則配置目標仿真車輛接收云端控制指令。

17、其中，判斷目標仿真車輛是否同時滿足預設(shè)位置和預設(shè)狀態(tài)：判斷目標仿真車輛是否位于預設(shè)位置；判斷目標仿真車輛是否處于預設(shè)狀態(tài)；若目標仿真車輛同時滿足位于預設(shè)位置和處于預設(shè)狀態(tài)，則判定目標仿真車輛同時滿足預設(shè)位置和預設(shè)狀態(tài)；若目標仿真車輛同時滿足預設(shè)位置和預設(shè)狀態(tài)，則提取云端控制指令中的控制參數(shù)，其中，云端控制指令包括：限速控制：根據(jù)井下路況和安全要求，下發(fā)合適的最高速度限制；緊急制動：在檢測到危險情況（如障礙物、人員）時，立即下發(fā)制動指令；軌道引導：根據(jù)車輛位置和目標軌道，下發(fā)轉(zhuǎn)向控制指令，實現(xiàn)軌道引導功能。

18、進一步的，配置目標仿真車輛在預設(shè)位置時的判斷方法，包括：獲取目標仿真車輛在仿真坐標系下的當前位置坐標和航向角；計算當前位置坐標與預設(shè)的目標仿真車輛的預期坐標之間的距離差和角度差；判斷距離差是否在預設(shè)坐標誤差范圍內(nèi)：；計算當前航向角與預設(shè)的目標仿真車輛的預期航向角之間的角度差，判斷角度差是否在預設(shè)航向角誤差范圍內(nèi)，即：；如果距離差在預期坐標誤差范圍內(nèi)，且角度差在預期航向角誤差范圍內(nèi)，則判斷目標仿真車輛位于預設(shè)位置；其中，距離差通過歐氏距離公式計算：。

19、角度差根據(jù)航向角計算：，其中為預期坐標的參考方向。

20、其中，配置目標仿真車輛在預設(shè)狀態(tài)時的判斷方法，包括：獲取目標仿真車輛在仿真坐標系下的當前速度大小；根據(jù)預設(shè)的速度閾值判斷目標仿真車輛是否處于運動狀態(tài)：若，則判定目標仿真車輛處于運動狀態(tài)；若，則判定目標仿真車輛處于靜止狀態(tài)；獲取當前時刻，并計算當前時刻與上一次接收云端控制指令時刻之間的時間間隔：；根據(jù)預設(shè)的最小時間間隔閾值判斷時間間隔是否滿足條件：若，則判定時間間隔條件滿足；若，則判定時間間隔條件不滿足；若目標仿真車輛處于預設(shè)的運動狀態(tài)，且時間間隔滿足最小時間間隔條件，則判定目標仿真車輛處于預設(shè)狀態(tài)。

21、優(yōu)選地，獲取目標仿真車輛在仿真坐標系下的當前速度向量，包括速度大小和速度方向；根據(jù)預設(shè)的速度閾值和速度方向閾值，判斷目標仿真車輛是否處于運動狀態(tài)：若且在范圍內(nèi)，則判定目標仿真車輛處于運動狀態(tài)；否則，判定目標仿真車輛處于靜止狀態(tài)；獲取目標仿真車輛的當前加速度、輪速和驅(qū)動電機電流，根據(jù)預設(shè)的加速度閾值、輪速閾值和電流閾值，進一步判斷目標仿真車輛的運動狀態(tài)：若或或，則判定目標仿真車輛處于運動狀態(tài)；否則，維持之前的運動狀態(tài)判斷結(jié)果；獲取當前時刻，并計算當前時刻與上一次接收云端控制指令時刻t_last之間的時間間隔δt：；根據(jù)云端控制指令的類型，選擇對應的最小時間間隔閾值：若為緊急制動指令，則選擇較小的；若為巡航控制指令，則選擇較大的；若為其他類型指令，則選擇常規(guī)的；判斷時間間隔δt是否滿足條件：若，則判定時間間隔條件滿足；若，則判定時間間隔條件不滿足；若目標仿真車輛處于預設(shè)的運動狀態(tài)，且時間間隔δt滿足最小時間間隔條件，則判定目標仿真車輛處于預設(shè)狀態(tài)。

22、相比于現(xiàn)有技術(shù)，本技術(shù)的優(yōu)點在于：

23、通過建立仿真礦卡模型并計算其行駛參數(shù)，可以在仿真環(huán)境下全面模擬礦卡的運動狀態(tài)和動力學特性，為無人駕駛礦卡算法測試提供逼真的數(shù)字孿生。結(jié)合礦卡的物理參數(shù)和控制指令生成行駛參數(shù)，保證了仿真礦卡與實際礦卡的一致性，提高了仿真測試的可靠性。

24、通過實時獲取仿真礦卡的位置、姿勢和速度信息并判斷其是否滿足預設(shè)條件，可以根據(jù)礦卡的實時狀態(tài)動態(tài)配置控制指令，實現(xiàn)對無人駕駛礦卡算法的適應性測試。預設(shè)不同的位置和狀態(tài)條件，可以全面驗證算法在各種工況下的可行性和魯棒性，無需實車和礦山現(xiàn)場即可高效完成測試任務。

25、通過向滿足預設(shè)條件的仿真礦卡發(fā)送預配置的控制指令，并比較其實際行駛軌跡與預期軌跡，可以定量評估無人駕駛礦卡算法的控制精度和路徑跟蹤能力。在仿真環(huán)境下以軟件方式自動化生成控制指令和分析軌跡偏差，大幅提升了算法測試的效率和覆蓋全面性，降低了人工操作的難度和工作量。

26、本技術(shù)的仿真測試方法適用于鉸接式和剛性車架兩種類型的礦卡，通過針對不同底盤類型給出相應的行駛參數(shù)計算方法，可以擴大算法測試的適用范圍，提高了測試結(jié)果的普適性。對接不同礦卡的物理模型，可以用一套測試系統(tǒng)評估多種礦卡的無人駕駛算法，具有很強的通用性和適應性。

27、本技術(shù)提供的仿真測試方法可以在研發(fā)初期即投入使用，通過軟件環(huán)境模擬礦卡和礦山，顯著降低了測試成本，提高了測試安全性，加快了算法迭代速度。同時測試過程可追溯、可復現(xiàn)，為算法優(yōu)化提供了豐富的數(shù)據(jù)支撐，與傳統(tǒng)的實車測試方式相比，極大地提升了無人駕駛礦卡的測試效率和算法性能。