{"id":28512744,"url":"https://github.com/saxijing/cilqr","last_synced_at":"2026-04-24T23:34:09.179Z","repository":{"id":282675724,"uuid":"949315659","full_name":"saxijing/cilqr","owner":"saxijing","description":"autonomous vehicle motion planning with CILQR methods","archived":false,"fork":false,"pushed_at":"2025-05-20T06:47:14.000Z","size":61961,"stargazers_count":0,"open_issues_count":0,"forks_count":2,"subscribers_count":1,"default_branch":"main","last_synced_at":"2025-06-09T01:01:49.602Z","etag":null,"topics":["cilqr","cpp","optimalization","ros","rviz"],"latest_commit_sha":null,"homepage":"","language":"Makefile","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":null,"status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/saxijing.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":null,"code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null,"zenodo":null}},"created_at":"2025-03-16T06:57:46.000Z","updated_at":"2025-05-20T06:47:17.000Z","dependencies_parsed_at":"2025-05-20T07:53:11.321Z","dependency_job_id":null,"html_url":"https://github.com/saxijing/cilqr","commit_stats":null,"previous_names":["saxijing/cilqr"],"tags_count":0,"template":false,"template_full_name":null,"purl":"pkg:github/saxijing/cilqr","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/saxijing%2Fcilqr","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/saxijing%2Fcilqr/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/saxijing%2Fcilqr/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/saxijing%2Fcilqr/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/saxijing","download_url":"https://codeload.github.com/saxijing/cilqr/tar.gz/refs/heads/main","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/saxijing%2Fcilqr/sbom","scorecard":null,"host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":286080680,"owners_count":32245149,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2026-04-24T13:21:15.438Z","status":"ssl_error","status_checked_at":"2026-04-24T13:21:15.005Z","response_time":64,"last_error":"SSL_read: unexpected eof while reading","robots_txt_status":"success","robots_txt_updated_at":"2025-07-24T06:49:26.215Z","robots_txt_url":"https://github.com/robots.txt","online":false,"can_crawl_api":true,"host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["cilqr","cpp","optimalization","ros","rviz"],"created_at":"2025-06-09T01:00:52.500Z","updated_at":"2026-04-24T23:34:09.172Z","avatar_url":"https://github.com/saxijing.png","language":"Makefile","readme":"# CILQR方法用于自动驾驶运动规划的原理推导与操作指南\n注：为避免引入错误，全文所有矩阵求导运算均采用分子布局\n## 0 Results\n\n项目成果展示如下：\n\n\u003cdiv align=center\u003e \u003cimg src=\"https://github.com/saxijing/cilqr/blob/main/data/display_materials/Results/Following.gif\" width=500\u003e\u003cimg src=\"https://github.com/saxijing/cilqr/blob/main/data/display_materials/Results/MultiObs.gif\" width=500\u003e \u003c/div\u003e\n\n\u003cdiv align=center\u003e \u003cimg src=\"https://github.com/saxijing/cilqr/blob/main/data/display_materials/Results/LaneChange.gif\" width=500\u003e\u003cimg src=\"https://github.com/saxijing/cilqr/blob/main/data/display_materials/Results/Overtake.gif\" width=500\u003e \u003c/div\u003e\n\n\u003cp align=\"center\"\u003e项目成果展示\u003c/p\u003e\n\n\n\n本项目代码在跟车、多障碍物躲避、变道、超车这些常见场景下均表现良好，满足使用需求。由于项目使用C++ 在ROS架构下实现，所以使用rviz做成果展示，其中绿色矩形为主车，灰色矩形为npc车辆，尺寸均按照tesla model3设置，主车前面的绿色曲线为实时规划路径，可以看出项目在静态、动态场景下均有良好表现，可以根据决策结果不同设置不同参数，达到不同的速度与路径选择。\n\n表1所示为本项目代码在各个测试场景下的平均迭代次数和平均求解时间，可以看出各场景下的**平均迭代次数均小于10**， 其中多障碍物躲避和超车场景下的平均迭代次数均小于5。表1所示的平均求解时间为处理器i7-9750h下的运行速度，单位为毫秒。结合平均迭代次数来看，算法可以快速收敛至最优解，但实际求解时间仍高于预期值，表明当前性能瓶颈可能源于硬件限制或处理器算力未充分调用，后续会将此类优化作为重点改进方向。\n\n**表1 迭代次数与求解时间**\n\n|场景 |平均迭代次数 |平均求解时间/ms (i7-9750H)\n|:------:|:------:|:------:|\n|跟车|7|418|\n|多障碍物躲避|4.5|529.5|\n|变道|5|320|\n|超车|2|215.5|\n\n## 1 Vehicle Model\n将车辆位置姿态进行建模，其离散化状态空间方程为：\n\n$$\n\\vec{X}_{k+1}=f(\\vec{X_k}, \\vec{u_k})=\n\\begin{pmatrix}\nx_k+(v_k \\cdot \\Delta t+ \\frac{1}{2} \\dot{v_k} \\cdot \\Delta t^2) \\cdot \\cos \\theta_k \\\\\ny_k+(v_k \\cdot \\Delta t+ \\frac{1}{2} \\dot{v_k} \\cdot \\Delta t^2) \\cdot \\sin \\theta_k \\\\\nv_k+\\dot{v_k} \\cdot \\Delta t \\\\\n\\theta_k + \\dot{\\theta_k} \\cdot \\Delta t\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(1)\u003c/p\u003e\n其中，\n\n$$\\vec{X_k}=\\begin{pmatrix} x_k \\\\\ny_k \\\\\nv_k \\\\\n\\theta_k\n\\end{pmatrix} $$\n\n\u003cp align=\"right\"\u003e(2)\u003c/p\u003e\n\n$$\\vec{u_k}=\\begin{pmatrix} \\dot{v_k}\\\\ \n\\dot{\\theta_k} \\end{pmatrix} $$\n\n\u003cp align=\"right\"\u003e(3)\u003c/p\u003e\n对上述离散系统进行线性化，即在(xk, uk)处进行一阶Taylor展开:\n\n$$\n\\vec{X}_{k+1} + \\delta \\vec{X} _ {k+1}=f(\\vec{X_k}+ \\delta \\vec{X_k}, \\vec{u_k} +\\delta \\vec{u_k})=f(\\vec{X_k}, \\vec{u_k})+\\frac{\\partial f}{\\partial \\vec{X_k}} \\Big| _{\\vec{X_k}} + \\frac{\\partial f}{\\partial \\vec{u_k}} \\Big| _{\\vec{u_k}}\n$$\n\n\u003cp align=\"right\"\u003e(4)\u003c/p\u003e\n\n$$\n即 \\qquad \\delta \\vec{X} _{k+1}=\\frac{\\partial f}{\\partial \\vec{X_k}} \\Big| _{\\vec{X_k}}+\\frac{\\partial f}{\\partial \\vec{u_k}} \\Big| _{\\vec{u_k}}\n$$\n\n\u003cp align=\"right\"\u003e(5)\u003c/p\u003e\n\n则车辆模型离散状态空间方程的矩阵A、B分别为：\n\n$$\nA_k=\\frac{\\partial f}{\\partial \\vec{X_k}}=\\begin{pmatrix} \\frac{\\partial f}{\\partial x} \u0026 \\frac{\\partial f}{\\partial y} \u0026 \\frac{\\partial f}{\\partial v} \u0026 \\frac{\\partial f}{\\partial \\theta}\\end{pmatrix}\n=\\begin{pmatrix} \\frac{\\partial f_1}{\\partial x} \u0026 \\frac{\\partial f_1}{\\partial y} \u0026 \\frac{\\partial f_1}{\\partial v} \u0026 \\frac{\\partial f_1}{\\partial \\theta}\\\\\n\\frac{\\partial f_2}{\\partial x} \u0026 \\frac{\\partial f_2}{\\partial y} \u0026 \\frac{\\partial f_2}{\\partial v} \u0026 \\frac{\\partial f_2}{\\partial \\theta}\\\\\n\\frac{\\partial f_3}{\\partial x} \u0026 \\frac{\\partial f_3}{\\partial y} \u0026 \\frac{\\partial f_3}{\\partial v} \u0026 \\frac{\\partial f_3}{\\partial \\theta}\\\\\n\\frac{\\partial f_4}{\\partial x} \u0026 \\frac{\\partial f_4}{\\partial y} \u0026 \\frac{\\partial f_4}{\\partial v} \u0026 \\frac{\\partial f_4}{\\partial \\theta}\n\\end{pmatrix}\n=\\begin{pmatrix}\n1 \u0026 0 \u0026 \\cos \\theta_k \\cdot \\Delta t \u0026 -\\sin \\theta_k \\cdot (v_k \\cdot \\Delta t + \\frac{1}{2} v_k \\cdot \\Delta t^2)\\\\\n0 \u0026 1 \u0026 \\sin \\theta_k \\cdot \\Delta t \u0026 \\cos \\theta_k \\cdot (v_k \\cdot \\Delta t + \\frac{1}{2} v_k \\cdot \\Delta t^2)\\\\\n0 \u0026 0 \u0026 1 \u0026 0\\\\\n0 \u0026 0 \u0026 0 \u0026 1\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(6)\u003c/p\u003e\n\n$$\nB_k=\\frac{\\partial f}{\\partial \\vec{u_k}}=\n\\begin{pmatrix}\n\\frac{\\partial f_1}{\\partial \\dot{v_k}} \u0026 \\frac{\\partial f_1}{\\partial \\dot{\\theta_k}}\\\\\n\\frac{\\partial f_2}{\\partial \\dot{v_k}} \u0026 \\frac{\\partial f_2}{\\partial \\dot{\\theta_k}}\\\\\n\\frac{\\partial f_3}{\\partial \\dot{v_k}} \u0026 \\frac{\\partial f_3}{\\partial \\dot{\\theta_k}}\\\\\n\\frac{\\partial f_4}{\\partial \\dot{v_k}} \u0026 \\frac{\\partial f_4}{\\partial \\dot{\\theta_k}}\\\\\n\\end{pmatrix}\n=\\begin{pmatrix}\n\\frac{1}{2}\\cos \\theta_k \\cdot \\Delta t^2 \u0026 0\\\\\n\\frac{1}{2}\\sin \\theta_k \\cdot \\Delta t^2 \u0026 0\\\\\n\\Delta t \u0026 0\\\\\n0 \u0026 \\Delta t\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(7)\u003c/p\u003e\n\n## 2 Cost Function\n### 2.1 原始问题\n该运动规划过程原始的最优化问题为\n\n$$\nJ=\\frac{1}{2} (\\vec{X_N}-\\vec{X^r_N})^T S (\\vec{X_N}-\\vec{X^r_N})+ \\sum_{k=1}^{N-1} \\Big[ \\frac{1}{2}(\\vec{X_k}-\\vec{X^r_k})^T Q (\\vec{X_k}-\\vec{X^r_k}) +\\frac{1}{2} \\vec{u_k}^T R \\vec{u_k} \\Big]\n$$\n$$\ns.t \\begin{cases}\na_{low} \\leq \n[\\dot v \u0026 \\dot \\theta]\n\\begin{bmatrix} \n1 \\\\ \n0 \n\\end{bmatrix} \n\\leq a_{high} \\\\\n\\frac{v \\cdot \\tan \\delta_{min}}{L} \\leq\n[\\dot v \u0026 \\dot \\theta]\n\\begin{bmatrix} \n1 \\\\ \n0 \n\\end{bmatrix}\n\\leq \\frac{v \\cdot \\tan \\delta_{max}}{L}\\\\\n1-\\vec{X^O_ {front}}^T P \\vec{X^O _ {front}} \u003c 0\\\\\n1-\\vec{X^O_ {rear}}^T P \\vec{X^O_{rear}} \u003c 0\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(8)\u003c/p\u003e\n\n### 2.2 约束转换为障碍函数\n#### 2.2.1 避障约束\n**(1)避障约束函数推导**\n\n避障场景中，将主车近似为两个圆，障碍物考虑其速度，近似为椭圆，如图1所示。\n\n\u003cdiv align=center\u003e \u003cimg src=\"https://github.com/saxijing/cilqr/blob/main/data/display_materials/Figures/ego_obs_approx.jpg\" width=800\u003e \u003c/div\u003e\n\n\u003cp align=\"center\"\u003e图1 避障约束近似示意图\u003c/p\u003e\n\n\n障碍物长轴：\n\n$$\na=\\frac{1}{2}l_{obs} + v_{obs} \\cdot t_{safe} \\cdot \\cos \\theta_{obs} + s_{safe \\_a} + r_{ego}\n$$\n\n\u003cp align=\"right\"\u003e(9)\u003c/p\u003e\n\n障碍物短轴：\n\n$$\nb=\\frac{1}{2}l_{obs} + v_{obs} \\cdot t_{safe} \\cdot \\sin \\theta_{obs} + s_{safe \\_b} + r_{ego}\n$$\n\n\u003cp align=\"right\"\u003e(10)\u003c/p\u003e\n\n则避障约束中的矩阵P为\n\n$$\nP=\n\\begin{pmatrix}\n\\frac{1}{a^2} \u0026 0 \u0026 0 \u0026 0\\\\\n0 \u0026 \\frac{1}{b^2} \u0026 0 \u0026 0\\\\\n0 \u0026 0 \u0026 0 \u0026 0\\\\\n0 \u0026 0 \u0026 0 \u0026 0\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(11)\u003c/p\u003e\n\n主车两个近似圆圆心的位置为：\n\n$$\n\\vec{X} _ {front}=\\begin{pmatrix}\nx_{front} \\\\\ny_{front} \\\\\nv_{front} \\\\\n\\theta_{front}\n\\end{pmatrix}\n=\\begin{pmatrix}\nx_{ego} + l_f \\cdot \\cos \\theta_{ego} \\\\\ny_{ego} + l_f \\cdot \\sin \\theta_{ego} \\\\\nv_{ego}\n\\theta_{ego} \\\\\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(12)\u003c/p\u003e\n\n$$\n\\vec{X} _ {rear}=\\begin{pmatrix}\nx_{rear} \\\\\ny_{rear} \\\\\nv_{rear} \\\\\n\\theta_{rear}\n\\end{pmatrix}\n=\\begin{pmatrix}\nx_{ego} - l_r \\cdot \\cos \\theta_{ego} \\\\\ny_{ego} - l_r \\cdot \\sin \\theta_{ego} \\\\\nv_{ego} \\\\\n\\theta_{ego}\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(13)\u003c/p\u003e\n\n则前后两个近似圆圆心处对主车状态求导如下：\n\n$$\n\\frac{\\partial \\vec{X}_{front}}{\\partial \\vec{X}}=\\begin{pmatrix}\n\\frac{\\partial f_1}{\\partial \\vec{X}} \\\\\n\\frac{\\partial f_2}{\\partial \\vec{X}} \\\\\n\\frac{\\partial f_3}{\\partial \\vec{X}} \\\\\n\\frac{\\partial f_4}{\\partial \\vec{X}}\n\\end{pmatrix}\n=\\begin{pmatrix}\n\\frac{\\partial f_1}{\\partial \\vec{x_1}} \u0026 \\frac{\\partial f_1}{\\partial \\vec{x_2}} \u0026 \\frac{\\partial f_1}{\\partial \\vec{x_3}} \u0026 \\frac{\\partial f_1}{\\partial \\vec{x_4}} \\\\\n\\frac{\\partial f_2}{\\partial \\vec{x_1}} \u0026 \\frac{\\partial f_2}{\\partial \\vec{x_2}} \u0026 \\frac{\\partial f_2}{\\partial \\vec{x_3}} \u0026 \\frac{\\partial f_2}{\\partial \\vec{x_4}} \\\\\n\\frac{\\partial f_3}{\\partial \\vec{x_1}} \u0026 \\frac{\\partial f_3}{\\partial \\vec{x_2}} \u0026 \\frac{\\partial f_3}{\\partial \\vec{x_3}} \u0026 \\frac{\\partial f_3}{\\partial \\vec{x_4}} \\\\\n\\frac{\\partial f_4}{\\partial \\vec{x_1}} \u0026 \\frac{\\partial f_4}{\\partial \\vec{x_2}} \u0026 \\frac{\\partial f_4}{\\partial \\vec{x_3}} \u0026 \\frac{\\partial f_4}{\\partial \\vec{x_4}}\n\\end{pmatrix}\n=\\begin{pmatrix}\n1 \u0026 0 \u0026 0 \u0026 -l_f \\sin \\theta _{ego} \\\\\n0 \u0026 1 \u0026 0 \u0026 l_f \\cos \\theta _{ego} \\\\\n0 \u0026 0 \u0026 1 \u0026 0 \\\\\n0 \u0026 0 \u0026 0 \u0026 1\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(14)\u003c/p\u003e\n\n同理，\n\n$$\n\\frac{\\partial \\vec{X}_{rear}}{\\partial \\vec{X}}\n=\\begin{pmatrix}\n1 \u0026 0 \u0026 0 \u0026 -l_r \\sin \\theta _{ego} \\\\\n0 \u0026 1 \u0026 0 \u0026 l_r \\cos \\theta _{ego} \\\\\n0 \u0026 0 \u0026 1 \u0026 0 \\\\\n0 \u0026 0 \u0026 0 \u0026 1\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(15)\u003c/p\u003e\n\n分别将前后近似圆的位置坐标转换到障碍物obstacle坐标系下：\n\n$$\n\\vec{X}^O_{front}=T \\cdot (\\vec{X} _ {front} - \\vec{X} _ {obs})\n=\\begin{pmatrix}\n\\cos \\theta _ {obs} \u0026 \\sin \\theta _ {obs} \u0026 0 \u0026 0 \\\\\n-\\sin \\theta _ {obs} \u0026 \\cos \\theta_{obs} \u0026 0 \u0026 0 \\\\\n0 \u0026 0 \u0026 0 \u0026 0 \\\\\n0 \u0026 0 \u0026 0 \u0026 0\n\\end{pmatrix}\n\\begin{pmatrix}\nx_{front} - x_{obs} \\\\\ny_{front} - y_{obs} \\\\\n0-v_{obs} \\\\\n0-\\theta_{obs}\n\\end{pmatrix}\n=\\begin{pmatrix}\n(x_{front}-x_{obs}) \\cdot \\cos \\theta_{obs} + (y_{front}-y_{obs}) \\cdot \\sin \\theta_{obs} \\\\\n-(x_{front}-x_{obs}) \\cdot \\sin \\theta_{obs} + (y_{front}-y_{obs}) \\cdot \\cos \\theta_{obs} \\\\\n0 \\\\\n0\n\\end{pmatrix}\n$$\n\n\u003cp align=\"right\"\u003e(16)\u003c/p\u003e\n\n同理，\n\n$$\n\\vec{X}^O_{rear}=T \\cdot (\\vec{X}_{rear}-\\vec{X} _{obs})\n$$\n\n\u003cp align=\"right\"\u003e(17)\u003c/p\u003e\n\n将式(8)中的第三四个不等式约束对状态量求导\n\n$$\n\\begin{cases}\n  C_f(\\vec{X}) =1 - \\vec{X^O_{front}}^T \\cdot P \\cdot \\vec{X^O} _ {front} \u003c0  \\\\\n  C_r(\\vec{X}) =1 - \\vec{X^O _ {rear}}^T \\cdot P \\cdot \\vec{X^O}_{rear} \u003c0\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(18)\u003c/p\u003e\n\n则\n\n$$\n\\frac{\\partial C_f}{\\partial \\vec{X}}=\\frac{\\partial \\vec{C_f}}{\\partial \\vec{X}^O_{front}} \\cdot \\frac{\\partial \\vec{X}^O_{front}}{\\partial \\vec{X}_{front}} \\cdot \\frac{\\partial \\vec{X} _{front}}{\\partial \\vec{X}} = -2 \\vec{X^O _{front}}^T \\cdot P \\cdot T \\cdot \\frac{\\partial \\vec{X} _{front}}{\\partial \\vec{X}}\n$$\n\n\u003cp align=\"right\"\u003e(19)\u003c/p\u003e\n\n$$\n\\frac{\\partial C_r}{\\partial \\vec{X}}=\\frac{\\partial \\vec{C_r}}{\\partial \\vec{X}^O_{rear}} \\cdot \\frac{\\partial \\vec{X}^O_{rear}}{\\partial \\vec{X}_{rear}} \\cdot \\frac{\\partial \\vec{X} _{rear}}{\\partial \\vec{X}} = -2 \\vec{X^O _{rear}}^T \\cdot P \\cdot T \\cdot \\frac{\\partial \\vec{X} _{rear}}{\\partial \\vec{X}}\n$$\n\n\u003cp align=\"right\"\u003e(20)\u003c/p\u003e\n\n将式(11)(14)(16)代入式(19), 式(11)(15)(17)代入式(20)，即可求出避障约束对状态向量的一阶导。\n\n**(2) 避障约束函数线性化**\n在Xk处对上述 $C_f,C_r$ 函数进行泰勒一阶展开，将约束函数线性化：\n\n$$\nC_f(\\vec{X}_k + \\delta \\vec{X}_k) = C_f(\\vec{X}_k) + \\frac{\\partial C_f}{\\partial \\vec{X}} \\Big| _{\\vec{X}=\\vec{X}_k} \\cdot \\delta \\vec{X}_k\n$$\n\n\u003cp align=\"right\"\u003e(21)\u003c/p\u003e\n\n$$\n\\delta C_f(\\vec{X}_k)=\\frac{\\partial C_f}{\\partial \\vec{X}} \\Big| _{\\vec{X}_k} \\cdot \\delta \\vec{X}_k\n$$\n\n\u003cp align=\"right\"\u003e(22)\u003c/p\u003e\n\n同理，\n\n$$\n\\delta C_r(\\vec{X}_k)=\\frac{\\partial C_r}{\\partial \\vec{X}} \\Big| _{\\vec{X}_k} \\cdot \\delta \\vec{X}_k\n$$\n\n\u003cp align=\"right\"\u003e(23)\u003c/p\u003e\n\n将线性化后的避障约束函数写为\n\n$$\n\\begin{cases}\nf_{Cf}(\\vec{X})=\\frac{\\partial C_f}{\\partial \\vec{X}} \\Big| _ {\\vec{X}_ k} \\cdot \\vec{X} \\\\\nf_{Cr}(\\vec{X})=\\frac{\\partial C_r}{\\partial \\vec{X}} \\Big| _{\\vec{X}_k} \\cdot \\vec{X}\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(24)\u003c/p\u003e\n\n**(3) 避障约束转换为barrier function**\n\n为了将约束问题转化为无约束问题，将式(8)(24)中的避障约束不等式转换为障碍函数(barrier function)添加到目标函数中，前后两个近似圆对应的障碍函数分别为\n\n$$\n\\begin{cases}\nb_f(\\vec{X})=q_1 e^{q_2 f_{Cf}(\\vec{X})} \\\\\nb_r(\\vec{X})=q_1 e^{q_2 f_{Cr}(\\vec{X})}\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(25)\u003c/p\u003e\n\n上述barrier function对状态向量的一阶导为\n\n$$\n\\begin{cases}\n\\frac{\\partial b_f(\\vec{X})}{\\partial \\vec{X}}=q_1 q_2e^{q_2 f_{Cf}(\\vec{X})} \\cdot \\frac{\\partial f_{Cf}(\\vec{X})}{\\partial \\vec{X}}= q_1 q_2 e^{q_2 f_{Cf}(\\vec{X})} \\cdot \\frac{\\partial C_f}{\\partial \\vec{X}} \\Big| _ {\\vec{X}_ k} \\\\\n\\frac{\\partial b_r(\\vec{X})}{\\partial \\vec{X}}=q_1 q_2e^{q_2 f_{Cr}(\\vec{X})} \\cdot \\frac{\\partial f_{Cr}(\\vec{X})}{\\partial \\vec{X}}= q_1 q_2 e^{q_2 f_{Cr}(\\vec{X})} \\cdot \\frac{\\partial C_r}{\\partial \\vec{X}} \\Big| _{\\vec{X}_k}\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(26)\u003c/p\u003e\n\n将式(18)(19)(20)(24)代入式(26)可求出避障约束barrier function对状态向量的一阶导。\n\n由于已经对避障约束函数进行了线性化，所以避障函数 $C_f,C_r$ 对状态向量的二阶导为0，即\n\n$$\n\\begin{cases}\n\\frac{\\partial ^2 {C_f}}{\\partial \\vec{X}^2} = 0 \\\\\n\\frac{\\partial ^2 {C_r}}{\\partial \\vec{X}^2} = 0\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(27)\u003c/p\u003e\n\n所以避障约束barrier function对状态向量的二阶导为\n\n$$\n\\begin{cases}\n\\frac{\\partial ^2 {b_f}(\\vec{X})}{\\partial \\vec{X}^2} = q_1 q_2^2 e ^{q_2 f_{Cf}(\\vec{X})} \\cdot \\big( \\frac{\\partial C_f}{\\partial \\vec{X}} \\big|_ {\\vec{X}_ k} \\big)^T \\cdot \\big( \\frac{\\partial C_f}{\\partial \\vec{X}} \\big|_ {\\vec{X}_ k} \\big) \\\\\n\\frac{\\partial ^2 {b_r}(\\vec{X})}{\\partial \\vec{X}^2} = q_1 q_2^2 e ^{q_2 f_{Cr}(\\vec{X})} \\cdot \\big( \\frac{\\partial C_r}{\\partial \\vec{X}} \\big|_ {\\vec{X}_ k} \\big)^T \\cdot \\big( \\frac{\\partial C_r}{\\partial \\vec{X}} \\big|_{\\vec{X}_k} \\big)\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(28)\u003c/p\u003e\n\n将式(18)(19)(20)(24)代入式(28)中，即可求出barrier function的二阶导。\n\n上述所求的barrier function的一二阶导会在使用iLQR方法进行迭代求解时用到。\n\n### 2.2 控制量约束\n本节的主要目的是将式(8)中的第一二个不等式约束转换为障碍函数形式作为目标函数的一部分，将约束问题转换为无约束问题。\n\n由于控制约束本身就是线性的，因此无需进行线性化近似，直接转换为指数形式的障碍函数(barrier function)。首先将控制约束拆解为以下四个不等式：\n\n$$\n\\begin{cases}\nC_1(\\vec{u})=\\dot{v} - a_{high} \\leq 0 \\\\\nC_2(\\vec{u}) = a_{low} -\\dot{v} \\leq 0 \\\\\nC_3(\\vec{u}) = \\dot \\theta - \\frac{v \\cdot \\tan \\delta _{max}}{L} \\leq 0 \\\\\nC_4(\\vec{u}) =\\frac{v \\cdot \\tan \\delta _{min}}{L} -\\dot \\theta \\leq 0\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(29)\u003c/p\u003e\n\n再将上述四个控制约束函数转换为障碍函数：\n\n$$\n\\begin{cases}\nb_1(\\vec{u})= q_1 e^{q_2 C_1(\\vec{u})} \\\\\nb_2(\\vec{u})= q_1 e^{q_2 C_2(\\vec{u})} \\\\\nb_3(\\vec{u})= q_1 e^{q_2 C_3(\\vec{u})} \\\\\nb_4(\\vec{u})= q_1 e^{q_2 C_4(\\vec{u})} \\\\\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(30)\u003c/p\u003e\n\n控制约束障碍函数的一阶导为\n\n$$\n\\begin{cases}\n\\frac{\\partial b_1(\\vec{u})}{\\partial \\vec{u}} = q_1 q_2 e^{q_2 C_1(\\vec{u})} \\cdot \\frac{\\partial C_1{\\vec{u}}}{\\partial \\vec{u}} \\\\\n\\frac{\\partial b_2(\\vec{u})}{\\partial \\vec{u}} = q_1 q_2 e^{q_2 C_2(\\vec{u})} \\cdot \\frac{\\partial C_2{\\vec{u}}}{\\partial \\vec{u}} \\\\\n\\frac{\\partial b_3(\\vec{u})}{\\partial \\vec{u}} = q_1 q_2 e^{q_2 C_3(\\vec{u})} \\cdot \\frac{\\partial C_3{\\vec{u}}}{\\partial \\vec{u}} \\\\\n\\frac{\\partial b_4(\\vec{u})}{\\partial \\vec{u}} = q_1 q_2 e^{q_2 C_4(\\vec{u})} \\cdot \\frac{\\partial C_4{\\vec{u}}}{\\partial \\vec{u}}\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(31)\u003c/p\u003e\n\n其中，\n\n$$\n\\begin{cases}\n\\frac{\\partial C_1(\\vec{u})}{\\partial \\vec{u}} = \\begin{pmatrix} \\frac{\\partial C_1(\\vec{u})}{\\partial u_1} \u0026 \\frac{\\partial C_1(\\vec{u})}{\\partial u_2} \\end{pmatrix} = \\begin{pmatrix} 1 \u0026 0 \\end{pmatrix} \\\\\n\\frac{\\partial C_2(\\vec{u})}{\\partial \\vec{u}} = \\begin{pmatrix} \\frac{\\partial C_2(\\vec{u})}{\\partial u_1} \u0026 \\frac{\\partial C_2(\\vec{u})}{\\partial u_2} \\end{pmatrix} =\\begin{pmatrix} -1 \u0026 0 \\end{pmatrix} \\\\\n\\frac{\\partial C_3(\\vec{u})}{\\partial \\vec{u}} = \\begin{pmatrix} \\frac{\\partial C_3(\\vec{u})}{\\partial u_1} \u0026 \\frac{\\partial C_3(\\vec{u})}{\\partial u_2} \\end{pmatrix} =\\begin{pmatrix} 0 \u0026 1 \\end{pmatrix} \\\\\n\\frac{\\partial C_4(\\vec{u})}{\\partial \\vec{u}} = \\begin{pmatrix} \\frac{\\partial C_4(\\vec{u})}{\\partial u_1} \u0026 \\frac{\\partial C_4(\\vec{u})}{\\partial u_2} \\end{pmatrix} =\\begin{pmatrix} 0 \u0026 -1 \\end{pmatrix} \\\\\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(32)\u003c/p\u003e\n\n将式(29)(32)代入式(31), 求出控制约束障碍函数的一阶导。\n\n控制约束障碍函数的二阶导为\n\n$$\n\\begin{cases}\n\\frac{\\partial ^2 b_1(\\vec{u})}{\\partial \\vec{u}^2} = q_1 q_2^2 e ^{q_2 C_1(\\vec{u})} \\cdot \\Big(\\frac{\\partial C_1(\\vec{u})}{\\partial \\vec{u}} \\Big)^T \\cdot \\Big( \\frac{\\partial C_1(\\vec{u})}{\\partial \\vec{u}} \\Big) \\\\\n\\frac{\\partial ^2 b_2(\\vec{u})}{\\partial \\vec{u}^2} = q_1 q_2^2 e ^{q_2 C_2(\\vec{u})} \\cdot \\Big(\\frac{\\partial C_2(\\vec{u})}{\\partial \\vec{u}} \\Big)^T \\cdot \\Big( \\frac{\\partial C_2(\\vec{u})}{\\partial \\vec{u}} \\Big) \\\\\n\\frac{\\partial ^2 b_3(\\vec{u})}{\\partial \\vec{u}^2} = q_1 q_2^2 e ^{q_2 C_3(\\vec{u})} \\cdot \\Big(\\frac{\\partial C_3(\\vec{u})}{\\partial \\vec{u}} \\Big)^T \\cdot \\Big( \\frac{\\partial C_3(\\vec{u})}{\\partial \\vec{u}} \\Big) \\\\\n\\frac{\\partial ^2 b_4(\\vec{u})}{\\partial \\vec{u}^2} = q_1 q_2^2 e ^{q_2 C_4(\\vec{u})} \\cdot \\Big(\\frac{\\partial C_4(\\vec{u})}{\\partial \\vec{u}} \\Big)^T \\cdot \\Big( \\frac{\\partial C_4(\\vec{u})}{\\partial \\vec{u}} \\Big)\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(33)\u003c/p\u003e\n\n将式(29)(32)代入式(33)可得到控制约束障碍函数的二阶导，在后续使用iLQR方法迭代求导时会用到。\n\n### 2.3 最终问题\n\n经过上述处理，该运动规划问题由2.1节的约束优化问题转换为式(34)所示的无约束优化问题。\n\n$$\nJ=\\frac{1}{2} \\Big( \\vec{X}_ N - \\vec{X}^r _ N \\Big) ^T S \\Big(\\vec{X}_ N - \\vec{X} ^ r_N \\Big) + \\sum_{k=1}^{N-1} \\Big[ \\frac{1}{2} {\\Big( \\vec{X}_ k - \\vec{X} _ k ^ r)} ^ T Q ( {\\vec{X}_ k - \\vec{X}^r_k} \\Big) + \\frac{1}{2} \\vec{u}_ k^T R \\vec{u} _ k + \\sum_{m=0} ^{M} \\Big( b _{fm} (\\vec{X}_k) + b _{rm} (\\vec{X}_k) \\Big) + b_1(\\vec{u}_k) + b_2(\\vec{u}_k) + b_3(\\vec{u}_k) + b_4(\\vec{u}_k)\n \\Big]\n$$\n\n\u003cp align=\"right\"\u003e(34)\u003c/p\u003e\n\n其中M为该时刻主车周围障碍物总数， $b_{fm}$ , $b_{rm}$ 分别为主车两个近似圆的避障障碍函数， $b_1, b_2, b_3, b_4$ 分别为控制约束障碍函数。目标函数对状态向量和控制向量的一阶导、二阶导求法均已在2.2节给出。以下给出使用iLQR方法求解问题的过程。\n\n## 3 Backward Pass\n\n### 3.1 代价函数处理\n\n根据LQR先验知识，代价函数J由末端代价和过程代价两部分组成，根据式(8)(34), 可将代价函数写为\n\n$$\nJ=\\ell _f(\\vec{X}_N) + \\sum _{k=1}^{N-1} \\ell (\\vec{X}_k, \\vec{u}_k)\n$$\n\n\u003cp align=\"right\"\u003e(35)\u003c/p\u003e\n\n### 3.2 Q函数与贝尔曼方程\n\nwhen $k=N$, cost to go function \n\n$$ V_N= \\ell _f(\\vec{X}_N) = \\frac{1}{2} \\Big( \\vec{X}_N - \\vec{X}_N^r \\Big)^T S \\Big( \\vec{X}_N - \\vec{X}_N^r \\Big) $$\n\n\u003cp align=\"right\"\u003e(36)\u003c/p\u003e\n\nwhen $k \\neq N$, cost to go function\n\n$$\nV_k = min \\\\{ \\ell (\\vec{X}_ k, \\vec{u}_ k) + V_{k+1} \\big( f(\\vec{X}_k, \\vec{u}_k) \\big) \\\\}\n$$\n\n\u003cp align=\"right\"\u003e(37)\u003c/p\u003e\n\n令\n\n$$\nQ_k(\\vec{X}_k, \\vec{u}_k) = \\ell (\\vec{X}_k, \\vec{u} _ k) + V _{k+1} \\big[ f(\\vec{X}_k, \\vec{u}_k) \\big]\n$$\n\n\u003cp align=\"right\"\u003e(38)\u003c/p\u003e\n\n在 $(\\vec{X}_k, \\vec{u}_k)$ 处进行二阶泰勒展开：\n\n$$\nQ(\\vec{X}_k + \\delta \\vec{X}_k, \\vec{u}_k + \\delta \\vec{u}_k) \n= Q(\\vec{X}_k, \\vec{u}_k) + \\frac{\\partial Q}{\\partial \\vec{X}} \\Big| _ {(\\vec{X}_k, \\vec{u}_k)} \\cdot \\delta \\vec{X}_k+ \n\\frac{\\partial Q}{\\partial \\vec{u}} \\Big| _ {(\\vec{X}_k, \\vec{u}_k)} \\cdot \\delta \\vec{u}_k +\n\\frac{1}{2} {(\\delta \\vec{X}_k)}^T \\cdot \\frac{\\partial ^2 Q}{\\partial \\vec{X}^2} \\bigg| _ {(\\vec{X}_k, \\vec{u}_k)} (\\delta \\vec{X}_k) +\n\\frac{1}{2} {(\\delta \\vec{u}_k)}^T \\cdot \\frac{\\partial ^2 Q}{\\partial \\vec{u}^2} \\bigg| _ {(\\vec{X}_k, \\vec{u}_k)} (\\delta \\vec{u}_k) + \n\\frac{1}{2} {(\\delta \\vec{X}_k)}^T \\cdot \\frac{\\partial ^2 Q}{\\partial \\vec{u} \\partial \\vec{X}} \\bigg| _ {(\\vec{X}_k, \\vec{u}_k)} (\\delta \\vec{u}_k) +\n\\frac{1}{2} {(\\delta \\vec{u}_k)}^T \\cdot \\frac{\\partial ^2 Q}{\\partial \\vec{X} \\partial \\vec{u}} \\bigg| _ {(\\vec{X}_k, \\vec{u}_k)} (\\delta \\vec{X}_k)\n$$\n\n\u003cp align=\"right\"\u003e(39)\u003c/p\u003e\n\n则\n\n$$\n\\begin{aligned}\n\\delta Q(\\vec{X}_k, \\vec{u}_k) \u0026= Q_x \\cdot \\delta \\vec{X}_k +Q_u \\cdot \\delta \\vec{u}_k + \\frac{1}{2}(\\delta \\vec{X}_k)^T Q _{xx} (\\delta \\vec{X}_k) +\n\\frac{1}{2}(\\delta \\vec{u}_k)^T Q _{uu} (\\delta \\vec{u}_k) + \\frac{1}{2}(\\delta \\vec{X}_k)^T Q _{ux} (\\delta \\vec{u}_k) +\n\\frac{1}{2}(\\delta \\vec{u}_k)^T Q _{xu} (\\delta \\vec{X}_k) \\\\\\\n\u0026=\\begin{bmatrix} Q_x \u0026 Q_u \\end{bmatrix} \\begin{bmatrix} \\delta \\vec{X}_k \\\\\\ \\delta \\vec{u}_k \\end{bmatrix} + \n\\frac{1}{2} \\begin{bmatrix} \\delta \\vec{X}_k \\\\\\ \\delta \\vec{u}_k \\end{bmatrix}^T \\begin{bmatrix} Q _{xx} \u0026 Q _{ux} \\\\\\ Q _{xu} \u0026 Q _{uu}\\end{bmatrix}\n\\begin{bmatrix} \\delta \\vec{X}_k \\\\\\ \\delta \\vec{u}_k \\end{bmatrix} \u0026\n\\end{aligned}\n$$\n\n\u003cp align=\"right\"\u003e(40)\u003c/p\u003e\n\n其中\n\n$$\nQ_X = \\frac{\\partial \\ell}{\\partial \\vec{X}} \\Big| _ {\\vec{X}_k, \\vec{u}_k} + \\frac{\\partial V _{k+1}}{\\partial \\vec{X} _{k+1}} \\cdot \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{X} _{k}}\n$$\n\n\u003cp align=\"right\"\u003e(41)\u003c/p\u003e\n\n$$\nQ_u = \\frac{\\partial \\ell}{\\partial \\vec{u}} \\Big| _ {\\vec{X}_k, \\vec{u}_k} + \\frac{\\partial V _{k+1}}{\\partial \\vec{X} _{k+1}} \\cdot \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{u} _{k}}\n$$\n\n\u003cp align=\"right\"\u003e(42)\u003c/p\u003e\n\n$$\nQ_{XX} = \\frac{\\partial Q_X}{\\partial \\vec{X}} = \\frac{\\partial ^2 \\ell}{\\partial \\vec{X}^2} \\Big| _{\\vec{X}_k, \\vec{u}_k} + \\frac{\\partial ^2 V _{k+1}}{\\partial \\vec{X} _{k+1} \\partial \\vec{X}_k} \\cdot \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{X} _{k}}\n=\\frac{\\partial ^2 \\ell}{\\partial \\vec{X}^2} \\Bigg| _{\\vec{X}_k, \\vec{u}_k} + \\Big( \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{X}_k} \\Big)^T \\cdot \\frac{\\partial ^2 V _{k+1}}{\\partial \\vec{X} _{k+1} ^2} \\cdot \\big( \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{X}_k} \\big)\n$$\n\n\u003cp align=\"right\"\u003e(43)\u003c/p\u003e\n\n$$\nQ_{uu} = \\frac{\\partial Q_u}{\\partial \\vec{u}} = \\frac{\\partial ^2 \\ell}{\\partial \\vec{u}^2} \\Big| _{\\vec{X}_k, \\vec{u}_k} + \\frac{\\partial ^2 V _{k+1}}{\\partial \\vec{X} _{k+1} \\partial \\vec{u}_k} \\cdot \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{u} _{k}}\n=\\frac{\\partial ^2 \\ell}{\\partial \\vec{u}^2} \\Bigg| _{\\vec{X}_k, \\vec{u}_k} + \\Big( \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{u}_k} \\Big)^T \\cdot \\frac{\\partial ^2 V _{k+1}}{\\partial \\vec{X} _{k+1} ^2} \\cdot \\big( \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{u}_k} \\big)\n$$\n\n\u003cp align=\"right\"\u003e(44)\u003c/p\u003e\n\n$$\nQ_{Xu} = \\frac{\\partial Q_X}{\\partial \\vec{u}} = \\frac{\\partial ^2 \\ell}{\\partial \\vec{X} \\partial \\vec{u}} \\Big| _{\\vec{X}_k, \\vec{u}_k} + \\frac{\\partial ^2 V _{k+1}}{\\partial \\vec{X} _{k+1} \\partial \\vec{u}_k} \\cdot \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{X} _{k}}\n=\\frac{\\partial ^2 \\ell}{\\partial \\vec{X} \\partial \\vec{u}} \\Bigg| _{\\vec{X}_k, \\vec{u}_k} + \\Big( \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{u}_k} \\Big)^T \\cdot \\frac{\\partial ^2 V _{k+1}}{\\partial \\vec{X} _{k+1} ^2} \\cdot \\big( \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{X}_k} \\big)\n$$\n\n\u003cp align=\"right\"\u003e(45)\u003c/p\u003e\n\n$$\nQ_{uX}=Q_{Xu}^T\n$$\n\n\u003cp align=\"right\"\u003e(46)\u003c/p\u003e\n\n其中， $\\quad \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{X} _{k}} =A_k \\quad $, 即式(6);  $\\quad \\frac{\\partial \\vec{X} _{k+1}}{\\partial \\vec{u} _{k}} =B_k \\quad $, 即式(7)。\n\n要求使得 $\\delta Q$ 最小的 $\\delta \\vec{u}$ ，即是要求令 $\\frac{\\partial (\\delta Q)}{\\partial (\\delta \\vec{u})}=0$ 的最优控制量 $\\delta \\vec{u}^*$ 。\n\n$$\n\\frac{\\partial (\\delta Q)}{\\partial (\\delta \\vec{u})} = Q_u+(\\delta \\vec{u})^T Q _{uu} + (\\delta \\vec{X})^T Q _{uX} = 0\n$$\n\n\u003cp align=\"right\"\u003e(47)\u003c/p\u003e\n\n求出\n\n$$\n\\delta \\vec{u}_ k^* = -(Q_{uu}^{-1})^T \\big[ Q_u^T + Q_{Xu} \\cdot (\\delta \\vec{X}_k) \\big]\n$$\n\n\u003cp align=\"right\"\u003e(48)\u003c/p\u003e\n\n令\n\n$$\n\\begin{cases}\nK=-(Q_{uu}^{-1})^T \\cdot Q_{Xu} \\\\\nd= -(Q_{uu}^{-1})^T \\cdot Q_u^T\n\\end{cases}\n$$\n\n\u003cp align=\"right\"\u003e(49)\u003c/p\u003e\n\n则 $\\delta \\vec{u}_k^*$ 可写为\n\n$$\n\\delta \\vec{u}_k^* = K \\cdot \\delta \\vec{X}_k +d\n$$\n\u003cp align=\"right\"\u003e(50)\u003c/p\u003e\n\n由于 $V_k = min \\\\{ Q_k(\\vec{X}_k, \\vec{u}_k) \\\\}$ , 所以 $\\delta V_k = min \\\\{ \\delta Q_k(\\vec{X}_k, \\vec{u}_k) \\\\}$ ,将式(48)代入式(40)求出 $\\delta Q _ {min}$ , 即 $\\delta V$ :\n\n$$\n\\begin{aligned}\n\\delta V \u0026= min \\\\{ \\delta Q_k(\\vec{X} _ k, \\vec{u} _ k) \\\\} \\\\\n\u0026= Q_X \\cdot \\delta \\vec{X} _ k + Q _ u \\cdot \\delta \\vec{u} _ k ^ * + \\frac{1}{2} (\\delta \\vec{X} _ k)^T Q_{XX} (\\delta \\vec{X} _ k) + \\frac{1}{2} (\\delta \\vec{u} _ k ^ * )^T Q_{uu} (\\delta \\vec{u} _ k ^ * ) + \\frac{1}{2} (\\delta \\vec{X} _ k)^T Q_{uX} (\\delta \\vec{u} _ k ^ *) + \\frac{1}{2} (\\delta \\vec{u} _ k ^ *)^T Q_{Xu} (\\delta \\vec{X}_ k) \\\\ \n\u0026= Q_X \\cdot \\delta \\vec{X}_ k + Q_u(K \\cdot \\delta \\vec{X}_ k +d) + \\frac{1}{2} (\\delta \\vec{X} _ k)^T Q_{XX} (\\delta \\vec{X} _ k) + \\frac{1}{2} (K \\cdot \\delta \\vec{X}_ k +d) ^ T Q_{uu} (K \\cdot \\delta \\vec{X}_ k +d) + \\frac{1}{2} (\\delta \\vec{X} _ k) ^ T \\cdot Q_{uX} (K \\cdot \\delta \\vec{X}_ k +d) + \\frac{1}{2}(K \\cdot \\delta \\vec{X}_ k +d) ^ T Q_{Xu} (\\delta \\vec{X}_k) \\\\\n\u0026= \\Big( Q_X + Q _u K + d^T Q _{uu} K + d ^ T Q _ {Xu} \\Big) \\delta \\vec{X} _ k + \\frac{1}{2}(\\delta \\vec{X} _ k) ^ T \\big[ Q _ {XX} + K ^ T Q _ {uu} K + Q _ {uX} K + K ^T Q _ {Xu} \\big] (\\delta \\vec{X}_k) + Q_u d + \\frac{1}{2} d^T Q _{uu} d\n\\end{aligned}\n$$\n\n\u003cp align=\"right\"\u003e(51)\u003c/p\u003e\n\n由此推导出式(41)~式(46)中的 $\\frac{\\partial V_{k+1}}{\\partial \\vec{X}_{k+1}}$ , $\\quad \\frac{\\partial ^ 2 V _ {k+1}}{\\partial \\vec{X} _ {k+1} ^ 2}$ 的求法如下：\n\n①当 $k=N$ 时， $\\frac{\\partial V_{k+1}}{\\partial \\vec{X}_{k+1}}, \\quad \\frac{\\partial ^ 2 V _ {k+1}}{\\partial \\vec{X} _ {k+1} ^ 2}=0$ \n\n②当 $k=N-1$ 时， \n\n$$\n\\frac{\\partial V_{k+1}}{\\partial \\vec{X}_{k+1}} = \\frac{\\partial \\ell _f (\\vec{X} _ N)}{\\partial \\vec{X} _ N} = \\big( \\vec{X} _ N - \\vec{X} _ N ^ r \\big) ^ T S\n$$\n\n\u003cp align=\"right\"\u003e(52)\u003c/p\u003e\n\n$$\n\\frac{\\partial ^ 2 V _ {k+1}}{\\partial \\vec{X} _ {k+1} ^ 2} = \\frac{\\partial ^ 2 \\ell _f (\\vec{X} _ N)}{\\partial \\vec{X} _ N ^ 2} =S \n$$\n\n\u003cp align=\"right\"\u003e(53)\u003c/p\u003e\n\n③当 $k \\leq N-2$ 时，说明此时 $V_{N-1}$已经算完。\n\n在 $(\\vec{X}_k, \\vec{u}_k)$ 处对 $V_k$ 进行二阶泰勒展开：\n\n$$\nV_k ( \\vec{X}_k + \\delta \\vec{X}_k, \\quad \\vec{u}_k + \\delta \\vec{u}_k ) = V_k ( \\vec{X}_k, \\vec{u} _ k ) + \\frac{\\partial V}{\\partial \\vec{X}} \\Bigg| _ {(\\vec{X}_k, \\vec{u}_k)} \\cdot \\delta \\vec{X}_k + \\frac{1}{2} (\\delta \\vec{X}_k) ^ T \\cdot \\frac{\\partial ^ 2 V}{\\partial \\vec{X} ^ 2} \\Bigg| _ {(\\vec{X}_k, \\vec{u}_k)} \\cdot (\\delta \\vec{X}_k)\n$$\n\n\u003cp align=\"right\"\u003e(54)\u003c/p\u003e\n\n$$\n\\delta V_k(\\vec{X}_k, \\vec{u} _ k) = \\frac{\\partial V}{\\partial \\vec{X}} \\Bigg| _ {(\\vec{X}_k, \\vec{u}_k)} \\cdot \\delta \\vec{X}_k + \\frac{1}{2} (\\delta \\vec{X}_k) ^ T \\cdot \\frac{\\partial ^ 2 V}{\\partial \\vec{X} ^ 2} \\Bigg| _ {(\\vec{X}_k, \\vec{u}_k)} \\cdot (\\delta \\vec{X}_k)\n$$\n\n\u003cp align=\"right\"\u003e(55)\u003c/p\u003e\n\n结合式(51)与式(55), 可得：\n\n$$\n\\frac{\\partial V}{\\partial \\vec{X}} \\Bigg| _ {(\\vec{X}_k, \\vec{u}_k)} =  Q_X + Q _u K + d^T Q _{uu} K + d ^ T Q _ {Xu}\n$$\n\n\u003cp align=\"right\"\u003e(56)\u003c/p\u003e\n\n$$\n\\frac{\\partial ^ 2 V}{\\partial \\vec{X} ^ 2} \\Bigg| _ {(\\vec{X}_k, \\vec{u}_k)} = Q _ {XX} + K ^ T Q _ {uu} K + Q _ {uX} K + K ^T Q _ {Xu}\n$$\n\n\u003cp align=\"right\"\u003e(57)\u003c/p\u003e\n\n$$\n\\Delta V = \\frac{1}{2} d^T Q _{uu} d + Q_u d\n$$\n\n\u003cp align=\"right\"\u003e(58)\u003c/p\u003e\n\n$\\Delta V$ 为余项。\n\n### 3.3 Backward Pass算法流程总结\n\n\u003cdiv align=center\u003e \u003cimg src=\"https://github.com/saxijing/cilqr/blob/main/data/display_materials/Figures/backward_persudo.jpg\" width=600\u003e \u003c/div\u003e\n\n## 4 Forward Pass\n\nBackward Pass过程计算出了 $0$ ~ $N-1$ 时刻的最优控制量 $\\delta \\vec{u} _ k ^ *$ ， Forward Pass阶段需要根据最优控制量序列、车辆模型及参考轨迹序列计算出一条目标轨迹。过程如下：\n\n令 $\\delta \\vec{X}_k= \\vec{X}_k ^ {new} - \\vec{X} ^ r _ k$\n\n\u003cp align=\"right\"\u003e(59)\u003c/p\u003e\n\n其中， $\\vec{X}_k ^ {new}$ 为本次迭代计算出的轨迹点， $\\vec{X} ^ r _ k$ 为参考轨迹点。\n\n则根据第3部分的计算，\n\n$$\n\\delta \\vec{u}_k ^ {new} = K \\cdot \\delta \\vec{X}_k + \\alpha \\cdot d, \\qquad \\alpha \\in [0,1]\n$$\n\n\u003cp align=\"right\"\u003e(60)\u003c/p\u003e\n\n$$\n\\vec{u}_k ^ {new} = \\vec{u}_k + \\delta \\vec{u}_k\n$$\n\n\u003cp align=\"right\"\u003e(61)\u003c/p\u003e\n\n$$\n\\vec{X}_{k+1} ^ {new} = f(\\vec{X}_k ^ {new}, \\vec{u}_k ^ {new})\n$$\n\n\u003cp align=\"right\"\u003e(62)\u003c/p\u003e\n\n其中， $f(\\vec{\\overline X}_k, \\vec{\\overline u}_k)$ 参考式(1)。由此计算出一系列标称轨迹点。\n\n式(60)中的 $\\alpha$ 为线搜索的迭代步长，用于在Backward Pass获得的解附近搜索最优解，当未达到终止条件时，每次 $\\alpha$ 的更新规则为\n\n$$\n\\alpha = \\gamma \\cdot \\alpha, \\quad usually \\quad \\gamma = 0.5\n$$\n\n\u003cp align=\"right\"\u003e(63)\u003c/p\u003e\n\n当满足终止条件时，更新目标轨迹为新计算出的轨迹 $\\vec{X}_k ^ {new}$ 、控制输入为新计算出的 $\\vec{u}_k ^ {new}$ ,并同步更新路径代价 $J$ 。\n\nForward Pass算法流程总结如下：\n\n\u003cdiv align=center\u003e \u003cimg src=\"https://github.com/saxijing/cilqr/blob/main/data/display_materials/Figures/forward_pass_persudo.jpg\" width=600\u003e \u003c/div\u003e\n\n## 5 Project Architecture\n\n为更好地贴近实车环境，本项目采用C++编程，将仿真场景与运动规划分为两个进程，使用ROS架构实现进程间的通信。同时为了配合控制模块，每帧都进行了规划起点的确定和轨迹拼接、规划与控制采用了不同的计算频率。项目整体架构如下：\n\n\u003cdiv align=center\u003e \u003cimg src=\"https://github.com/saxijing/cilqr/blob/main/data/display_materials/Figures/project_architecture.jpg\" width=800\u003e \u003c/div\u003e\n\n\u003cp align=\"center\"\u003e图2 项目架构\u003c/p\u003e\n\n## 6 Commands\n\n四个测试场景的运行命令如下：\n\n**(1)Following:**\n\nroslaunch launch following_scene.launch\n\nroslaunch launch cilqr_planner_following.launch\n\n**(2)Obstacles Avoidance:**\n\nroslaunch launch multi_obstacles_scene.launch\n\nroslaunch launch cilqr_planner_multi_obstacles.launch\n\n**(3)Lane Change:**\n\nroslaunch launch lane_change_scene.launch\n\nroslaunch launch cilqr_planner_lane_change.launch\n\n**(4)Overtake:**\n\nroslaunch launch overtake_scene.launch\n\nroslaunch launch cilqr_planner_overtake.launch\n","funding_links":[],"categories":[],"sub_categories":[],"project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fsaxijing%2Fcilqr","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fsaxijing%2Fcilqr","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fsaxijing%2Fcilqr/lists"}