本發明涉及自動駕(jia)駛仿真，特(te)別涉及一種基于g29方向盤和ros的駕(jia)考(kao)全場(chang)景仿真平臺(tai)。

背景技術：

1、目前，借助于(yu)人(ren)工智能及(ji)自(zi)動(dong)駕(jia)駛技術(shu)的(de)發展(zhan)，駕(jia)考機器(qi)人(ren)在(zai)駕(jia)培場景(jing)中(zhong)應用越來越廣泛。駕(jia)考機器(qi)人(ren)從架構上劃分(fen)為定位、地圖(tu)、感(gan)知、規劃控(kong)制和(he)教學(xue)，對環境(jing)的(de)感(gan)知和(he)響應以及(ji)對地圖(tu)的(de)認知及(ji)規劃、控(kong)制、教學(xue)是(shi)駕(jia)考機器(qi)人(ren)的(de)兩(liang)大核心任務；如何(he)在(zai)實驗室環境(jing)完成各(ge)算法模塊的(de)快速(su)驗證，直接影響駕(jia)考機器(qi)人(ren)的(de)迭代速(su)度(du)。

2、由(you)于駕考機(ji)器人主要依賴當前定位和實(shi)時傳感(gan)(gan)器信息，以及地圖信息進行規劃控制和教學，目前主流方法(fa)是在真(zhen)實(shi)環境(jing)下接入rtk設(she)備(bei)、感(gan)(gan)知設(she)備(bei)進行場地內(nei)調試，在真(zhen)實(shi)場景下開發存(cun)在效(xiao)率(lv)低、難以覆蓋各(ge)種odd場景的問題(ti)。

3、因此，如(ru)何(he)對(dui)定位、感知等(deng)(deng)功能進(jin)行全(quan)方位仿真(zhen)(zhen)，從而(er)快(kuai)速驗證真(zhen)(zhen)實(shi)駕(jia)考場景下的(de)規劃、避障、acc、aeb、自動示教等(deng)(deng)算法，推進(jin)駕(jia)考機器人(ren)算法的(de)場景適(shi)應能力及快(kuai)速迭代(dai)能力，是(shi)亟(ji)需解決的(de)技術問(wen)題(ti)。

技術實現思路

1、有(you)鑒于此，本發(fa)明(ming)提供一種基(ji)于g29方向盤和(he)ros的(de)駕考全(quan)場景仿(fang)真(zhen)(zhen)平臺，該仿(fang)真(zhen)(zhen)平臺基(ji)于g29方向盤驅(qu)動(dong)和(he)ros平臺的(de)rviz工具搭建，具備定位(wei)仿(fang)真(zhen)(zhen)、障礙物仿(fang)真(zhen)(zhen)等功能，可在實驗室環境(jing)下(xia)對定位(wei)、感知進行全(quan)方位(wei)仿(fang)真(zhen)(zhen)，從而(er)快(kuai)速驗證(zheng)真(zhen)(zhen)實駕考場景下(xia)的(de)規劃(hua)、避障、acc、aeb、自動(dong)示教等算法(fa)，極(ji)大地推進駕考機(ji)器(qi)人算法(fa)的(de)場景適應能力(li)及快(kuai)速迭(die)代能力(li)。

2、為實(shi)現上述目的，本發明采(cai)取的技術方案為：

3、本發明提供(gong)一種基于g29方(fang)向盤(pan)和(he)ros的駕考全(quan)場(chang)景仿(fang)真平(ping)臺，應用于駕考場(chang)景，該仿(fang)真平(ping)臺包括(kuo)：g29方(fang)向盤(pan)仿(fang)真模塊、激光雷達仿(fang)真模塊、毫米波雷達仿(fang)真模塊、定(ding)位仿(fang)真模塊以及(ji)算(suan)法模塊，所述算(suan)法模塊包括(kuo)：acc/aeb系統(tong)、自動示教系統(tong)、自動駕駛(shi)系統(tong)，其中(zhong)：

4、所述(shu)g29方(fang)(fang)向(xiang)(xiang)盤仿真(zhen)模(mo)塊，用于將(jiang)g29方(fang)(fang)向(xiang)(xiang)盤發布消(xiao)(xiao)息(xi)中的方(fang)(fang)向(xiang)(xiang)盤、油門(men)和檔位(wei)信息(xi)轉(zhuan)為預(yu)設(she)消(xiao)(xiao)息(xi)格式并發布，所述(shu)預(yu)設(she)消(xiao)(xiao)息(xi)格式包括：線速(su)度(du)和角速(su)度(du)；

5、所述激光雷達仿真模塊(kuai)(kuai)，用于發布傳(chuan)感器數(shu)據以供算法(fa)模塊(kuai)(kuai)進行速度規劃、路徑規劃以及避障的測試；

