智能机器人与运动控制- 课件 第13-20讲-线性二次型最优控制- -ROS的EKF定位与SLAM实现

智能机器人与运动控制系统13.线性二次型最优控制课程基本内容线性二次型最优控制有限时域线性二次型最优控制无限时域线性二次型最优控制有限时域含噪声线性二次型最优控制无限时域含噪声线性二次型最优控制线性二次型最优控制线性二次调节（LinearQuadraticRegulator,LRQ)是一种经典的控制方法适用于线性系统在给定性能指标下表现最优常用于：控制工程机器人学飞行器控制线性二次型最优控制考虑一个线性时不变系统：LQR的目标是找到一个状态反馈控制器：线性二次型最优控制LQR的目标是找到一个状态反馈控制器：为了设计最优的控制器增益矩阵𝑲(𝑡)，LQR考虑以下性能指标：线性二次型最优控制性能指标：𝑸是对状态变量的加权矩阵，表示对不同状态的重视程度𝑹是对控制输入的加权矩阵，表示对控制输入的成本的重视程度通过调整𝑸和𝑹的值，我们可以调整系统的状态变量和控制输入对性能指标的影响程度线性二次型最优控制LQR目标：找到一个最优的控制器增益矩阵𝑲∗(𝑡)，使得成本函数𝐽(𝒙,𝒖)最小化LQR的适用范围：线性系统性能指标必须是二次型有限时域线性二次型最优控制实际物理模型中，信息是离散的：传感器采集到的信息控制器发出的信息所以我们考虑为一个离散时间下的直流电机状态空间模型设计LQR控制器：有限时域线性二次型最优控制离散时间下的状态空间模型：离散时间下的控制输入：离散时间下的性能指标：有限时域线性二次型最优控制

有限时域线性二次型最优控制问题建模：有限时域线性二次型最优控制LQR可以使用动态规划的方法求解可以把𝐽(𝒙,𝒖)拆分成𝑡=𝑘时刻之前和𝑡=𝑘时刻之后两个部分。考虑直到时刻𝑁累积的最小代价：有限时域线性二次型最优控制

有限时域线性二次型最优控制

有限时域线性二次型最优控制

有限时域线性二次型最优控制

有限时域线性二次型最优控制

有限时域线性二次型最优控制

有限时域线性二次型最优控制总结离散时间下的有限时域LQR的求解过程：有限时域线性二次型最优控制有限时域主要适用于以下情况：有明确终止时间的任务：比如机器人需要在一定时间内完成一项任务时间依赖的性能指标：系统的性能指标随时间变化或者只在特定时间段内有效模型或目标参数变化：有限时域LQR能够根据每个阶段设定不同的优化目标有限操作环境：只在有限时间内运行或存在的系统无限时域线性二次型最优控制有限时域的LQR在某些时候并不适用：长期或持续操作的系统稳态或周期性任务缺乏明确终止条件的情况系统稳定性要求无限时域线性二次型最优控制因此，我们引入无限时域线性二次型最优控制无限时域线性二次型最优控制的性能指标无限时域线性二次型最优控制

无限时域线性二次型最优控制无限时域LQR可以通过同样的过程得到然而，无限时域的LQR不存在一个终端时刻和终端状态，不能递推，但并非无解，我们可以通过一个定理求解。无限时域线性二次型最优控制无限时域线性二次型最优控制基于上述定理，在无限时域的LQR中，当系统和目标函数满足上述三点条件，离散时间的Riccati差分方程存在且存在唯一的稳态解𝑷求解稳态解𝑷，可以计算当前时刻的最优控制策略无限时域线性二次型最优控制总结离散时间下的无限时域LQR的求解过程有限时域含噪声线性二次型最优控制实际的控制系统中，观测噪声和控制噪声是不可避免，我们要考虑如何为有噪声的控制模型设计最优控制器（LinearQuadraticGaussian,LQG)考虑离散时间带噪声的状态空间模型有限时域含噪声线性二次型最优控制因为状态空间模型引入了作为随机变量的噪声，有限时域的LQG的代价函数以期望的方式表示有限时域含噪声线性二次型最优控制

有限时域含噪声线性二次型最优控制

