13. State Estimation
Kinematic measurements
Inertial measurements
Extended Kalman Filter
• No assumption on terrain
• Kinematic measurements
(encoders)for legs in contact
• Fused with inertial
measurements(IMU)
• Error < 5% over distance
14. State Estimation
Kinematic measurements
Inertial measurements
Extended Kalman Filter
• No assumption on terrain
• Kinematic measurements
(encoders)for legs in contact
• Fused with inertial
measurements(IMU)
• Error < 5% over distance
• Optionally combined with
external pose (GPS, laser,
vision, etc.)
M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoepflinger and R. Siegwart, “State Estimation
for Legged Robots on Unstable and Slippery Terrain”, in International Conference on Intelligent Robots
and Systems (IROS), 2013.
17. Kinematics, Dynamics, and Control of Quadruped
Joint position ↔ Task space position r=r(q)
Kinematics
18. Joint velocity ↔ Task space velocity 𝑟= 𝑟 (q)
Kinematics
Kinematics, Dynamics, and Control of Quadruped
19. Joint force ↔ motion / external forces
Dynamics
Kinematics, Dynamics, and Control of Quadruped
20. Quadruped robot
Inverse Kinematics of a New Quadruped Robot Control Method
Cai RunBin, Chen YangZheng
International Journal of Advanced Robotic Systems, 2013
26. Quadrupedal Robot with Point Feet
• Floating base system with 12 actuated joint and 6 base coordinates (18DoF)
Kinematics of Floating Base / Mobile Systems
27. 1. How many generalized coordinates ?
2. How many base coordinates ?
3. How many actuated joint coordinates ?
4. How many contact constraints ?
Quadrupedal robot
• Static walking
• 3 legs in stance [NR 1,2,3]
• 1 in swing [NR 4]
Kinematics of Floating Base / Mobile Systems
28. • Describe system by base and joint coordinates
• Base coordinates : rotation and position of base
• Contact constraints :
Lecture «Robot Dynamics»: Kinematics 3
Kinematics of Floating Base / Mobile Systems
29. Importance of Jacobian
Kinematics (mapping of changes from joint to task space)
• Inverse kinematics control
• Resolve redundancy problems
• Express contact constraints
Statics (and later also dynamics)
• Principle of virtual work
Variations in work must cancel for all virtual displacement
Internal forces of ideal joint don’t contribute
Kinematics of Floating Base / Mobile Systems
30. • Generalized velocities and
accelerations ?
Time derivatives 𝑞, 𝑞 depend on
parameterization
• Linear Mapping
Kinematics of Floating Base / Mobile Systems
31. Kinematics of Floating Base / Mobile Systems
Robot design - top view
Novel design of a quadruped Robot for Research Purpose
Yam Geva and Amir Shapiro
International Journal of Advanced Robotic Systems, 2013
Robot design - side view
33. Forward Kinematics
Kinematics of Floating Base / Mobile Systems
describe the robot posture in the world coordinate frame.
define a specific posture of the robot by the configuration vector 𝑞 ∈ 𝑅18
q = (CoG, L), CoG = (Cx, Cy, Cz, Φroll , Φpitch, Φyaw)
CoG : location of the center
of gravity and the body roll,
pitch and yaw angles.
L = (L1, L2, L3, L4) vector defines the
footholds of all four legs where Li =
(Lix,Liy,Liz) denotes the coordinates of leg
ith foothold (i=1,2,3,4) as represented in
the world coordinate frame
34. In order to calculate the position of one footpad in the world fixed frame (frame 0)
First transform it to the body fixed frame (frame C).
Forward Kinematics
Kinematics of Floating Base / Mobile Systems
𝐴 𝐶
𝑜
37. Forward Kinematics
Kinematics of Floating Base / Mobile Systems
The result allows us to calculate the locations of the robot footpad given
the location and orientation angles of the robot body and each joint angle.
𝐿𝑖
𝐴 𝐶
0
@ origin of world fixed frame (frame 0)
38. Position of an arbitrary point on the robot
Velocity of this point
𝑰 𝑱 𝑸(𝒒)
Kinematics of Floating Base / Mobile Systems
Differential kinematics Jacobian
40. Contact Constraints
• A contact point 𝐶𝑖 is not allowed to move :
• Constraint as a function of generalized coordinates :
• Stack of constraints
Kinematics of Floating Base / Mobile Systems
41. Properties of Contact Jacobian
Contact Jacobian tells us, how a system can move.
• Separate stacked Jacobian
• Base is fully controllable if
• # of kinematic constraints for joint actuators:
Generalized coordinates DON’T correspond to the degrees of freedom
• Contact constraints !
Minimal coordinates (= correspond to degrees of freedom)
• Require to switch the set of coordinates depending on contact state
(=> never used)
relation between base motion and constraints
-
Kinematics of Floating Base / Mobile Systems
43. Support Consistent Inverse Kinematics
Inverse kinematics to floating base systems
how to move individual joints in order to achieve certain task-
space motion without violating contact constraints.
Multi-task approach with prioritization,
contact constraints are considered to have higher priority
than the task-space motion
44. contact constraints of the nc legs in ground contact are given by
Support Consistent Inverse Kinematics
Motion of the system in contact
given a demanded task space motion
Joint velocity required to achieve this
𝐽𝑐 𝑞 = 0
𝑞 =
𝑞 = 𝐽𝑐
+
0 + 𝑁𝑐 𝑞 0= 𝑁𝑐 𝑞 0
𝑤𝑡 = 𝐽𝑡 𝑞
47. Multi-body Dynamics
How to get the equation of motion
in joint space
• Newton-Euler
• Projected Newton-Euler
• Lagrange II
Started from the principle for virtual work
Dynamics of Floating Base / Mobile Systems
48. Impulse and angular momentum
Use the following definitions
Conservation of impulse and angular momentum
Newton
Euler
External forces
and moments
Change in impulse and
angular momentum
A free body can move
In all directions
Dynamics of Floating Base / Mobile Systems
49. Projected Newton Euler
Consider only directions the system can move(c.f. generalized coordinates)
Resulting in
Dynamics of Floating Base / Mobile Systems
50. Get equation of motion from
• Kinetic energy
• Potential energy
Lagrange II
Dynamics of Floating Base / Mobile Systems
52. External Forces
Some notes
External forces from constraints
• Equation of motion (1)
• Contact constraint (2)
• Substitute 𝑞 in (2) from (1) (3)
• Solve (3) for contact force
Dynamics of Floating Base Systems
53. Support Consistent Dynamics
Equation of motion (1)
• Cannot directly be used for control due to the
occurrence of contact forces
• Contact constraint
• Contact force
• Back-substitute in (1),
replace and use
support null-space projection
• Support consistent dynamics
Dynamics of Floating Base Systems
54. Inverse Dynamics
• Joint impedance control
• Inverse dynamics control
• Generalized motion and force control
55. Joint Impedance Control
• Torque as function of position and velocity error
• Closed loop behavior
Static offset due to gravity
• Impedance control and gravity compensation
Estimated gravity term
Simple set-up…
But configuration dependent load
State error term
56. Inverse Dynamics Control
• Compensate for system dynamics
• In case of no modeling errors,
• the desired dynamics can be perfectly prescribed
• PD-control law
Model
Can achieve great performance…
But requires accurate modeling
57. Inverse Dynamics of Floating Base Systems
• Equation of motion of floating base systems
• Support-consistent
• Inverse-dynamics
• Multiple solutions
58. Control Concepts
an overview
Kinematic control:
• High-gain joint position trajectory tracking
Impedance control with joint space inverse dynamics
• Low-gain joint control with model compensation
Task-space inverse dynamics control
• Directly regulating in «task space»
Virtual model control
• dynamic control of a quasistatic system
63. • Equations of motion
• Contact constraint
• Desired motion optimization
• Contact force optimization
• Joint torque optimization
Whole-body Control of a Legged Robot
Whole-body control = large scale simultaneous optimization of 𝒒, Fs,τ
64. Operational Space Control as Quadratic Program
A general problem
search for a solution that fulfills the equation of motion
• Motion tasks:
• Force tasks:
• Torque min:
