PythonRobotics 如何用 LQR 控制小车保持倒立摆平衡【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics这篇文章解决一个具体任务在 PythonRobotics 仓库中运行小车倒立摆inverted pendulum on a cart的 LQR 控制示例验证控制器能否把偏离竖直方向的摆杆拉回并保持平衡。示例位于 inverted_pendulum_lqr_control.py只依赖 README 中列出的 NumPy、SciPy、Matplotlib 环境装好依赖后即可运行。示例背后的模型与控制结构文档页 inverted_pendulum_main.rst 定义了系统质量为M的小车可在水平轨道上移动车上竖直铰接一根长l的杆杆顶有质量m。控制目标是通过给小车施加水平力u来保持摆杆平衡。物理量对应关系如下M小车质量m杆顶质量l杆长u施加在小车上的力x小车位置θ摆杆相对竖直方向的角度文档先用 Lagrange 方程推导出非线性运动方程再在小角度假设下cos θ ≈ 1、sin θ ≈ θ、θ̇² ≈ 0得到线性化模型并写成状态空间形式ẋ Ax Bu状态向量取x [x, ẋ, θ, θ̇]A、B矩阵的表达式见该文档页。LQR 部分控制器最小化代价函数J xᵀQx uᵀRu使代价最小的反馈律为u -Kx其中K (BᵀPB R)⁻¹BᵀPAP是离散代数 Riccati 方程DAREP AᵀPA - AᵀPB(R BᵀPB)⁻¹BᵀPA Q的正定解。示例代码实现了这条完整链路get_model_matrix()按线性化模型构造A、B并用仿真步长delta_t离散化A I delta_t·AB delta_t·Bsolve_DARE()迭代求解P当相邻两次迭代的差值最大分量小于eps时提前停止最多迭代maxiter次dlqr()由P计算增益K并同时返回A - BK的特征值lqr_control()计算u -Kx并打印该步的求解耗时。注意仿真函数simulation()直接用离散化后的矩阵做单步更新x A x B u其中A、B就是上面离散化后的版本。安装依赖并运行README 要求运行各示例代码需要 Python 3.13.x 以及 NumPy、SciPy、Matplotlib、cvxpy。两种安装方式conda env create -f requirements/environment.yml或pip install -r requirements/requirements.txtrequirements.txt 中固定了具体版本numpy 2.3.5、scipy 1.18.1、matplotlib 3.11.0、cvxpy 1.8.1、ecos 2.0.14 等。LQR 示例本身只用到 NumPy 和 Matplotlibcvxpy 是同目录其他示例如 MPC 示例的依赖。在仓库根目录执行脚本python InvertedPendulum/inverted_pendulum_lqr_control.py默认show_animation True会弹出 Matplotlib 窗口实时绘制小车、杆和轮子窗口标题同步显示当前的x和theta单位 deg。代码里注册了按键事件在窗口中按 ESC 键会退出仿真源码注释写明 for stopping simulation with the esc key。模型与仿真参数都集中在文件顶部含义如下参数示例中的取值用途按源码注释l_bar2.0杆长M1.0[kg]小车质量m0.3[kg]杆顶质量g9.8[m/s^2]重力加速度Qnp.diag([0.0, 1.0, 1.0, 0.0])state cost matrixRnp.diag([0.01])input cost matrixdelta_t0.1[s]时间步长用于A、B离散化sim_time5.0[s]仿真总时长初始状态x0为[0.0, 0.0, 0.3, 0.0]小车位于原点摆杆初始偏离竖直方向 0.3 rad。无图形界面时如何运行单元测试 test_inverted_pendulum_lqr_control.py 给出了无窗口跑法的标准做法在调用main()前把show_animation置为Falsefrom InvertedPendulum import inverted_pendulum_lqr_control as m m.show_animation False m.main()你可以直接把源码顶部的show_animation True改成False也可以直接跑仓库里现成的测试python -m pytest tests/test_inverted_pendulum_lqr_control.py如何判断运行结果脚本运行时有两类输出这是示例自带的核对方式每个控制步都会打印一行求解耗时格式固定为数值为运行时实际测量值calc time:… [sec]打印次数对应仿真时间步总数由sim_time与delta_t决定。仿真循环结束后打印收尾两行Finish x… [m] , theta… [deg]第二个数值分别是仿真结束时刻的小车位置与摆杆角度。文档没有给出固定的预期数值核对时应结合控制目标来判断文档明确控制系统的目标是 balance the inverted pendulum即最终theta应回到接近 0竖直方向的量级而不是停留在初始的 0.3 rad。开启动画时也可以直接观察窗口中杆是否保持在竖直附近、小车左右移动的姿态。可调参数与模型适用边界Q、R代价函数中状态误差与控制量的权重矩阵是调整二者相对强弱的入口。调整后可以对照最终的theta数值与每步calc time的变化。delta_t既决定A、B的离散化也决定仿真步长sim_time决定总时长。x0初始状态其中第三项是初始摆角。solve_DARE()的收敛参数maxiter150、eps0.01分别是最多迭代次数和收敛判据相邻两次P的最大差值小于eps即停止。边界线性化模型只在小角度下成立文档中给出的是cos θ ≈ 1、sin θ ≈ θ、θ̇² ≈ 0的近似。示例取 0.3 rad 的初始摆角如果你修改x0或参数使摆杆大幅摆动就超出了该文档页推导适用的小角度假设。下一步完整的 Lagrange 方程推导、线性化过程和A/B/C/D矩阵见 inverted_pendulum_main.rst。同一文档页还给出了对照的 MPC 控制示例 inverted_pendulum_mpc_control.py它最小化同样的代价函数J xᵀQx uᵀRu约束为线性化倒立摆模型与初始状态预测时域T 30依赖 cvxpy。跑通 LQR 后可以用同一组参数运行它比较两种控制器的仿真输出。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考