有限时域含噪声线性二次型最优控制

有限时域含噪声线性二次型最优控制

有限时域含噪声线性二次型最优控制

有限时域含噪声线性二次型最优控制

有限时域含噪声线性二次型最优控制回忆可以得到有限时域含噪声线性二次型最优控制

有限时域含噪声线性二次型最优控制总结离散时间下的有限时域LQG的求解过程：无限时域含噪声线性二次型最优控制有限时域下的LQG控制器可以分为线性二次调节和最优状态估计两部分

LQG控制器推广到无限时域下时，需要分别分析两个部分的稳态情况无限时域含噪声线性二次型最优控制无限时域下LQG线性二次调节的稳态分析与无限时域的LQR控制问题一样假设系统和目标函数满足代数Riccati方程存在唯一正定解的条件，则最优控制系数𝑲最后会趋于一个稳定解𝑷是下方代数Riccati方程的唯一正定解无限时域含噪声线性二次型最优控制

无限时域含噪声线性二次型最优控制

无限时域含噪声线性二次型最优控制

无限时域含噪声线性二次型最优控制总结离散时间下的无限时域LQG的求解过程谢谢大家智能机器人与运动控制系统14.直流电机的位置控制与跟踪直流电机控制系统、直流电机的位置控制、位置跟踪直流电机控制系统、直流电机的位置控制、位置跟踪直流电机的位置控制直流电机的位置跟踪直流电机的位置控制直流电机的位置控制是指通过调节电机的旋转角度，使其达到特定的位置或沿预定轨迹精确移动的一种任务。应用场景：机器人臂、数控机床、卫星定位系统等直流电机的位置控制利用LQG控制器解决直流电机的位置控制问题有以下好处：噪声和扰动的抑制鲁棒性和稳定性优化性能灵活性和可适应性系统整合和实现综上所述，LQG控制为直流电机的位置控制提供了一种结合了优化性能、鲁棒性和噪声处理能力的解决方案直流电机的位置控制

直流电机的位置控制

直流电机的位置控制

直流电机的位置控制

直流电机的位置控制

直流电机的位置控制

直流电机的位置控制

直流电机的位置控制实际情况中，出于控制成本的考虑，电机的控制器的算力并不会很高。对于低算力的控制器，如果用有限时域的LQG方法迭代计算反馈控制律，会严重影响控制器的实时性能。不过，回顾无限时域的LQG算法，我们发现其反馈控制系数是一个固定值，而且可以离线计算。对于低成本的控制器，我们可以考虑采用无限时域的LQG方法来实现电机的位置控制，可以提前离线计算反馈控制系数和Kalman滤波器的相关系数。直流电机的位置控制

直流电机的位置控制

直流电机的位置控制

直流电机的位置跟踪

直流电机的位置跟踪

直流电机的位置跟踪

直流电机的位置跟踪

直流电机的位置跟踪

直流电机的位置跟踪

直流电机的位置跟踪

直流电机的位置跟踪

谢谢大家智能机器人与运动控制系统15.直流电机的速度控制与跟踪课程基本内容空载速度控制、空载速度跟踪空载速度控制与跟踪空载速度控制、空载速度跟踪直流电机的位置控制是指控制电机的旋转速度，使其能够精确地保持给定的参考速度。直流电机的速度跟踪是指控制电机的旋转速度，使其能够准确地跟随一个给定的速度参考轨迹或命令信号。我们可以把速度控制看作单个给定的速度的速度跟踪，因此本章仅讨论速度跟踪的方法。空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪对于直流电机的速度跟踪问题，在设计LQG的代价函数时，我们仅以速度误差作为性能指标因为需要电机保持转动，不存在终止时刻，所以代价函数中只存在过程代价，不存在末值代价空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

空载速度控制、空载速度跟踪

谢谢大家智能机器人与运动控制系统16.移动机器人模型课程基本内容移动机器人模型双轮差速运动学模型阿克曼转向机器人运动学模型双轮差速机器人运动学模型辨识（里程计标定）双轮差速运动学模型双轮差速模型：结构简单运动灵活物理结构：两个主动轮一个随动轮双轮差速运动学模型双轮差速的运动状态圆周运动直线运动原地旋转运动双轮差速运动学模型双轮差速运动学模型的假设始终在一个二维平面上运动非完整性约束轮子沿切向移动无横向移动无侧向移动双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