6、所述毫米(mi)(mi)波(bo)雷(lei)達仿真模塊，用于給定毫米(mi)(mi)波(bo)初(chu)始位置和初(chu)始速度(du)，通過訂(ding)閱方向盤預設按(an)鍵進行速度(du)更(geng)新(xin)、毫米(mi)(mi)波(bo)位置更(geng)新(xin)、坐標轉換及毫米(mi)(mi)波(bo)雷(lei)達目標發(fa)布(bu)；

7、所述定(ding)位(wei)仿(fang)真模(mo)塊，用于進行(xing)車(che)輛位(wei)置更新(xin)以及動態(tai)tf和里程計(ji)的發(fa)布；

8、所述算法(fa)模塊(kuai)，用(yong)于根據g29方向盤仿(fang)真(zhen)模塊(kuai)、激光雷達(da)仿(fang)真(zhen)模塊(kuai)、毫米(mi)波雷達(da)仿(fang)真(zhen)模塊(kuai)以及(ji)定位(wei)仿(fang)真(zhen)模塊(kuai)發布的(de)信息進行速(su)度規劃(hua)(hua)、路(lu)徑規劃(hua)(hua)和避障。

9、進一步(bu)地，所述預設消息格式(shi)為(wei)geometry_msgs/twiststamped消息格式(shi)。

10、進(jin)一步地，所(suo)述(shu)g29方向(xiang)盤仿真模塊中，線速(su)度和角(jiao)速(su)度的計算公式為：

11、vx＝vmax×gear×(axes2+1.0)/2.0

12、ωz＝vx/r

13、其中，vx表(biao)示x軸(zhou)(zhou)線(xian)速(su)度(du)，vmax表(biao)示限制的最大速(su)度(du)；gear表(biao)示檔位值；axes2表(biao)示油門踏板的值；ωz表(biao)示z軸(zhou)(zhou)角速(su)度(du)，r表(biao)示車輛后(hou)軸(zhou)(zhou)中心轉彎半徑；

14、r的求解方程為：

15、r＝wheel_base/tan(abs(max_steer_angle×axes0))

16、其(qi)中(zhong)，wheel_base表示(shi)車輛軸(zhou)距；axes0表示(shi)方向盤的(de)值；max_steer_angle表示(shi)設定(ding)的(de)最大(da)方向盤角度。

17、進(jin)一步(bu)地，所述激(ji)光雷達(da)仿真模(mo)塊通過(guo)ros平(ping)臺的rviz工具發布geometry_msgs/pointstamped的消(xiao)息格式，通過(guo)訂閱該(gai)消(xiao)息，將該(gai)消(xiao)息轉換成sensor_msgs/pointcloud2的點云格式，并發布。

18、進一步地，所述毫(hao)米波(bo)雷達(da)仿真模(mo)塊中(zhong)，進行速度(du)更新(xin)、毫(hao)米波(bo)位置更新(xin)、坐標轉換及毫(hao)米波(bo)雷達(da)目標發布，具體方法(fa)為(wei)：

19、速度更新：

20、通過訂閱方(fang)向盤的(de)第(di)(di)一按(an)鈕和第(di)(di)二按(an)鈕進(jin)行速(su)(su)度(du)更新；當第(di)(di)一按(an)鈕為1時，毫(hao)米波雷達目標(biao)的(de)橫向絕對速(su)(su)度(du)增加1km/h；當第(di)(di)二按(an)鈕為1時，毫(hao)米波雷達目標(biao)的(de)橫向絕對速(su)(su)度(du)減少1km/s；縱向不做速(su)(su)度(du)仿真；其速(su)(su)度(du)為在世界坐標(biao)系下的(de)速(su)(su)度(du)；

21、毫米波(bo)位置更新：

22、根據速度消息，對毫(hao)米波(bo)的絕對位置進行更新，該值為世(shi)界坐標系下值：

23、ref_x＝ref_x+vx*delta_t/3.6

24、ref_y＝ref_y+vy*delta_t/3.6

25、其中，ref_x和ref_y表(biao)示初始參(can)考(kao)位(wei)置；vx和vy表(biao)示毫米波雷(lei)達目標的縱向和橫(heng)向絕對速(su)度；delta_t表(biao)示采樣時間；

26、坐標轉換及毫米(mi)波雷達目標發布：

27、根(gen)據動態和(he)靜態tf，將世界坐標系(xi)下的(de)(de)初(chu)始(shi)參(can)考位置通(tong)過ros的(de)(de)tf工具，轉換到毫米波本體坐標系(xi)中；其中橫(heng)向相對速度為仿真速度-車速。

28、進(jin)一步地，所述定位仿真模(mo)塊中，車(che)輛(liang)位置(zhi)(zhi)更(geng)新的方法為(wei)：根據不(bu)同的駕(jia)駛模(mo)式，接入不(bu)同模(mo)塊發布的geometry_msgs/twiststamped進(jin)行位置(zhi)(zhi)更(geng)新，其位置(zhi)(zhi)更(geng)新的公式如下：

