As we all know Ardupilot supports a varielty of vehicles on ground, air and water , this summer I intend to add legged robots to Ardupilots arsneal.I have the pleasure to be working along with @rmackay9 and @khancyr in the coming months to add support for a quadruped to start with.
1.INTRODUCTION
Quadruped robots have an important place in robotics and their popularity is rapidly increasing. Quadruped robots are very complex in structure and arguably more difficult to control than wheeled rovers. Quadruped robots mimic an animal’s walking gait and they have advantages like walking on different terrains and extremely rough surfaces. Obstacles can obstruct the movement of wheeled vehicles, whereas a quadruped can adapt itself to avoid obstacles by adjusting its body height. Low speed, difficult to build and control, and the need for onboard power are limitations of quadruped robots.
The Ardupilot framework already has most of the components required to support the walking robot through inbuilt libraries including inertial measurement drivers, extended kalman filters, and rc_channel libraries. Support for walking robot can be achieved with addition of modules to the rover codebase
Implementation of walking robot over ardupilot
The integration of walking robot support will be done as quickly as possible by reusing existing code efficiently, and make sure any upgrades to the rover code also reflects the new walking robot frame making sure there is no integration issue
Solid kinematics model
The kinematics model will be able to find the values of the servos at any given frame or terrain, this is only possible by extensive real world testing and calibration of the model
SITL simulator
A basic sitl simulator to give outputs of the servo joint angles to graphics simulator like pybullet/gazebo to visualize the kinematics and gaits.
Support for GCS
The ground control will have the ability to differentiate the walking robot from the other vehicles and provide the user with walkingbot specific configuration settings.
Documentation
For a non power user or even a skilled user documentation is very necessary to assemble and configure the setup. Documentations of the assembly , parameter configuration , parameter tuning , how to add new gaits etc will be made available.
2.IMPLEMENTING KINEMATICS OF A FOUR-LEGGED ROBOT
In quadruped robots, a good kinematic model is necessary for stability analysis and trajectory planning of the system.There are two types of kinematic analysis: forward and inverse kinematics analysis.
- In the forward kinematic analysis, the joint variables are given to find the location of the body of the robot.
- In the inverse kinematic analysis, the location of the body is given to find the joint variables necessary to bring the body to the desired location.
The quadruped robot has four legs and each one has 3 degrees of freedom (3DOF).The links of legs are connected to each other by rotary joints.Goal of the kinematic model is to return all the twelve angles (4 Legs x 3 Servos) for a Robot when Body-Pose and Positions for all four feet in Global-Space are given
- In the inverse kinematic analysis, the location of the body is given to find the joint variables necessary to bring the body to the desired location.
The quadruped robot has four legs and each one has 3 degrees of freedom (3DOF).The links of legs are connected to each other by rotary joints.Goal of the kinematic model is to return all the twelve angles (4 Legs x 3 Servos) for a Robot when Body-Pose and Positions for all four feet in Global-Space are given.
- https://www.ijstr.org/final-print/sep2017/Inverse-Kinematic-Analysis-Of-A-Quadruped-Robot.pdf
- https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6299039/
These papers describe in detail the kinematics implementation.
MANUAL MODES
1.body translation mode
In this mode the the inputs from the transmitter channel are mapped to the position and pose of the bot
bodyposX
bodyposY
bodyrotX //pitch
bodyrotY //roll
2.walking mode
In this mode the inputs for the transmitter channel are mapped to the linear velocity of the robot .With change in input the cycle time of the gait is increased or decreased proportionally .multiple gaits will be made to choose from.
Xspeed // forward walk speed
Yspeed // side walk speed
3.Stabilized mode
In this mode the robot will be able to stabilize itself on uneven terrain using the onboard imu.
3. SIMULATION (SITL)
simulation is a vital part of any project and should be the first course of action in the development process. Simulation in the ardupilot is handled by SITL . SITL also forms the backend of the ardupilot’s CI checks. These checks are to be passed for any patch to be accepted into the codebase But, in this case it becomes essential to have a graphical simulation with a physics environment to simulate and test gaits. To solve this I will be using 3rd party physics simulators like pybullet
Reference link : https://pybullet.org/wordpress/
Kinematics model simulation : https://www.youtube.com/watch?v=dJiqUui1npY
4. FRAME AND ACTUATOR
RobotBody-https://www.robotshop.com/en/lynxmotion-symmetric-quadrapod-body-kit-mini.html
Aluminum Femur Pair-https://www.robotshop.com/en/lynxmotion-3-aluminum-femur-pair.html
Tibia Pair (Legs )-https://www.robotshop.com/en/lynxmotion-robot-leg-rl-01.html
Lynxmotion Servo Bracket-https://www.robotshop.com/en/lynxmotion-aluminum-multi-purpose-servo.html
Servo - HS-645MG https://www.robotshop.com/en/hitec-hs-645mg-servo-motor.html
REFERENCE/SOURCES
- ArduPilot Codebase:​- https://github.com/ArduPilot/ardupilot
- ArduPilot Developer Documentation​ - http://ardupilot.org/dev/
- http://zlethic.com/quadruped-tsuki/
- https://oscarliang.com/inverse-kinematics-implementation-hexapod-robots/?utm_source=rb-community&utm_medium=forum&utm_campaign=hexapod-robot-using-arduino-full-source-code-available
- https://gitlab.com/custom_robots/spotmicroai
- https://habr.com/en/post/225845/
- https://www.ijstr.org/final-print/sep2017/Inverse-Kinematic-Analysis-Of-A-Quadruped-Robot.pdf
- https://www.faizbalweel.com/single-post/2016/03/18/Inverse-Kinematics-3DOF-Quadruped
- https://www.instructables.com/id/Synopsis/