双轮差速运动学模型

阿克曼转向机器人运动学模型阿克曼转向机器人运动学模型：运动原理与汽车相似，整体为四轮式结构两后轮作为驱动轮提供动力两前轮作为转向轮控制方向阿克曼转向机器人运动学模型阿克曼转向原理如果要让它能够稳定地转弯，即车轮与地面不发生滑动摩擦，那么四个车轮需要围绕同一个圆心做圆周运动前两轮的旋转角度会有所差异，后两轮的前进速度会有所差异阿克曼转向机器人运动学模型阿克曼转向原理复杂，可以等效为自行车模型阿克曼转向机器人运动学模型阿克曼模型到自行车模型：基于以下假设：1.假设阿克曼转向机器人始终在同一个二维平面上运动2.机器人左右轮子与地面之间的接触约束使其只能在接触面的切线方向上滚动，而不能横向或纵向移动阿克曼转向机器人运动学模型阿克曼模型到自行车模型：基于以下假设：1.假设阿克曼转向机器人始终在同一个二维平面上运动2.机器人左右轮子与地面之间的接触约束使其只能在接触面的切线方向上滚动，而不能横向或纵向移动，这是一个典型的非完整性约束阿克曼转向机器人运动学模型

阿克曼转向机器人运动学模型

阿克曼转向机器人运动学模型

阿克曼转向机器人运动学模型

阿克曼转向机器人运动学模型

阿克曼转向机器人运动学模型基于以上数学推导，阿克曼模型的四轮的控制信号可以由自行车模型的控制信号解算得到。两种模型等效。阿克曼转向机器人运动学模型

阿克曼转向机器人运动学模型

阿克曼转向机器人运动学模型

双轮差速机器人运动学模型辨识（里程计标定）里程计对位姿的估计也受到误差的影响。里程计主要受三个误差源的影响系统误差：用于将车轮位移转换为车辆位移的运动方程中的参数不确定性造成的建模误差非系统误差，如车轮打滑或地面不平造成的误差数值漂移，即离散化运动学模型带来的误差双轮差速机器人运动学模型辨识（里程计标定）在这三个误差源中，非系统误差不可避免，数值漂移的问题可以通过从硬件上适当缩短采样周期来提高离散时间积分的精度。对于系统误差，我们可以通过减少运动参数不确定性的方法，尽可能的减少系统误差，提高机器人对当前的位置和方向的估计精度。对于双轮差速机器人，我们需要对其运动学模型进行辨识，以减少系统误差。双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

双轮差速机器人运动学模型辨识（里程计标定）

谢谢大家智能机器人与运动控制系统18.ROS运动控制实践ROS简介发布/订阅ROS节点基于消息发布的小乌龟运动编程ROS简介ROS简介-到底是什么ROS通信机制ROSROS的通讯机制为松耦合分布式通讯ROS通信机制ROS通讯机制通讯机制的组成节点：最小可执单元，即一个进程箭头：进行数据传输话题：单向的异步通信的机制ROS的模块化设计初步了解ROS后，有以下疑问：ROS为什么会设计成这几个模块？它的通信机制为什么会是这种方式？当需要ROS实现某个任务的时候，

它又是如何运转的？一个例子假设你和同学组团去旅游：建立微信群做攻略分配任务每个人根据自己的任务和做的攻略各自完成在群里讨论进展，动态调整准备出发ROS模块化设计ROS模块化设计类比举例：组团旅游ROS模块化设计1.便于同时开发各个模块，提高协作开发效率。

2.模块内容可以更换、扩展，便于实现更复杂的功能。

3.模块具有复用性，便于快速搭建系统。ROS应用：小乌龟运动编程小海龟仿真器开启一个终端，打开roscore开启另外一个终端，启动小海龟节点再开启一个终端，进行键盘控制$roscore$rosrunturtlesimturtlesim_node$rosrunturtlesimturtle_teleop_keyturtlesim功能包中的话题和服务

名称类型描