29、x＝x+vbx×delta_t×cos(yaw)

30、y＝y+vbx×delta_t×sin(yaw)

31、yaw＝yaw+ωbz*delta_t

32、其(qi)中(zhong)，x、y為車(che)輛(liang)當前位置；delta_t為采樣時間；yaw為車(che)輛(liang)當前航向角；vbx為車(che)輛(liang)后(hou)軸中(zhong)心(xin)(xin)的(de)的(de)線速(su)度；ωbz為車(che)輛(liang)后(hou)軸中(zhong)心(xin)(xin)的(de)角速(su)度；

33、動(dong)態tf和里程計的發布方法為：

34、進行位姿更新后，利(li)用ros的transfrombroadcaster發(fa)布(bu)車輛base_link相對(dui)于世(shi)界坐標(biao)系下的動態tf，動態tf和傳感器之間的靜(jing)態tf構成機器人系統完整的坐標(biao)變換(huan)；同時發(fa)布(bu)類(lei)型為nav_masgs/odometry格(ge)式的里程計信息，供其他(ta)模塊使用。

35、進一步地(di)，所述算法模塊(kuai)中，所述acc/aeb系統訂閱定位仿(fang)(fang)真模塊(kuai)發(fa)布(bu)的(de)(de)(de)里程計信息(xi)、g29方向盤仿(fang)(fang)真模塊(kuai)發(fa)布(bu)的(de)(de)(de)方向盤信息(xi)以及(ji)激光雷(lei)達(da)仿(fang)(fang)真模塊(kuai)和毫米(mi)波雷(lei)達(da)仿(fang)(fang)真模塊(kuai)的(de)(de)(de)仿(fang)(fang)真數據，進行速度規劃，并(bing)發(fa)布(bu)geometry_msgs/twiststamped的(de)(de)(de)消息(xi)格(ge)式；

36、所述自動示教系(xi)統訂閱定位仿(fang)真模塊發布的里程計(ji)(ji)信息(xi)，結合科目地(di)圖信息(xi)，進(jin)行(xing)pnc，計(ji)(ji)算出車輛后(hou)軸中心(xin)處(chu)的轉(zhuan)向角速度(du)(du)和(he)速度(du)(du)值(zhi)，發布geometry_msgs/twiststamped消息(xi)格式；

37、所述(shu)自動(dong)駕駛系統訂閱(yue)定位仿真模(mo)塊發(fa)(fa)布的里程計信息，結合駕培(pei)場地全局(ju)柵格地圖信息和(he)激光雷(lei)達仿真模(mo)塊發(fa)(fa)布的激光雷(lei)達點云信息，規劃(hua)路徑，并計算轉向(xiang)角(jiao)度(du)和(he)速度(du)值，發(fa)(fa)布geometry_msgs/twiststamped消息格式。

38、與現有(you)技術(shu)相比(bi)，本發明的技術(shu)方案至少具(ju)有(you)如下(xia)有(you)益技術(shu)效果：

39、本(ben)發明提(ti)供了一(yi)種基于(yu)g29方向(xiang)盤和(he)(he)ros的駕考全場景(jing)仿真(zhen)(zhen)(zhen)平(ping)臺，該仿真(zhen)(zhen)(zhen)平(ping)臺基于(yu)g29方向(xiang)盤驅動和(he)(he)ros平(ping)臺的rviz工具搭建，具備定位(wei)仿真(zhen)(zhen)(zhen)、障礙物仿真(zhen)(zhen)(zhen)等(deng)功(gong)能(neng)，可在實(shi)驗室(shi)環(huan)境下對(dui)定位(wei)、感知(zhi)進行全方位(wei)仿真(zhen)(zhen)(zhen)，從而快速(su)驗證真(zhen)(zhen)(zhen)實(shi)駕考場景(jing)下的規劃、避障、acc、aeb、自動示教(jiao)等(deng)算(suan)法，有(you)助于(yu)推(tui)進駕考機(ji)器人算(suan)法的場景(jing)適應能(neng)力(li)及(ji)快速(su)迭代能(neng)力(li)，提(ti)高效率，節約成本(ben)。

40、本(ben)發(fa)明的(de)其(qi)(qi)它特征和(he)優(you)點(dian)將(jiang)在隨后的(de)說明書中(zhong)闡述，并且，部分地從說明書中(zhong)變得顯而易見，或者通(tong)過(guo)(guo)實施本(ben)發(fa)明而了解(jie)。本(ben)發(fa)明的(de)目的(de)和(he)其(qi)(qi)他優(you)點(dian)可通(tong)過(guo)(guo)在所寫(xie)的(de)說明書以及附圖中(zhong)所特別(bie)指出的(de)結構來(lai)實現和(he)獲得。

41、下面通過附圖(tu)和實(shi)施例，對本發明的技(ji)術方(fang)案做進一步的詳細描(miao)述。