【技術(shù)實(shí)現(xiàn)步驟摘要】
本專利技術(shù)設(shè)計(jì)自動(dòng)駕駛，尤其涉及一種自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法和控制器。
技術(shù)介紹
1、軌跡是關(guān)于位置、姿態(tài)、速度、加速度等狀態(tài)信息的時(shí)間函數(shù)，不僅包含了自動(dòng)駕駛設(shè)備每一時(shí)刻要到達(dá)的位置、姿態(tài)、速度、加速度等狀態(tài)信息，還包含了時(shí)間信息。然而現(xiàn)有的無人車軌跡跟蹤方法，一般只注重空間維度上的橫向偏差，在時(shí)間維度上跟蹤性能上較差，往往會(huì)產(chǎn)生較大的縱向偏差，或無法同時(shí)兼顧空間維度的橫向誤差和時(shí)間維度的縱向誤差，一方面可能會(huì)頻繁觸發(fā)上層軌跡規(guī)劃器端的重規(guī)劃，引起額外的資源消耗，且重規(guī)劃時(shí)一般會(huì)帶來小車速度上的波動(dòng)，使得車輛運(yùn)行不夠平穩(wěn)；另一方面，在涉及到動(dòng)態(tài)障礙物避障、超車等場(chǎng)景時(shí)，縱向偏差大，將導(dǎo)致避障或超車不及時(shí)等問題的發(fā)生。
技術(shù)實(shí)現(xiàn)思路
1、本專利技術(shù)針對(duì)現(xiàn)有技術(shù)中的不足，提供了一種自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，包括如下步驟：
2、s1，獲取自動(dòng)行駛設(shè)備的當(dāng)前位姿信息和規(guī)劃軌跡，在規(guī)劃軌跡中查找獲取距離當(dāng)前位姿信息的最接近規(guī)劃點(diǎn)，根據(jù)當(dāng)前行駛時(shí)間在規(guī)劃規(guī)則中查找對(duì)應(yīng)的規(guī)劃途徑點(diǎn)，所述規(guī)劃途徑點(diǎn)為在規(guī)劃軌跡中經(jīng)過當(dāng)前行駛時(shí)間計(jì)劃到達(dá)的行駛點(diǎn)；
3、s2，從最接近規(guī)劃點(diǎn)開始沿規(guī)劃軌跡截取設(shè)定長(zhǎng)度的一參考軌跡段，計(jì)算獲取跟蹤所述參考軌跡段的最優(yōu)控制輸出量，所述最優(yōu)控制輸出量包括第一線速度控制量vlat和第一角速度控制量wlat；
4、s3，根據(jù)所述規(guī)劃途徑點(diǎn)的位姿信息計(jì)算獲取跟蹤規(guī)劃途徑點(diǎn)的第二線速度控制量vlon；
5、s4，將從當(dāng)前
6、優(yōu)選的，所述步驟s2包括：
7、根據(jù)設(shè)定的約束條件和優(yōu)化求解問題的代價(jià)函數(shù)算法，通過非線性優(yōu)化求解，計(jì)算出跟蹤所述參考軌跡段的最優(yōu)控制輸入量序列：uk，k＝0，1，2...nc；
8、取u0作為初始最優(yōu)控制輸入量，計(jì)算獲取跟蹤所述參考軌跡段的最優(yōu)控制輸出量，所述最優(yōu)控制輸出量包括第一線速度控制量vlat和第一角速度控制量wlat：
9、其中vfb為當(dāng)前自動(dòng)行駛設(shè)備的線速度，wfb為當(dāng)前自動(dòng)行駛設(shè)備的角速度，dt為控制周期。
10、優(yōu)選的，所述步驟s3包括：將所述規(guī)劃途徑點(diǎn)的位姿信息輸入pid控制模型中獲取第二線速度控制量vlon，所述pid控制模型為：
11、其中vt為對(duì)應(yīng)規(guī)劃途徑點(diǎn)的位姿信息中的參考速度值，kp、ki、kd分別為比例、積分、微分增益，e(t)、e(t-1)分別為當(dāng)前和上一個(gè)控制周期的縱向跟蹤誤差值，為根據(jù)縱向跟蹤誤差值計(jì)算的線速度控制量補(bǔ)償值；所述縱向跟蹤誤差值stationerror為：stationerror＝s1-s2，其中s1為自動(dòng)行駛設(shè)備的當(dāng)前位姿狀態(tài)在規(guī)劃軌跡frenet坐標(biāo)系下的路程，s2為相應(yīng)規(guī)劃途徑點(diǎn)處規(guī)劃軌跡frenet坐標(biāo)系下的路程。
12、優(yōu)選的，所述步驟s3還包括：對(duì)第二線速度控制量vlon，設(shè)定如下加速度限制：其中，和為預(yù)設(shè)的線加速度約束邊界，vfb為自動(dòng)行駛設(shè)備的當(dāng)前的線速度反饋；對(duì)積分項(xiàng)dt設(shè)置積分飽和值限制emax，且當(dāng)輸出的第二線速度控制量vlon觸發(fā)所述線加速度約束邊界的線加速度限制時(shí)，停止積分項(xiàng)的累積。
13、優(yōu)選的，所述步驟s4還包括：將從當(dāng)前參考軌跡段中獲取的第一線速度控制量vlat和第一角速度控制量wlat、與第二線速度控制量vlon進(jìn)行協(xié)調(diào)處理后得到控制輸出量，所述協(xié)調(diào)處理被配置為根據(jù)第二線速度控制量vlon與第一線速度控制量vlat之間的偏差量對(duì)第一線速度控制量vlat和第一角速度控制量wlat進(jìn)行補(bǔ)償來使得自動(dòng)行駛設(shè)備的轉(zhuǎn)動(dòng)速度和直行速度符合預(yù)設(shè)協(xié)調(diào)條件，所述控制輸出量包括第三線速度控制量vout和第三角速度控制量wout，將所述第三線速度控制量和第三角速度控制量發(fā)送至控制執(zhí)行器件來驅(qū)動(dòng)所述自動(dòng)行駛設(shè)備實(shí)時(shí)跟蹤目標(biāo)軌跡。將從當(dāng)前參考軌跡段中獲取的第一線速度控制量vlat和第一角速度控制量wlat、與第二線速度控制量vlon輸入?yún)f(xié)調(diào)控制模型中來計(jì)算獲取最終輸出的第三線速度控制量vout和第三角速度控制量wout，所述協(xié)調(diào)控制模型為：
14、
15、其中vcmpst和wcmpst為中間量，表示對(duì)vlat和wlat的補(bǔ)償值；kv和kw為補(bǔ)償增益，vdelt為vlon和vlat之間的偏差量，tmin和tmax為自動(dòng)行駛設(shè)備的角加速度的約束邊界，wfb為自動(dòng)行駛設(shè)備的當(dāng)前的角速度反饋。
16、本專利技術(shù)還公開了一種軌跡跟蹤控制裝置，用于自動(dòng)行駛設(shè)備的行駛軌跡控制，包括位姿獲取模塊、橫向控制模塊、縱向控制模塊和橫縱協(xié)調(diào)控制模塊，其中位姿獲取模塊，用于根據(jù)獲取的當(dāng)前位姿信息和規(guī)劃軌跡，在規(guī)劃軌跡中查找獲取距離當(dāng)前位姿信息的最接近規(guī)劃點(diǎn)，根據(jù)當(dāng)前行駛時(shí)間在規(guī)劃規(guī)則中查找對(duì)應(yīng)的規(guī)劃途徑點(diǎn)，所述規(guī)劃途徑點(diǎn)為在規(guī)劃軌跡中經(jīng)過當(dāng)前行駛時(shí)間計(jì)劃到達(dá)的行駛點(diǎn)；橫向控制模塊，用于從最接近規(guī)劃點(diǎn)開始沿規(guī)劃軌跡截取設(shè)定長(zhǎng)度的一參考軌跡段，計(jì)算獲取跟蹤所述參考軌跡段的最優(yōu)控制輸出量，所述最優(yōu)控制輸出量包括第一線速度控制量和第一角速度控制量；縱向控制模塊，用于根據(jù)所述規(guī)劃途徑點(diǎn)的位姿信息計(jì)算獲取跟蹤規(guī)劃途徑點(diǎn)的第二線速度控制量；橫縱協(xié)調(diào)控制模塊，用于將從當(dāng)前參考軌跡段中獲取的第一線速度控制量和第一角速度控制量、與第二線速度控制量進(jìn)行協(xié)調(diào)處理后得到控制輸出量，所述協(xié)調(diào)處理被配置為根據(jù)第二線速度控制量與第一線速度控制量之間的偏差量對(duì)第一線速度控制量和第一角速度控制量進(jìn)行補(bǔ)償來使得自動(dòng)行駛設(shè)備的轉(zhuǎn)動(dòng)速度和直行速度符合預(yù)設(shè)協(xié)調(diào)條件，所述控制輸出量包括第三線速度控制量和第三角速度控制量，將所述第三線速度控制量和第三角速度控制量發(fā)送至控制執(zhí)行器件來驅(qū)動(dòng)所述自動(dòng)行駛設(shè)備實(shí)時(shí)跟蹤目標(biāo)軌跡。
17、優(yōu)選的，所述橫向控制模塊被配置為根據(jù)設(shè)定的約束條件和優(yōu)化求解問題的代價(jià)函數(shù)算法，通過非線性優(yōu)化求解，計(jì)算出跟蹤所述參考軌跡段的最優(yōu)控制輸入量序列：uk，k＝0，1，2...nc；取u0作為初始最優(yōu)控制輸入量，計(jì)算獲取跟蹤所述參考軌跡段的最優(yōu)控制輸出量，所述最優(yōu)控制輸出量包括第一線速度控制量vlat和第一角速度控制量wlat：
18、其中vfb為當(dāng)前自動(dòng)行駛設(shè)備的線速度，wfb為當(dāng)前自動(dòng)行駛設(shè)備的角速度，dt為控制周期。
19、優(yōu)選的，所述縱向控制模塊被配置為將所述規(guī)劃途徑點(diǎn)的位姿信息輸入pid控制模型中獲取第二線速度控制量vlon，所述pid控制模型為：
20、其中v本文檔來自技高網(wǎng)...
【技術(shù)保護(hù)點(diǎn)】
1.一種自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，包括：
2.根據(jù)權(quán)利要求1所述的自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，所述步驟S2包括：
3.根據(jù)權(quán)利要求2所述的自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，所述步驟S3包括：
4.根據(jù)權(quán)利要求3所述的自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，所述步驟S3還包括：
5.根據(jù)權(quán)利要求4所述的自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，所述步驟S4還包括：
6.一種軌跡跟蹤控制裝置，用于自動(dòng)行駛設(shè)備的行駛軌跡控制，其特征在于，包括：
7.根據(jù)權(quán)利要求6所述的軌跡跟蹤控制裝置，其特征在于，所述橫向控制模塊被配置為根據(jù)設(shè)定的約束條件和優(yōu)化求解問題的代價(jià)函數(shù)算法，通過非線性優(yōu)化求解，計(jì)算出跟蹤所述參考軌跡段的最優(yōu)控制輸入量序列：uk，k＝0，1，2...Nc；取u0作為初始最優(yōu)控制輸入量，計(jì)算獲取跟蹤所述參考軌跡段的最優(yōu)控制輸出量，所述最優(yōu)控制輸出量包括第一線速度控制量vlat和第一角速度控制量ωlat：其中vfb為當(dāng)前自動(dòng)行駛設(shè)備的線速度，ωfb為當(dāng)前自動(dòng)行
8.根據(jù)權(quán)利要求7所述的軌跡跟蹤控制裝置，其特征在于，所述縱向控制模塊被配置為將所述規(guī)劃途徑點(diǎn)的位姿信息輸入PID控制模型中獲取第二線速度控制量vlon，所述PID控制模型為：其中vt為對(duì)應(yīng)規(guī)劃途徑點(diǎn)的位姿信息中的參考速度值，Kp、Ki、Kd分別為比例、積分、微分增益，e(t)、e(t-1)分別為當(dāng)前和上一個(gè)控制周期的縱向跟蹤誤差值，為根據(jù)縱向跟蹤誤差值計(jì)算的線速度控制量補(bǔ)償值；所述縱向跟蹤誤差值StationError為：StationError＝s1-s2，其中S1為自動(dòng)行駛設(shè)備的當(dāng)前位姿狀態(tài)在規(guī)劃軌跡Frenet坐標(biāo)系下的路程，S2為相應(yīng)規(guī)劃途徑點(diǎn)處規(guī)劃軌跡Frenet坐標(biāo)系下的路程。
9.根據(jù)權(quán)利要求8所述的軌跡跟蹤控制裝置，其特征在于，所述縱向控制模塊還被配置為對(duì)第二線速度控制量vlon，設(shè)定如下加速度限制：其中，和為預(yù)設(shè)的線加速度約束邊界，vfb為自動(dòng)行駛設(shè)備的當(dāng)前的線速度反饋；對(duì)積分項(xiàng)設(shè)置積分飽和值限制Emax，且當(dāng)輸出的第二線速度控制量vlon觸發(fā)所述線加速度約束邊界的線加速度限制時(shí)，停止積分項(xiàng)的累積。
10.一種控制器，包括存儲(chǔ)器、處理器以及存儲(chǔ)在所述存儲(chǔ)器中并可在所述處理器上運(yùn)行的計(jì)算機(jī)程序，其特征在于：所述處理器執(zhí)行所述計(jì)算機(jī)程序時(shí)實(shí)現(xiàn)如權(quán)利要求1-5任一所述方法的步驟。
...【技術(shù)特征摘要】
1.一種自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，包括：
2.根據(jù)權(quán)利要求1所述的自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，所述步驟s2包括：
3.根據(jù)權(quán)利要求2所述的自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，所述步驟s3包括：
4.根據(jù)權(quán)利要求3所述的自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，所述步驟s3還包括：
5.根據(jù)權(quán)利要求4所述的自動(dòng)行駛設(shè)備的軌跡跟蹤控制方法，其特征在于，所述步驟s4還包括：
6.一種軌跡跟蹤控制裝置，用于自動(dòng)行駛設(shè)備的行駛軌跡控制，其特征在于，包括：
7.根據(jù)權(quán)利要求6所述的軌跡跟蹤控制裝置，其特征在于，所述橫向控制模塊被配置為根據(jù)設(shè)定的約束條件和優(yōu)化求解問題的代價(jià)函數(shù)算法，通過非線性優(yōu)化求解，計(jì)算出跟蹤所述參考軌跡段的最優(yōu)控制輸入量序列：uk，k＝0，1，2...nc；取u0作為初始最優(yōu)控制輸入量，計(jì)算獲取跟蹤所述參考軌跡段的最優(yōu)控制輸出量，所述最優(yōu)控制輸出量包括第一線速度控制量vlat和第一角速度控制量ωlat：其中vfb為當(dāng)前自動(dòng)行駛設(shè)備的線速度，ωfb為當(dāng)前自動(dòng)行駛設(shè)備的角速度，dt為控制周期。
8.根據(jù)權(quán)利要求7所述的軌跡跟蹤控制裝置，其特征...
【專利技術(shù)屬性】
技術(shù)研發(fā)人員：楊中原，胡晉，王振華，
申請(qǐng)(專利權(quán))人：嘉興新生紀(jì)智能科技有限公司，
類型：發(fā)明
國(guó)別省市：
還沒有人留言評(píng)論。發(fā)表了對(duì)其他瀏覽者有用的留言會(huì)獲得科技券。