述话题订阅turtleX/cmd_velgeometry_msgs/Twist控制海龟角速度与线速度的输入指令话题发布turtleX/poseturtlesim/Pose海龟的姿态信息，包括xy的位置、角度、线速度和角速度服务clearstd_srvs/Empty清除仿真器中的背景颜色resetstd_srvs/Empty复位仿真器到初始配置killturtlesim/Kill删除一只海龟spawnturtlesim/Spawn新生一只海龟turtleX/set_penturtlesim/SetPen设置画笔的颜色和线宽turtleX/teleport_absoluteturtlesim/TeleportAbsolute移动海龟到指定姿态turtleX/teleport_relativeturtlesim/TeleportRelative移动海龟到指定的角度和距离ROS应用：小乌龟运动编程小海龟仿真器打开一个新终端，查看ROS计算图节点/turtlesim:接受数据/teleop_turtle:发布数据主题/turtle1/cmd_vel$rqt_graph发布/订阅ROS节点发布/订阅ROS节点小海龟例程中的发布/订阅节点节点/turtlesim:接受数据（订阅节点）/teleop_turtle:发布数据（发布节点）

rostopic/turtle1/cmd_vel发布/订阅节点是ROS系统中最基本、最常用的通信方式，接下来我们就以经典的“HelloWorld”为例学习如何创建发布/订阅节点Catkin工作空间编辑和编译代码的文件夹可以协助编译目录下的所有项目操作：打开一个终端，并且输入：$source/opt/ros/kinetic/setup.bash$mkdir-pcatkin_ws/src$cdcatkin_ws$catkin_makecatkin_make是catkinworkspace下的编译指令。初次运行时，目录下会生成build、devel两个文件夹，并在src文件夹生成CMakeLists.txt文件。发布/订阅ROS节点创建新功能包在工作空间里创建一个新功能包：catkin_create_pkg创建了一个名叫tutorials的新功能包，这个包依赖于已有的ros功能包：std_msgs,rospy,在生成的package.xml中包括了名称描述、作者、证书、依赖项等$cdsrc#进入src文件夹再进行操作$catkin_create_pkgtutorialsstd_msgsrospy发布/订阅ROS节点创建新功能包编译这个新功能包$cd..#返回到catkin_ws文件夹$catkin_make发布/订阅ROS节点发布者/订阅者节点对一个Topic，节点A，B注册成为该Topic的发布者(Publisher)，他们都可以不定时的向Topic发送格式化的消息(message)节点C，D可以订阅(Subscribe)该Topic，每当Topic上出现新message，C，D都会收到通知，并调用各自的回调函数(CallbackFunction)处理新message发布者节点创建（需要在codeserver中编辑python文件）$cdsrc/tutorials#进入tutorials文件夹再进行后续操作$mkdirscripts$cdscripts$touchstr_talker.py发布/订阅ROS节点创建发布者节点编辑str_talker.py，代码如下:第一行#!/usr/bin/envpython设置环境编译并测试发布者节点#确保目前在tutorials/scripts文件夹$chmod+x./str_talker.py给文件以执行权限$cd/home/headless/catkin_ws#确保自己在catkin_ws文件夹下进行操作$

catkin_make$source./devel/setup.bash$rosruntutorialsstr_talker.py发布/订阅ROS节点发布者/订阅者节点对一个Topic，节点A，B注册成为该Topic的发布者(Publisher)，他们都可以不定时的向Topic发送格式化的消息(message)节点C，D可以订阅(Subscribe)该Topic，每当Topic上出现新message，C，D都会收到通知，并调用各自的回调函数(CallbackFunction)处理新message订阅者节点创建（需要编辑python文件）$cdsrc/tutorials#进入tutorials文件夹再进行后续操作$cdscripts$touchstr_listener.py发布/订阅ROS节点创建订阅者节点编辑str_listener.py，代码如下:第一行#!/usr/bin/envpython设置环境编译并测试订阅者节点#确保目前在tutorials/scripts文件夹#给文件以执行权限$chmod+x./str_listener.py$cd/home/headless/catkin_ws#确保在catkin_ws文件夹下操作$

