Zero Moment Point Experiments on a Biped Robot
From Experimental Robotics
Zero Moment Point Experiments on a Biped Robot
David Collien, 2009
Aim
The primary objective of this project was to use force sensitive resistors (FSRs) on the foot surface of a biped Cycloid II robot to track the trajectory of the zero moment point (ZMP) and use this trajectory information to aid in the balance of humanoid movement, ultimately correcting the balance of a walking gait.
Since bipedal robots tend to tip over very easily, a real-time balance control method is required to modify the posture of the robot to counteract forces caused by disturbances and to allow a motion patterns to adapt to unknown environments and adjust to mechanical and environmental inaccuracies and fluctuations.
Background
The Cycloid II robot used in experiments was previously used by Tak Fai Yik to in “Trial-and-Error Learning of a Biped Gait Constrained by Qualitative Reasoning” and "Locomotion of Bipedal Humanoid Robots: Planning and Learning to Walk”. The aim of Tak Fai’s research was to use learning and optimisation algorithms to produce a stable walking gait for the biped robot. Foot-force sensors were used to produce a stability criteria derived from the zero moment point. This stability criteria was used to determine the fitness of the output produced by a genetic algorithm and symbolic planner.
Gait patterns generated by this method produced a gait for the robot, but this was generated off-line. This gait is only stable if the actual model of the robot and the environmental conditions are the same as those used to plan a gait pattern. When the gait is operating on the robot, differences in both the robot mechanics and the environment, as well as unexpected disturbances, can quickly cause the pre-planned gait to become unstable. A real-time (on-line) balance controller using sensor feedback can be used to minimise the effect of such disturbances on the stability of the gait.
In “Balance Control of a Biped Robot Combining Off-line Pattern with Real-time Modification”, a method is proposed to modify an off-line gait pattern in real-time. This method use a similar configuration of foot-force sensors to track the zero moment trajectory point in real-time and adjust the body posture of the robot to react to disturbances during the operation of an off-line gait.
Four foot-force sensors, or force sensitive resistors (FSRs) are mounted on each foot to sense the ground reaction force. The ZMP is the point on the ground plane which is a projection of the sum of all forces acting on the robot and can be calculated from the position and magnitude of the sensed ground reaction forces. Since four sensors per foot are used as the standard sensing configuration, to sense the complete ground reaction force these four sensors must be the only points of support. This prevents the possibility of an un-sensed reaction force invalidating the ZMP calculation. If the ZMP lies outside of the robot’s support area (the convex hull of all points of contact with the ground) the robot will start to topple. Whilst the most stable position for the ZMP is that with the greatest distance to the edge of the support area, this paper points out that controlling the ZMP to stay at the centre of the support area constrains its ability to walk:
“the robot cannot move at high speed, and the movement of the robot becomes radical even when walking at low speed. A suitable stability margin can be the same as the minimum one of the stability margins of the off-line walking pattern, or can be computed according to environmental disturbances”. [Balance Control of a Biped Robot Combining Off-line Pattern with Real-time Modification]
All papers reviewed have mentioned that a biped gait is formed from two phases: a single support phase, where one foot’s surface is in contact with the ground, and a double support phase, where both feet are in contact with the ground. To calculate the ZMP in the single double support phase is less trivial then in the single support phase. When in the single support phase, all sensors on a single foot have constant relative positions. When in the double-support phase, or when a topple allows an extra sensor on at the toe or heel to touch the ground, calculating the ZMP requires a forward-kinematics calculation to determine the relative positions of the feet, as projected onto the ground.
To further investigate the trajectory that the ZMP traces whilst walking, Erbatur et. al. in “A Study on the Zero Moment Point Measurement for Biped Walking Robots” attached a similar configuration of foot-force sensors to human feet to trace the ZMP trajectory of a human walk. The resulting trajectory is shown in Fig. 11 of this paper, reproduced below:
This human ZMP trajectory can be used as a reference for biped robot walking, showing the shift of weight in the double support phase is a much faster movement than the steady control of the ZMP in the single support phase.
Sensing and calculating the ZMP trajectory of the Cycloid robot is an important step to provide insights into developing a real-time balance controller.
Problem Decomposition
Understanding of the Platform and Firmware Development
Upon receiving the robot, some example firmware was available from the manufacturer, using AVR C-code to communicate with the Dynamixel servos using the CM-2 ATMega128-based board.
The addition of sensors to the robot’s hardware configuration requires the development of some additional firmware. Since no such firmware was found amongst the software previously used with this robot, the first development required was the appropriate code to retrieve the analog-to-digital readings from the FSR sensors. This was combined with improved Dynamixel control code (to hide uglier RS-485 level interfacing) and improved RS-232 (serial to the computer) code to produce the final firmware
Foot-sensor Rebuild and Evaluation of Mounting
The existing foot sensor set-up relied on masking tape to fasten it to the foot, which was becoming brittle and falling apart. The foot sensor set-up required reconstruction, and at the same time re-evaluation. Background reading suggested that any contact with the ground that produces a ground reaction force that is not sensed invalidates the calculation of the zero moment point and that it is useful for real-time controller to sense when the toe touches the ground to provide a ZMP even when a slight topple has occurred, as well as providing useful information for “landing time control”.
Whilst the previous sensor set-up used a soft felt sole underneath the sensors to encourage all ground reaction forces to be directed onto the sensors, this particular configuration only operated successfully when the entire foot surface was in contact with the ground. In order to sense the correct ground reaction force with partial foot contact, an alternate method was required.
The option of changing the FSR connection to the CM-2 board was also considered. Instead of running wires from each of the FSRs up to the CM-2 board on the chest of the robot, via a voltage divider, a cleaner option would be to perform the analog-to-digital conversion at the foot, and allow it to act as an extension to the RS-485 leg servo daisy-chain. Whilst plans for this can be found later in this report, this remained only a consideration.
Foot-sensor Calibration
The provided foot-force sensors (FSRs) did not provide a linear output. In order to be able to calculate the zero moment point, an experiment was required to create a calibration function to transform FSR readings into the corresponding ground reaction force sensed. Weights of known masses were placed on a horisontal foot to measure the force of gravity as a reference.
Off-board Control and Experimentation Software
As a debugging and diagnostic tool, and to visualise a trace of the zero moment point, off-board software was developed, which talks to the CM-2 board via a RS-232 serial link at 57600 bps. This software also provided a method of controlling and sensing servo positions outside of the firmware.
Evaluation of FSR Noise and Sample Rate
Once the FSRs were operational and the data was calibrated and sent off-board to the serial link, the amount of noise and the rate at which samples could be sensed was evaluated.
ZMP Trajectory Plot
With operational FSRs, the next milestone was to plot the ZMP on a per-foot basis. The diagnostic software was to be modified to calculate the ZMP from the foot sensors and produce a plot of the trajectory over time whilst a motion pattern was running on the robot.
Robot Movement Patterns
Once the ZMP trace is available, a movement pattern is required to trace its ZMP path. A controller can then be added to change the pose of the robot to keep the ZMP inside the support area.
ZMP Stability Region Control and Stability Margins
The last achievable milestone was to attempt to control the ZMP to remain inside a defined region of stability whilst in single-support, using stability margins as described in “Balance Control of a Biped Robot Combining Off-line Pattern with Real-time Modification”.
Developments and Evaluation
Firmware for CM-2
The developed firmware consists of four parts:
- RS-485 and RS-232 UART drivers (RSDriver.c/h)
- Analog to digital conversion functions (adc.c/h)
- Dynamixel control functions (dynamixel.c/h)
- Main loop to process RS-232 communication (main.c)
RS-485 and RS-232 drivers were appropriated from the provided Example.c code bundled with the Cycloid II. It should be noted that this example firmware does not function out-of-the-box on the Cycloid II robot, as the DX-117 servos are using an upgraded firmware which allows them to run at a higher baud rate. Changing the SerialInitialize function on the RS-485 port (UART port 0) to take a baud rate value of ‘1’ (representing 1Mbps at 16Mhz) allows this firmware to operate.
Dynamixels
The UART communication functionality was extracted from this example to provide a basis for Dynamixel and serial-link communication. On top of these functions a set of higher-level functions to control and sense Dynamixel positions was developed.
These functions allow the setting and sensing of positions, speeds, etc. as well as diagnostic functions such as turning Dynamixel LEDs on or off and setting and removing power to individual servos. Each Dynamixel can also operate from a ‘control table’ which can be read or modified. The parameters of which can be found in the DX-117 manual (attached as an appendix). These include setting the allowed angles in which each servo can operate (compliance margins). Note that if a servo is instructed to move to a position which it cannot physically achieve, and no compliance margin is set, then it may over-heat and shut off its power. A Dynamixel usually flashes its LED as a warning that it is approaching its heat threshold.
A problem encountered was that Dynamixels did not seem to respond to position messages sent in quick succession. A quick-fix was to introduce a small delay. However this was later found to actually be due to the messages only incrementing the position in fractional quantities. Very small increments in a Dynamixel’s target angle are ignored if the increments are too small.
Analog to Digital Conversion
The analog to digital conversion (ADC) code was developed with the assistance of online forums and ATMEL data sheet resources. ADC is performed on the ATMega128 by using the ADC Control and Status Register A (ADCRA) and using Port F as the ADC input.
To configure Port F as an ADC input, DDRF must be set to zero. The internal pull-up resistors must also be turned off for this particular configuration, by setting PORTF to zero.
The ADEN bit of the ADCR is set to enable ADC readings. The ADFR bit of the ADCR enables “Free Running” mode. This automatically continuously samples the active ADC channel updating the ADC data register. This is turned off in the firmware, to allow manual control of when samples are taken, as 8 channels need to be sampled in turn, one for each foot sensor. The ADC Conversion Complete Interrupt is disabled, as every time a conversion completed, the entire program reset. This is thought to be a conflict with the CM-2 boot-loader present on the device. The completion of a conversion is detected by polling the ADSC bit of the ADCR when a conversion is requested. To choose the channel, the ADC Multiplexer register is configured by setting the required bit of ADMUX. The result of the conversion is stored in the registers ADCL and ADCH. Note that ADCL must be read before ADCH for a valid result. The last three bits of ADCRA relate to the clock prescaler. The higher the prescaler, the more accurate the ADC reading, but the slower the sample rate. It is currently set at 1/64’th of the clock. More information is available in the ATMega128 data sheet (attached).
This functionality is abstracted into two C functions, adc_init and getADCReading.
Compilation and Upload
Compilation is achieved using the avr-gcc and avr-objcopy tools available on POSIX-like systems. (N.B. on OS-X a convenient “AVR Cross-Pack” package is available). avr-gcc compiles into the Executable and Linkable Format (ELF), avr-objcopy converts this into either a binary object (.bin file) or Intel hex format (.hex) file. The supplied Windows-only Robot Terminal uploads .hex files to the CM-2 bootloader.
Reverse engineering this application revealed that it internally converted this .hex file to a binary file, and sent the raw binary data to the bootloader, followed by a checksum byte. A successful upload could be executed even if the checksum sent was incorrect, so no effort was put into trying to determine how the checksum was calculated. avr-objcopy provides a binary output, which was fed into a python script using the PySerial library. This was written to complete the upload process; automatically invoking the bootloader (a feature previously not present), sending the binary data and finally sending a dummy checksum value.
Foot Sensors
The original foot sensor set-up had the FSRs attached between a layer of soft felt separating the FSR from the hard foot surface, and a solid foot plate as shown below.
The FSRs operate by sensing the deformation of the sensor surface to determine the force applied. A FSR sandwiched between two hard surfaces with a force applied produces no reading, as the surface is not deformed. This original configuration works for two reasons; the soft felt allows the deformation of the FSR and also slightly discourages variation in the ground surface to support a section of the foot not covered by a force sensor. This works well for a foot placed evenly on the ground surface but not for situations when there is partial contact with the ground. When partial contact is made, e.g. at the toe of the foot, the plate below the sensors pulls on the heel sensors, causing a slight deformation. This deformation registers as a force reading, which is invalid. In some situations with large ankle tilt, the plate would also rest on the actual foot plate, giving invalid toe readings as well.
When tracing the ZMP during the single-support phase the original configuration is sufficient. However if attempting to track the ZMP through the double-support phase of a walk, especially in the transition between double-support and single-support (the ‘foot landing’), valid readings with partial foot contact is necessary.
The following graph shows the results of a sensor calibration experiment on the original foot sensor configuration, including the foot plate. The raw FSR values (1023 to 0) are plotted against known masses (grams) that were placed over each sensor.
Since the foot sensor set-up was deteriorating, and required work anyway, the possibility of a better solution was investigated. The first of which was to apply silicon insets on each sensor, held on with tape. The silicon insets worked well on their own, but attaching the silicon to each sensor was a challenge. Loose tape worked temporarily, but did not last. Pressure on the tape caused it to tighten, not releasing the silicon and producing readings when no force was applied from the ground.
The most successful solution was insets of thick, spongey double-sided tape, applied within the radius of each sensor as shown in the figure below. The thickness was similar to that of the original foot-plate, but allowed independence of the sensors when partial contact with the ground was made.
The above calibration experiment was performed on the new configuration with the following results:
This calibration curve was used to create a function to linearise the readings to reflect an actual force. When inverted, these readings form a curve close to a polynomial root. Raising the reading to a power pulled them closer to linear. Readings under 100g required a special case for accuracy. The display software allows each reading to have its own calibration function.
The sensor readings were able to be sampled at approximately 20 readings per second, by the off-board computer software with surprisingly little noise.
Off-Board Software
The off-board software developed provides an diagnostic view of the calibrated foot sensor values, and the trace of the zero moment point. This software uses the Mono development framework and its included GTK# to provide a cross-platform utility. The software sends commands to the firmware to collect sensor value readings over serial, and displays circles of radius proportional to the force reading sensed, and continuously plots the position of the ZMP to paint a trajectory.
Sensor value display and ZMP trace for an example single-support movement.
Stability Control
To create a real-time balance controller two methods were attempted. The first method was to adjust only the pose of the upper-body in reaction to foot force whilst supported on a single foot. The second was to use a ZMP stability region control method. A real-time controller using just the upper-body pose which tilted proportionally to the ZMP displacement from the centre of the support area was able to only correct for very slight disturbances. A reaction to a larger disturbance would throw the robot into an uncontrolled oscillation, unable to compensate for the large effect of the upper-body’s inertia, the robot would topple itself.
The ZMP stability region control method proposed in ““Balance Control of a Biped Robot Combining Off-line Pattern with Real-time Modification”, acts at the ankle:
"The most effective way of keeping the actual ZMP within the valid stable region is to control the ankle joints of the support feet... using the following equation..."
"...where ∆zmp is the distance between the actual ZMP and the boundary of the valid stable region (Fig. 5) and K is a coefficient."
Figure 5 from this paper is shown below:
This method operated more successfully at maintaining stability, with the robot correcting itself for slight pushes, whilst standing on one leg.
The ZMP trace for this is, and the defined valid stable region is shown below:
Demonstration firmware that exhibits these behaviours is provided.
Further Work
The next stage of experimentation is to apply this real-time balance control to a walking pattern. Attempts at generating a stable walking pattern were largely unsuccessful, as this is not a trivial task, and the gait resulting from Tak Fai’s work could not be located.
ZMP control is only one of the real-time correction mechanisms that can be used to correct a gait pattern in real-time. Some extensions to the real-time controller include:
- landing time control: controlling the transition of the swing foot to the support foot to occur at the appropriate time. If the foot lands late, the robot will fall forwards, if the foot lands early, it will fall backwards.
- foot planting location control: control using side-steps and step-length adjustments to correct for changes made by the ZMP control. This ensures the relationship to stride-length and torso speed is maintained.
- floor reaction control: countering for irregularities in the floor surface, or when the floor surface is not planar.
These control methods often require more than just foot-force sensors and can make use of gyroscopes and accelerometers to gain a better sense of the robot’s dynamics.
The current foot-force sensor configuration is also not ideal, as it runs many wires up the length of the leg. A considered improvement for this is to create ADC boards at the feet, which link to the RS-485 chain which drives the leg Dynamixels.
This would require an AVR (or similar) which could provide analog to digital conversion at the foot, with its UART Rx and Tx lines fed into a MAX485 line-driver IC (see Dynamixel manual for an example). Currently the FSRs are driven by a voltage-divider with free-formed resistors. This could be improved by using an op-amp IC to drive the FSRs before the signal is input into the AVR’s ADC port. This would additionally reduce any noise and give a better signal from the FSRs. The foot AVRs can then be programmed to act on RS-485 commands, as if it were a Dynamixel.
Conclusion
This work done on researching and experimenting with a real-time controller is only the first step of developing a dynamically stable humanoid gait. It is hoped that the knowledge gained from this experimentation, and the lessons learned will help in eventually producing a dynamically stable gait for this robot, and similar humanoid/biped robots.
References
- Erbatur, et. al., "A Study on the Zero Moment Point Measurement for Biped Walking Robots", IEEE (2002)
- Huang Q, Kaneko K, Yokoi K, Kajita S, et. al., "Balance Control of a Biped Robot Combining Off-line Pattern with Real-time Modification", Proceedings of the 2000 IEEE International Conference on Robotics & Automation (2000)
- Kajita S, Tani K, "Adaptive Gait Control of a Biped Robot based on Realtime Sensing of the Ground Profile", Proceedings of the 1996 IEEE International Conference on Robotics & Automation (1996)
- Yik T F, Sammut C, "Trial-and-Error Learning of a Biped Gait Constrained by Qualitative Reasoning", School of Computer Science and Engineering, UNSW (2002)
- Wyeth G, Kee D, Yik T F, "Evolving a Locus Based Gait for a Humanoid Robot", Proceedings of the 2003 IEEE/RSJ Intl. Conference on Intelligent Robots and Systems (2003)
- Yik T F, "Locomotion of Bipedal Humanoid Robots: Planning and Learning to Walk", School of Computer Science and Engineering, UNSW (2007)
- HONDA Motor Co, "Achieving Stable Walking", http://world.honda.com/ASIMO/history/technology2.html (accessed 2009)
Code and Material
Available at http://www.cse.unsw.edu.au/~dcollien/BipedBalance.zip or in ~/CDROM_dcollien in Group01 user account.
