wyllduck / deployed_vehicle_code Goto Github PK

Takagi-SugenoSLAM is a python-ROS based package for real-time 6 states estimation of the robot navigating in a 2D map.

First, the Gauss-Newton scan matching approach roughly estimate the state from the LIDAR endpoints using HectorSLAM and then model-based Takagi-Sugeno Kalman filter is applied to correct and estimate the full state of the vehicle.
LIDAR, and IMU sensors are used.

MPC controller is also added to the ROS package for full-body control of the vehicle following limits and constraints. The pipeline for SLAM, estimation, and controller can be seen in the figure below

The code is developed in Ubunutu 16.04 with ROS Kinetic Kame. Following libraries needed to be installed:

• OSQP : For MPC optimization
• scipy: For sparse matrix formulation

cd deployed_vehicle_code
catkin build

Launch files are independent of each other at this stage of development. A single launch file can be created to launch all the nodes together.

• To run on the simulation:
  roslaunch simulator simulator.launch : To launch simulation
  roslaunch observer state_estimator.launch sim:='1' : To launch Takagi-Sugeno estimator
  roslaunch controller controller.launch : To launch MPC controller
  roslaunch plotter mpc_plot.launch : To launch MPC plot to visualize trajectory generated
  roslaunch plotter observer_plot.launch : To launch estimator performance
  

• To run on the real vehicle:
  roslaunch manual_control rosserial.launch : To launch rosserial communication for arduino
  roslaunch fisheyecam_pose_estimation fisheye_tracker.launch : To launch fisheye camera localization
  roslaunch rplidar_ros rplidar.launch : To launch RPLIDAR for scan
  roslaunch rplidar_ros pose_estimation.launch : To launch rough pose estimation using LIDAR
  roslaunch observer state_estimator.launch : To launch Takagi-Sugeno estimator
  roslaunch controller controller.launch : To launch MPC controller
  roslaunch plotter mpc_plot.launch : To launch MPC plot to visualize trajectory generated
  roslaunch plotter observer_plot.launch : To launch estimator performance
  

Nodes:

• switching_lqr_observer: Implementation of Takagi-Sugeno Kalman estimator
  

  • Published Topics:
    • /est_state_info -> Publishes estimated states
    • /ol_state_info -> Publishes open-loop prediction using the model
    • /meas_state_info -> Publishes fused measurement from different sensors.
  • Subscribed Topics:
    • control/accel -> Dutycycle (D) control input from controller.
    • control/steering -> Steering angle () control input from controller.
    • /slam_out_pose -> Roughly estmated pose from LIDAR and scan matching algorithm.
    • /wheel_rpm_feedback -> Subscribes to Motor encoder feedback
    • /twist -> Subscribes to angular velocity from IMU
    • /pose -> Subscribes to orientation from IMU
    • /fused_cam_pose -> Subscribes to pose obtained from fisheye cam if used.

• control: MPC controller implementation
  

  • Published Topics:
    • control/LPV_prediction -> Publishes model predicted states using LPV model
    • control/MPC_prediction -> Publishes optimizer generated states
    • control/accel -> MPC control output 1. Motor dutycycle (D)
    • control/steering -> MPC control output 2. Steering angle ()
  • Subscribed Topics:
    • /est_state_info -> Subscribed to estimated state from the node switching_lqr_observer

Detailed interconnection of nodes, topics are shown in the below image.