catkin_make$source./devel/setup.bash$rosruntutorialsstr_listener.py发布/订阅ROS节点共同运行发布者和订阅者节点打开3个终端窗口，分别在里面运行主节点roscore，发布者节点str_talker.py，订阅者节点str_listener.py##=====TerminalNo.1=======$source/opt/ros/kinetic/setup.bash$

cd/home/headless/catkin_ws$source./devel/setup.bash$roscore##=====TerminalNo.2=======$source/opt/ros/kinetic/setup.bash$cd/home/headless/catkin_ws$source./devel/setup.bash$rosruntutorialsstr_talker.py##=====TerminalNo.3=======$source/opt/ros/kinetic/setup.bash$cd/home/headless/catkin_ws$source./devel/setup.bash$rosruntutorialsstr_listerner.py同时运行，可以看到一边发布了消息，一边订阅了消息发布/订阅ROS节点（练习）基于消息发布的小乌龟运动编程小乌龟运动开环控制按照以下步骤可以尝试使用开环控制让小乌龟进行圆形运动打开一个终端进入/catkin_ws/src文件夹,并且在工作空间中创建功能包$source/opt/ros/kinetic/setup.bash$cdDesktop$mkdir-pcatkin_ws/src$cdcatkin_ws$catkin_make$cdsrc#进入src文件夹再进行操作$catkin_create_pkglearning_topicroscpprospystd_msgsgeometry_msgsturtlesim基于消息发布的小乌龟运动编程（练习）基于消息发布的小乌龟运动编程小乌龟运动开环控制进入文件夹/catkin_ws/src/learning_topic/src，编辑velocity_publisher.py,控制小海龟按照圆形运动，并且改动文件的权限程序要点：（1）ROS节点初始化（2）创建Publisher（3）设置循环频率（4）循环：初始化消息类型，设置速度，发布消息，按照循环频率延时$touchvelocity_publisher.py$chmod+xvelocity_publisher.py（练习）基于消息发布的小乌龟运动编程小乌龟运动开环控制完成py文件编写后，在文件夹catkin_ws中进行编译，并且运行程序$cdcatkin_ws$catkin_make#确认没有报错信息后再进行下一步$sourcedevel/setup.bash#设置当前文件夹为ros工作空间$rosrunlearning_topicvelocity_publisher.py（练习）基于消息发布的小乌龟运动编程小乌龟运动开环控制同理，开环控制小乌龟做矩形运动，也可以按照上述流程，示例代码如下：（练习）基于消息发布的小乌龟运动编程（练习）小乌龟运动闭环控制小乌龟的开环矩形运动产生的效果并不是很好提示：通过灵活运用订阅者所返回的信息，可以调节小乌龟在四个角上的旋转角度。谢谢大家智能机器人与运动控制系统19.ROS的KF实现与线性定位KF定位系统与roslaunch简介KF定位系统节点与实验说明实验步骤与要求KF定位系统与roslaunch简介系统整体框图perception部分负责融合预测值与测量值，获得更接近真实值的滤波值driver部分对应于机器人动力学部分，作用是将控制信号转为机器人的移动controller作为控制信号的发出者，通过编写控制程序给出控制序列实现pid，与小海龟的pid控制对应。也可以使用teleop手动控制Gazebo部分作为仿真环境，给出机器人移动与环境的交互并提供传感器的值

perceptionTwistcallbackKalmanFilterLaserScancallbackplotModelstatecallbackRobotdriverTwistcallbackDynamic&DiscretizationPubmodelstateControlsignalControlsignalrobotSensorcallbackModelStateRobotcontrollerTeleop/controller.pyRobotcontrollerCallbackgazeboperceptionRobotdrivergazeboSensormsgs项目文件结构rospkg的目录结构(假设包的名称为cylinder_robot)-cylinder_robot -script

-driver.py #将控制信号转变为机器人在gazebo中的状态改变，对应“Robotdriver” -perception.py #对测量值与由控制信号和模型得到的预测值滤波，对应“perception” -controller.py #产生控制信号序列，对应“controller” -world

-cylinder.world #先前保存的带有机器人和房间的world文件 -config

-rvizConfig.rviz #使用rviz打开可以发出控制信号的teleop，仅作为测试使用 -launch

-runCylinder.launch #启动文件，将以上文件结合起来 -XXX.fig

#存储轨迹、新息等体现滤波效果的结果图

如果一个一个启动是否显得太麻烦？roslaunch简介为什么需要launch文件？用launch文件来启动工程可以将需要的节点同时启动，不用再一个一个进行，提高了效率。里面还有很多参数灵活使用会带来非常高效的调试。roslaunch的概念：launch文件是一个XML格式的文件，可以同时启动多个节点，还可以在参数服务器中设置参数。注意：roslaunch命令执行launch文件时，首先会判断是否启动了roscore,如果启动了，则不再启动，否则，会自动调用roscore。runCylinder.launch启动上述节点<launch>

<!--Weresumethelogicinempty_world.launch,changingonlythenameoftheworldtobelaunched-->

<include

file="$(findgazebo_ros)/launch/empty_world.launch">

<arg

name="world_name"

value="$(findcylinder_robot)/worlds/cylinder.world"/>

<!--Note:theworld_nameiswithrespecttoGAZEBO_RESOURCE_PATHenvironmentalvariable-->

<!–Thefollowingargswillbesetatinitialization-->

<arg

name="paused"

value="false"/>

<arg

name="use_sim_time"

value="true"/>

<arg

name="gui"

value="true"/>

<arg

name="recording"

value="false"/>

<arg

name="debug"

value="false"/>

</include>

<node

pkg="rviz"

name="rviz"

type="rviz"

args="-d$(findcylinder_robot)/config/rvizConfig.rviz"/>

<!–launch

three

python

nodes-->

<node

pkg="cylinder_robot"

name="controller"

type="controller.py"

output="screen"

respawn="false"/>

<node

pkg="cylinder_robot"

name=“driver"

type=“driver.py"

output="screen"

respawn="false"/>

<node

pkg="cylinder_robot"

name=“perception"

type=“perception.py"

output="screen"

respawn="false"/></launch>系统节点与实验说明实验目标机器人移动控制与定位实验机器人移动控制（driver）机器人动力学模型及离散化Gazebo机器人状态发布perceptionTwistcallbackKalmanFilterLaserScancallbackplotModelstatecallbackRobotdriver(driver.py)TwistcallbackDynamic&DiscretizationPubmodelstateGeometry/twistrobotRobotcontrollerTeleop/controller.pyCallbackgazeboperception设定控制程序（controller）设计轨迹及对应控制序列*加入pid闭环控制gazebo_msgs/ModelStatedriver.py订阅对应topic，设定A,B与对应的噪声classDriver(object):

def__init__(self):

self.time_save=0

='cylinderRobot'

self.pub=rospy.Publisher(...)

rospy.Subscriber(...)

self.state=np.zeros([4,1])

self.A=...

self.B=...

self.Sigma_w=np.eye(4)*0.00001

代码#类定义：驱动器。需补全#初始化函数。需补全以下部分：#gazebo状态发布，…代表需补全的部分#控制信号订阅#A,B的初始化解释driver.py接收到控制信号的回调函数，负责更新机器人状态并发到gazebodefcallback_control(self,twist):

……defforward_dynamics(self,u,dt): ……def_discretization_Func(self,dt): …

returnAtilde,Btilde,Sigma_w_tildedefsendStateMsg(self): …if__name__=='__main__':

try:

rospy.init_node(‘Driver',anonymous=True)

driver=Driver()

rospy.spin()

exceptrospy.ROSInterruptException:

pass代码#控制信号回调函数，收到一帧控制信号时自动执行，已完成，……代表已完成但省略的内容#前向动力学函数，根据当前状态与控制信号计算下一时刻系统状态，已完成#离散化函数，由时间dt求得离散化的A,B,Sigma_w，根据之前课上所学将其补全#将更新后的状态发送到gazebo中的，已完成#主函数，已完成解释controller.py产生一个加速5s减速5s的控制序列，实现移动一段距离的开环控制#!/usr/bin/envpython

importrospyimportsysfromstd_msgs.msgimportStringfromgeometry_msgs.msgimportTwist

defcontroller():

pub=rospy.Publisher('/robot/control',\

Twist,queue_size=10)

rospy.init_node(‘controller',anonymous=True)

rate=rospy.Rate(10)

foriinrange(0,100):

twist=Twist()

twist.linear.x=1.5*abs(i-49.5)/(i-49.5)#twist.angular.z=0.5*abs(i-49.5)/(i-49.5)

pub.publish(twist)

rate.sleep()

sys.exit(0)

if__name__=='__main__':

try:

controller()

exceptrospy.ROSInterruptException:

pass

代码#该程序为产生一段加速5s减速5s的控制序列，#实现移动一段距离，实验中可以自定义一段序#列实现某个控制目标#也可以在获得Kalmanfilter回传的反馈信息#基础上实现pid闭环控制解释实验目标机器人KF定位实现KalmanFilter理论与代码转化采样不一致的离散化处理滤波结果输出*滤波结果的反馈Perception(perception.py)TwistcallbackKalmanFilterLaserScancallbackplotModelstatecallbackRobotdriver(driver.py)TwistcallbackDynamic&DiscretizationPubmodelstateGeometry/twistrobotRobotcontrollerTeleop/controller.pyx_filteredgazeboSensor_msgs/LaserScangazebo_msgs/ModelStategazebo_msgs/ModelStateyx_realx_measuredux_filteredfigs使用launch文件启动工程perception.pyclassKalmanFilter(object):

#initializationthekalmanfilter.

#

x'(t)=Ax(t)+Bu(t)+w(t)

#

y(t)=Cx(t)+v(t)

#

x(0)~N(x_0,P_0)

def__init__(self,mass,C,Sigma_w,Sigma_v,x_0,P_0):……def_discretization_Func(self,dt):……def_predict_Step(self,ctrl_time): …def_correction_Step(self,y): …defcontrol_moment(self,u_new,time_now): …defobserve_moment(self,y_new,time_now): …代码#类定义：KalmanFilter，待补全#初始化函数，已完成#离散化函数，已完成#KF中的预测，待补全#KF中的更新，待补全#采样不一致时控制时刻的处理，待补全#采样不一致时观测时刻的处理，待补全解释KalmanFilter类定义perception.pyKalmanFilter的初始化函数def__init__(self,mass,C,Sigma_w,Sigma_v,x_0,P_0):

self.mass=mass

self.C=C

self.n=4

self.m=2

self.Sigma_w=Sigma_w

self.Sigma_v=Sigma_v

self.t=0

self.x=x_0

self.P=P_0

self.u=np.zeros([self.m,1])代码#初始化函数，主要是传递参数，已完成解释perception.pyKalmanFilter的离散化函数def_discretization_Func(self,dt):

Atilde=np.array([

[1,0,

0,

0],

[dt,1,

0,

0],

[0,0,

1,

0],

[0,0,

dt,1]

])

Btilde=np.array([

[dt/self.mass,

0],

[dt*dt/2/self.mass,0],

[0,

dt/self.mass],

[0,dt*dt/2/self.mass]

])

q1=self.Sigma_w[0,0]

q2=self.Sigma_w[1,1]

q3=self.Sigma_w[2,2]

q4=self.Sigma_w[3,3]

Sigma_w_tilde=np.array([

[dt*q1,

dt*dt/2*q1,

0,

0],

[dt*dt/2*q1,

(dt*q2)+(dt*dt*dt/3*q1),

0,

0],

[0,

0,

dt*q3,

dt*dt/2*q3],

[0,

0,

dt*dt/2*q3,(dt*q4)+(dt*dt*dt/3*q3)],

])

returnAtilde,Btilde,Sigma_w_tilde代码#离散化函数，计算离散化的系统矩阵。与上节#课相同，这里不再重复。已完成解释perception.pyLocalization类classLocalization(object):def__init__(self):……defcallback_control(self,twist):

#extractcontrolsignalfrommessage …

#callcontrolmomentfunctioninKalmanfilter …defcallback_observe(self,laserscan):

#extractobservesignalfrommessage

…

#callobservemomentfunctioninKalmanfilter …defcallback_state(self,state):……defsendStateMsg(self):……defvisualization(self):……代码#类定义：定位器，待补全#初始化函数，已完成，……代表省略#观测信号回调函数，待补全#twist是ros提供的数据帧格式，这里用来传递控制信息#twist.linear.x和twist.angular.z存储了控制信息#具体数据格式见下页#…代表待补全的部分#观测信号回调函数，待补全#laserscan是ros提供的激光雷达数据帧格式，#laserscan.ranges中存储的就是雷达测量到的距离数据。#具体数据格式见下页#真值回调函数，以作绘图用，已完成#发送滤波后定位，做pid用，已完成#可视化函数，已完成解释ROS消息类型/en/groovy/api/sensor_msgs/html/msg/LaserScan.html/en/lunar/api/geometry_msgs/html/msg/Twist.htmlperception.pyLocalization类的初始化classLocalization(object):def__init__(self): rospy.Subscriber('/robot/control',Twist,self.callback_control) rospy.Subscriber('/robot/observe',LaserScan,self.callback_observe) rospy.Subscriber('gazebo/set_model_state',ModelState,self.callback_state) self.pub=rospy.Publisher("/robot/esti_model_state",ModelState,queue_size=10) rospy.on_shutdown(self.visualization) self.kf=KalmanFilter(mass=10,C=np.array([[0,1,0,0],[0,0,0,1]]),Sigma_w=np.eye(4)*0.00001,Sigma_v=np.array([[0.008**2,0],[0,0.008**2]]),x_0=np.zeros([4,1]),P_0=np.eye(4)/500) self.x_esti_save=[] self.x_esti_time=[] self.x_true_save=[] self.x_true_time=[]代码#初始化函数，已完成#控制程序发出的控制值#传感器发出的激光观测值#gazebo发出的位置真值，仅作绘图用#将滤波后的定位值发送给controller以做pid#退出运行可视化程序，将移动过程中的滤波结果绘出#调用KF类创建该参数下的滤波器#保存滤波结果与真值信息以作绘图用解释perception.pyLocalization类的响应函数defcallback_state(self,state):

current_time=rospy.get_time()

x=np.zeros([4,1])

x[0,0]=state.twist.linear.x

x[1,0]=state.pose.position.x

x[2,0]=state.twist.linear.y

x[3,0]=state.pose.position.y

self.x_true_save.append(x)

self.x_true_time.append(current_time)defsendStateMsg(self):

msg=ModelState()

msg.model_name=

msg.pose.position.x=self.kf.x[1]

msg.pose.position.y=self.kf.x[3]

self.pub.publish(msg)代码#真值回调函数，以作绘图用，已完成#将当前滤波后的x发出，做pid用，已完成解释perception.pyLocalization类的可视化函数defvisualization(self):

print("Visualizing......")

t_esti=np.array(self.x_esti_time)

x_esti=np.concatenate(self.x_esti_save,axis=1)

p_obsv=np.concatenate(self.p_obsv_save,axis=1)

t_obsv=np.array(self.p_obsv_time)

t_true=np.array(self.x_true_time)

x_true=np.concatenate(self.x_true_save,axis=1)

fig_x=plt.figure(figsize=(16,9))

ax=fig_x.subplots(2,2)

ax[0,0].plot(t_esti,x_esti[1,:].T,label="esti")

ax[0,0].plot(t_true,x_true[1,:].T,label="true")

ax[0,0].legend(bbox_to_anchor=(0.85,1),loc='upperleft')

ax[0,0].set_title('px')

ax[1,0].plot(t_esti,x_esti[3,:].T,label="esti")

ax[1,0].plot(t_true,x_true[3,:].T,label="true")

ax[1,0].legend(bbox_to_anchor=(0.85,1),loc='upperleft')

ax[1,0].set_title('py')代码#可视化函数，将存储的各种值以图像存储，观#测滤波器的效果。已完成#

可以自定义观察的参数，得到不同的图像#接下页解释perception.pyLocalization类的可视化函数

ax[1,1].plot(x_esti[1,:].T,x_esti[3,:].T,label='esti')

ax[1,1].plot(p_obsv[0,:].T,p_obsv[1,:].T,label='obsv')

ax[1,1].legend(bbox_to_anchor=(0.1,1),loc='upperleft')

ax[1,1].set_title('trace:estiwithobservation')

fig_path=rospkg.RosPack().get_path('cylinder_robot')+"/"

fig_x.savef